chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)

- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
2026-09-12 10:28:21 +08:00
commit a4da5ad17c
517 changed files with 503795 additions and 0 deletions
@@ -0,0 +1,205 @@
+/* =============================================================================
+ * 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 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
+ * ========================================================================== */
+#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"
+
+#ifndef RAD_TO_ANGLE
+#define RAD_TO_ANGLE 57.295779513082320876798154814105f
+#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_voltage;
+static float pitch_voltage;
+
+/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
+static float yaw_sign_f = 0.0f;
+static uint8_t yaw_sign_valid = 0;
+
+/* 拨弹电流由 shoot 模块写入,云台与拨弹共帧(0x1FF CAN1)发送 */
+static int16_t rammer_current_shared = 0;
+
+void shoot_share_rammer_current(int16_t current);
+
+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, &hcan1, 0x207, 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, -5, 5);
+    PID_PositionSetOUTRange(&pitch_speed_pid, -30000, 30000);
+}
+
+uint8_t gimbal_detect_yaw_sign(void)
+{
+    if (gimbal_motor_offline())
+        return 0;
+
+    /* 静默期:输出清零等待机械/陀螺稳定 */
+    dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
+    HAL_Delay(50);
+
+    /* 正向电压脉冲 60ms,期间平均陀螺 z 角速度 */
+    float sum = 0.0f;
+    dji_motor_send_current_high(&hcan2, GIMBAL_YAW_DETECT_VOLTAGE, 0, 0, 0);
+    for (uint16_t i = 0; i < 60; i++)
+    {
+        sum += bmi088_gyro[2];  // rad/s(z 轴向上为正)
+        HAL_Delay(1);
+    }
+    dji_motor_send_current_high(&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
+
+    /* 首次进入控制:目标角同步为当前姿态(否则 Yaw 目标仍为初始 0°,
+     * 一闭环就会朝 0° 猛转) */
+    static uint8_t first_run = 1;
+    if (first_run)
+    {
+        first_run = 0;
+        yaw_target_deg = yaw_meas;
+        pitch_target_deg = 0;
+    }
+
+    if (reset_flag)
+    {
+        yaw_target_deg = yaw_meas;     // Yaw 复位为当前角度
+        pitch_target_deg = 0;          // Pitch 复位为上电位置
+        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);
+    yaw_voltage = yaw_speed_pid.OUT;
+
+    /* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */
+    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_voltage = pitch_speed_pid.OUT;
+
+    /* 发送:CAN2 0x1FF 槽1 = Yaw 电压;CAN1 0x1FF 槽2 = Pitch 电压,槽3 = 拨弹电流 */
+    dji_motor_send_current_high(&hcan2, (int16_t)yaw_voltage, 0, 0, 0);
+    dji_motor_send_current_high(&hcan1, 0, (int16_t)pitch_voltage, rammer_current_shared, 0);
+}
+
+void shoot_share_rammer_current(int16_t current)
+{
+    rammer_current_shared = current;
+}
+
+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_current_high(&hcan2, 0, 0, 0, 0);
+    dji_motor_send_current_high(&hcan1, 0, 0, 0, 0);
+    dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
+}