150 lines
5.6 KiB
C
150 lines
5.6 KiB
C
/* =============================================================================
|
|
* gimbal_task.c - 云台控制任务(2ms/500Hz)
|
|
* 状态机:
|
|
* YAW_WAIT 等待"姿态就绪+云台电机在线"(遥控可后到)
|
|
* YAW_RUN 正常串级控制(符号用参数回退值,直至手势自检完成)
|
|
* YAW_DETECT 装配符号自检——**仅由操作手手势显式触发,绝不自动执行**:
|
|
* 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
|
|
* 检测期间云台会轻转几度(约 20% 电流脉冲 60ms),操作手应
|
|
* 确认云台周围无人后再做手势。失败重试 3 次后回退参数符号。
|
|
* 任意门控失效(遥控离线/电机掉线/姿态不可用/底盘故障)立即清零输出
|
|
* Yaw/Pitch GM6020 采用 0x1FE 电流帧控制;拨弹 M2006 由 shoot_task 独立发 0x200
|
|
* ========================================================================== */
|
|
#include "app_tasks.h"
|
|
#include "cmsis_os.h"
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "robot_typedef.h"
|
|
#include "gimbal.h"
|
|
#include "bmi088.h"
|
|
#include "attitude.h"
|
|
#include "dr16.h"
|
|
#include "c_comm.h"
|
|
#include "robot_param.h"
|
|
|
|
/* 校准手势:左摇杆推最左 + 右摇杆推最右(正常操控不可能误触) */
|
|
#define CALIB_GESTURE_L_RL 400 // < 400 即最左
|
|
#define CALIB_GESTURE_R_RL 1600 // > 1600 即最右
|
|
#define CALIB_GESTURE_MS 500 // 保持时长
|
|
#define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务(500Hz)
|
|
|
|
typedef enum
|
|
{
|
|
YAW_WAIT = 0,
|
|
YAW_RUN,
|
|
YAW_DETECT,
|
|
} yaw_state_e;
|
|
|
|
void gimbal_task(void const * argument)
|
|
{
|
|
TickType_t last_wake = xTaskGetTickCount();
|
|
yaw_state_e state = YAW_WAIT;
|
|
uint8_t detect_retry = 0;
|
|
uint16_t gesture_cnt = 0;
|
|
uint8_t force_reset = 0;
|
|
|
|
while (!app_ready)
|
|
osDelay(2);
|
|
|
|
for (;;)
|
|
{
|
|
const uint8_t gates_ok = (cmd_robot.online && !gimbal_motor_offline() &&
|
|
bmi088_online && att_ready && !ctoc_chassis_fault());
|
|
|
|
/* 校准手势检测(仅遥控在线时可能成立;500 个采样窗防抖) */
|
|
if (cmd_robot.online && !gimbal_motor_offline() &&
|
|
dr16.rc_l_RL < CALIB_GESTURE_L_RL && dr16.rc_r_RL > CALIB_GESTURE_R_RL)
|
|
{
|
|
if (gesture_cnt < CALIB_GESTURE_TICKS)
|
|
gesture_cnt++;
|
|
}
|
|
else
|
|
gesture_cnt = 0;
|
|
const uint8_t gesture = (gesture_cnt >= CALIB_GESTURE_TICKS);
|
|
if (gesture)
|
|
gesture_cnt = 0;
|
|
|
|
#if 0
|
|
/* ===== 2026-10-15 台架 Pitch 阶跃发生器(已停用,保留备查) =====
|
|
* **有界**测试,不是持续抖动:只跑 GIMBAL_BENCH_PITCH_HALF_COUNT 个半周期,
|
|
* 跑完自动归零并锁存,之后云台恢复静止(重新上电/改开关才会再跑一次)。
|
|
* 配合 gimbal.c 的台架偏置使用;2ms 任务,半周期 tick 数 = HALF_MS / 2 */
|
|
{
|
|
static uint32_t bench_phase = 0;
|
|
static uint8_t bench_done = 0;
|
|
const uint32_t half_ticks = (uint32_t)GIMBAL_BENCH_PITCH_HALF_MS / 2u;
|
|
if (!bench_done && half_ticks > 0u)
|
|
{
|
|
const uint32_t total_ticks = half_ticks * (uint32_t)GIMBAL_BENCH_PITCH_HALF_COUNT;
|
|
if (bench_phase >= total_ticks)
|
|
{
|
|
bench_done = 1;
|
|
gimbal_bench_pitch_offset = 0.0f; /* 跑完立即停,不再抖动 */
|
|
}
|
|
else
|
|
{
|
|
gimbal_bench_pitch_offset = ((bench_phase / half_ticks) % 2u)
|
|
? GIMBAL_BENCH_PITCH_STEP_DEG
|
|
: 0.0f;
|
|
bench_phase++;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
switch (state)
|
|
{
|
|
case YAW_WAIT:
|
|
gimbal_output_zero();
|
|
/* 姿态与电机就绪即可进入运行(符号先用参数回退值) */
|
|
if (!gimbal_motor_offline() && bmi088_online && att_ready)
|
|
{
|
|
force_reset = 1; // 首次控制同步目标角
|
|
state = YAW_RUN;
|
|
}
|
|
break;
|
|
|
|
case YAW_DETECT:
|
|
if (gimbal_detect_yaw_sign())
|
|
{
|
|
force_reset = 1; // 检测期间云台被动转过,重锁目标
|
|
state = YAW_RUN;
|
|
}
|
|
else if (++detect_retry >= 3)
|
|
{
|
|
detect_retry = 0;
|
|
force_reset = 1;
|
|
state = YAW_RUN; // 回退参数符号
|
|
}
|
|
else
|
|
gimbal_output_zero();
|
|
break;
|
|
|
|
case YAW_RUN:
|
|
default:
|
|
if (gesture && !gimbal_motor_offline() && att_ready)
|
|
{
|
|
detect_retry = 0;
|
|
state = YAW_DETECT; // 操作手主动请求自检
|
|
}
|
|
else if (gates_ok)
|
|
{
|
|
gimbal_control(cmd_robot.gimbal_yaw_delta,
|
|
cmd_robot.gimbal_pitch_delta,
|
|
(uint8_t)(force_reset || cmd_robot.gimbal_reset));
|
|
force_reset = 0;
|
|
}
|
|
else
|
|
{
|
|
gimbal_output_zero();
|
|
force_reset = 1;
|
|
state = YAW_WAIT; // 门控失效回等待,恢复后重锁目标
|
|
}
|
|
break;
|
|
}
|
|
|
|
monitor_heartbeat(HB_GIMBAL);
|
|
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
|
|
}
|
|
}
|