/* ============================================================================= * gimbal.c - 云台双轴控制(串级 PID) * Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明 * 位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅) * 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电流(限幅) * 稳定性条件:SIGN*G>0,G 为"电流->陀螺 z 角速度"的装配增益, * 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架 * Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题) * 发送:CAN2 0x1FE 槽1=Yaw 电流;CAN1 0x1FE 槽2=Pitch 电流 * ========================================================================== */ #include "gimbal.h" #include "shoot.h" #include "pid.h" #include "attitude.h" #include "bmi088.h" #include "can.h" #include "robot_param.h" #include "filter.h" #include "robot_typedef.h" #include "dr16.h" #include #ifndef RAD_TO_ANGLE #define RAD_TO_ANGLE 57.295779513082320876798154814105f #endif #ifndef ANGLE_TO_RAD #define ANGLE_TO_RAD 0.01745329251994329576923690768489f #endif dji_motor_t motor_yaw_gimbal; dji_motor_t motor_pitch_gimbal; dji_motor_t motor_friction_l; dji_motor_t motor_friction_r; dji_motor_t motor_rammer; static PID_PositionTypeDef yaw_angle_pid, yaw_speed_pid; static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid; static float yaw_target_deg; // 世界系目标角(°) static float pitch_target_deg; // 目标角(°) static float yaw_current; float pitch_pid = 0.0f; float pitch_g = 0.0f; float pitch_current = 0.0f; /* ===== 2026-10-15 台架调参临时量(已停用,保留备查) ===== */ #if 0 /* 台架调参用 Pitch 目标角附加偏置(°)。由 gimbal_task 的方波发生器写入, * 只叠加到位置环设定值上,不累加进 pitch_target_deg,默认 0 无副作用 */ float gimbal_bench_pitch_offset = 0.0f; /* 实际施加的偏置(经限位安全门控后),仅用于遥测显示真实设定值 */ static float pitch_applied_offset = 0.0f; #endif /* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */ static float yaw_sign_f = 0.0f; static uint8_t yaw_sign_valid = 0; void gimbal_motor_init(void) { dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020); dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020); dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508); dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508); dji_motor_init(&motor_rammer, &hcan2, 0x201, DJI_MOTOR_2006); /* Yaw 位置环:° -> °/s */ PID_PositionStructureInit(&yaw_angle_pid, 0); PID_PositionSetParameter(&yaw_angle_pid, GIMBAL_YAW_ANGLE_KP, GIMBAL_YAW_ANGLE_KI, GIMBAL_YAW_ANGLE_KD); PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND); PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT); /* Yaw 速度环:°/s -> 电流 */ PID_PositionStructureInit(&yaw_speed_pid, 0); PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD); PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT); /* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */ PID_PositionStructureInit(&pitch_angle_pid, 0); PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD); PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND); PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200); PID_PositionStructureInit(&pitch_speed_pid, 300); PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD); PID_PositionSetEkRange(&pitch_speed_pid, 0, 0); PID_PositionSetOUTRange(&pitch_speed_pid, -16384, 16384); } uint8_t gimbal_detect_yaw_sign(void) { if (gimbal_motor_offline()) return 0; /* 静默期:输出清零等待机械/陀螺稳定 */ dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0); HAL_Delay(50); /* 正向电流脉冲 60ms,期间平均陀螺 z 角速度 */ float sum = 0.0f; dji_motor_send_gm6020_current_low(&hcan2, GIMBAL_YAW_DETECT_CURRENT, 0, 0, 0); for (uint16_t i = 0; i < 60; i++) { sum += bmi088_gyro[2]; // rad/s(z 轴向上为正) HAL_Delay(1); } dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0); HAL_Delay(80); // 回稳,随后 gimbal_task 的首次控制会同步目标角 const float rate_dps = sum / 60.0f * RAD_TO_ANGLE; if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN) { yaw_sign_f = 1.0f; // 正电流->陀螺正转,装配增益 G>0 yaw_sign_valid = 1; } else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN) { yaw_sign_f = -1.0f; // 皮带交叉等反向装配 yaw_sign_valid = 1; } else return 0; // 被按住/没转:不给结论,由调用方决定重试或回退 return 1; } float gimbal_yaw_sign(void) { return yaw_sign_valid ? yaw_sign_f : GIMBAL_YAW_SIGN_FALLBACK; } int8_t gimbal_encoder_ccw(void) { /* 自检:电流正脉冲 -> 陀螺 z 正转(云台 CCW),而电流正方向即编码器 * tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */ if (!yaw_sign_valid) return 0; return (yaw_sign_f > 0.0f) ? 1 : -1; } void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag) { /* 双环共用装配符号 */ const float sign = gimbal_yaw_sign(); const float yaw_meas = sign * att_deg_yaw; // ° const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE); // °/s /* 首次进入控制或复位:将目标角设定为设定的编码器零位朝向(类似 Pitch 上电归位) */ static uint8_t first_run = 1; if (first_run || reset_flag) { first_run = 0; /* 计算当前编码器距离设定零位的最短差值(-4096 ~ +4096) */ int16_t ecd_diff = (int16_t)motor_yaw_gimbal.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO; if (ecd_diff > 4096) ecd_diff -= 8192; else if (ecd_diff < -4096) ecd_diff += 8192; /* 将编码器 tick 差折算为机械角度偏差(°)，补偿给世界系目标角 */ const float ecd_err_deg = (float)ecd_diff * (360.0f / 8192.0f); yaw_target_deg = yaw_meas - ecd_err_deg; /* Pitch: 计算当前编码器距离机械零位的差值，折算为角度偏差补偿给目标角(上电自动归位到零位) */ int16_t pitch_ecd_diff = (int16_t)motor_pitch_gimbal.fdb.raw_angle - (int16_t)PITCH_GM6020_ENCODER_ZERO; const float pitch_ecd_err_deg = (float)pitch_ecd_diff * (360.0f / 8192.0f); pitch_target_deg = -att_deg_roll - pitch_ecd_err_deg; if (reset_flag) { PID_PositionClean(&yaw_angle_pid); PID_PositionClean(&yaw_speed_pid); PID_PositionClean(&pitch_angle_pid); PID_PositionClean(&pitch_speed_pid); } } yaw_target_deg += yaw_delta; pitch_target_deg += pitch_delta; /* Yaw 位置环:° -> °/s(反馈为带符号姿态角) */ yaw_angle_pid.Need_Value = yaw_target_deg; PID_PositionCalc(&yaw_angle_pid, yaw_meas); /* Yaw 速度环:°/s -> 电流(反馈为带符号陀螺角速度,双环符号自洽) */ yaw_speed_pid.Need_Value = yaw_angle_pid.OUT; PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas); /* 小陀螺底盘自旋解耦前馈:根据底盘相对转速补偿动摩擦与反电动势拖拽，消除同向/反向手感不对称 */ float spin_feedforward = 0.0f; if (cmd_robot.spin_flag || dr16.s2 == 1 || dr16.s2 == 2) { spin_feedforward = GIMBAL_YAW_SPIN_FEEDFORWARD_K * (float)motor_yaw_gimbal.fdb.rotor_speed; } yaw_current = yaw_speed_pid.OUT + spin_feedforward; if (yaw_current > GIMBAL_YAW_SPEED_OUT_LIMIT) yaw_current = GIMBAL_YAW_SPEED_OUT_LIMIT; else if (yaw_current < -GIMBAL_YAW_SPEED_OUT_LIMIT) yaw_current = -GIMBAL_YAW_SPEED_OUT_LIMIT; /* Pitch 串级闭环,位置环反馈为 AHRS Roll */ #if 0 /* ===== 2026-10-15 台架调参临时改动(已停用,保留备查) ===== * 启用时:先算编码器角与上下软限位,做台架偏置的限位安全门控(轴距限位 * 不足 GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 时不施加阶跃),并把下面一行 * 改为 pitch_angle_pid.Need_Value = pitch_target_deg + pitch_applied_offset; */ const float pitch_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle - (float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f); const float pitch_lim_up_deg = ((float)PITCH_GM6020_POSITION_UPPER_LIMIT - (float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f); const float pitch_lim_dn_deg = ((float)PITCH_GM6020_POSITION_LOWER_LIMIT - (float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f); pitch_applied_offset = gimbal_bench_pitch_offset; if (pitch_ecd_deg < pitch_lim_up_deg + GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG || pitch_ecd_deg > pitch_lim_dn_deg - GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG) pitch_applied_offset = 0.0f; #endif pitch_angle_pid.Need_Value = pitch_target_deg; PID_PositionCalc(&pitch_angle_pid, -att_deg_roll); pitch_speed_pid.Need_Value = pitch_angle_pid.OUT; PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed); pitch_pid = pitch_speed_pid.OUT; /* Pitch 重力拟合前馈补偿: * a 为当前物理编码值减去 3200*360/8192 的数值(折算为弧度送入三角函数) * I = 3105.88 * cos(a) + 3565.50 * sin(a) + 64.31 * 最终电流输出 pitch_out = pitch_pid + pitch_g (速度环不代入 pitch_g) */ const float a_deg = ((float)motor_pitch_gimbal.fdb.raw_angle - GIMBAL_PITCH_GRAVITY_ZERO_ECD) * (360.0f / 8192.0f); const float a_rad = a_deg * ANGLE_TO_RAD; pitch_g = GIMBAL_PITCH_GRAVITY_K_COS * cosf(a_rad) + GIMBAL_PITCH_GRAVITY_K_SIN * sinf(a_rad) + GIMBAL_PITCH_GRAVITY_K_CONST; pitch_current = pitch_pid + pitch_g; if (pitch_current > 16384.0f) pitch_current = 16384.0f; else if (pitch_current < -16384.0f) pitch_current = -16384.0f; #if 0 /* ===== 2026-10-15 台架调参临时改动:Pitch 软限位/堵转保护(已停用,保留备查) ===== * 背景:原设计的上下限位只作用于遥控输入路径(robot_cmd_task),控制环自身的 * 目标角/前馈没有任何限位约束。台架烧录会失力矩十几秒~几分钟,轴会掉到限位, * 若不加保护,原逻辑会持续输出大电流顶着限位(GM6020 直驱堵转 = 持续过载发热)。 * 做法:越限位时只允许"离开限位"方向的电流(正电流 = 抬头/编码器减小,故下限位 * 只留正向、上限位只留负向),反方向清零;限位外连续 GIMBAL_PITCH_LIMIT_STALL_MS * 编码器不动则判堵转,电流降到涓流上限。轴回到软限位内后闭环自动恢复正常。 */ if (pitch_ecd_deg >= pitch_lim_dn_deg || pitch_ecd_deg <= pitch_lim_up_deg) { /* 堵转计时:相对基准编码值移动超过 10 个刻度(0.44°)才算"在动"。 * 阈值取 10 而不是 2:陀螺/结构微振会让编码器抖 2~3 个刻度,阈值太小会把 * 静止误判成在动,保护永远不生效(实测踩过);慢速抬升 5°/s 在 300ms 内 * 也能走 34 个刻度,不会被误判。 */ static uint16_t lim_stall = 0; static uint16_t lim_ref_ecd = 0; const int16_t d_ecd = (int16_t)((int16_t)motor_pitch_gimbal.fdb.raw_angle - (int16_t)lim_ref_ecd); if (d_ecd > 10 || d_ecd < -10) { lim_ref_ecd = motor_pitch_gimbal.fdb.raw_angle; lim_stall = 0; } else if (lim_stall < 60000u) lim_stall++; const float cap = (lim_stall * 2u >= (uint16_t)GIMBAL_PITCH_LIMIT_STALL_MS) ? GIMBAL_PITCH_LIMIT_CURRENT : GIMBAL_PITCH_LIMIT_CURRENT_RECOVER; if (pitch_ecd_deg >= pitch_lim_dn_deg) /* 轴过低:只许抬升 */ { if (pitch_current < 0.0f) pitch_current = 0.0f; else if (pitch_current > cap) pitch_current = cap; } else /* 轴过高:只许下压 */ { if (pitch_current > 0.0f) pitch_current = 0.0f; else if (pitch_current < -cap) pitch_current = -cap; } PID_PositionClean(&pitch_speed_pid); } #endif /* ===== 停用块结束 ===== */ /* 发送:CAN2 0x1FE 槽1 = Yaw 电流;CAN1 0x1FE 槽2 = Pitch 电流 */ dji_motor_send_gm6020_current_low(&hcan2, (int16_t)yaw_current, 0, 0, 0); dji_motor_send_gm6020_current_low(&hcan1, 0, (int16_t)pitch_current, 0, 0); } #if 0 /* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) ===== */ /* Pitch 位置环实际设定值(°) = 真实目标角 + 经限位门控后的台架偏置 */ float gimbal_pitch_setpoint_deg(void) { return pitch_target_deg + pitch_applied_offset; } /* Pitch 位置环输出 = 速度环目标(°/s),用于判断内环是否被限幅 */ float gimbal_pitch_speed_ref(void) { return pitch_angle_pid.OUT; } #endif uint8_t gimbal_motor_offline(void) { uint32_t now = HAL_GetTick(); uint8_t rammer_off = 0; #if GIMBAL_RAMMER_INSTALLED rammer_off = (motor_rammer.fdb.last_online_tick == 0 || now - motor_rammer.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS); #endif return (motor_yaw_gimbal.fdb.last_online_tick == 0 || now - motor_yaw_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS || motor_pitch_gimbal.fdb.last_online_tick == 0 || now - motor_pitch_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS || motor_friction_l.fdb.last_online_tick == 0 || now - motor_friction_l.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS || motor_friction_r.fdb.last_online_tick == 0 || now - motor_friction_r.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS || rammer_off); } void gimbal_output_zero(void) { dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0); dji_motor_send_gm6020_current_low(&hcan1, 0, 0, 0, 0); dji_motor_send_current_low(&hcan1, 0, 0, 0, 0); dji_motor_send_current_low(&hcan2, 0, 0, 0, 0); }