refactor(modules): 按角色重排 modules/ 目录,消除命名不一致

原 modules/ 下目录大小写混用(BMI088/BUZZER/DR16/IST8310/LED/PID 为大写, attitude/c_comm/filter/gimbal/ins/motor/referee/shoot 为小写),且把第三方移植代码(QuaternionEKF/kalman_filter)与本工程封装(ins)混在同一目录。新结构(全部小写,按角色分层,第三方代码单独隔离): modules/ algorithm/ pid.c/h filter.c/h 通用算法 imu/ attitude.c/h ins.c/h 姿态与惯导封装 imu/qekf/ QuaternionEKF.c/h kalman_filter.c/h (移植自 Wang Hongxi INS 例程) sensor/ bmi088.c/h ist8310.c/h 板载传感器驱动 comms/ dr16.c/h c_comm.c/h referee.c/h motor/ dji_motor.c/h 电机抽象 control/ gimbal.c/h shoot.c/h 整车控制逻辑 (底盘为 chassis.c/h) dev/ led.c/h buzzer.c/h 板级指示/发声共移动 58 个文件(每板 29 个),旧目录全部删除。工程文件同步(每个工程各 2 个): - .uvprojx: Modules 分组 16 个 <FilePath> 改为新路径; 2 个非空 <IncludePath> 元素:15 个旧模块路径收敛为 8 个新目录, 并顺带删除陈旧项 ../modules/AHRS(目录根本不存在,legacy 时代残留) 与冗余项 ..\bsp(与 ../Bsp/Inc 重复) - .uvoptx: 16 个 <PathWithFileName> 同步关于"为什么不用改源码": Keil 的 include 靠 IncludePath 解析,本工程所有 #include 都是纯文件名形式(无路径限定),所以移动目录只需改工程配置。另外 .h 完全不登记在工程里(只有 16 个 .c 被登记),故头文件只需移动。回归验证(无法在此环境编译,用等价判据): - 两个工程引用的 98 / 95 个 <FilePath> 全部真实存在 - 磁盘上 47 / 45 个 .c 全部仍被工程引用 —— 即没有任何文件掉出编译 - 2 个 IncludePath 元素均含全部 8 个新模块目录,无旧路径残留 - 所有 IncludePath 指向的目录均真实存在 - .uvprojx/.uvoptx XML 良构 - 两棵树结构一致(chassis/gimbal 等板级差异符合预期) ⚠️ 仍需在 Keil 里点一次编译确认(见 README 的构建章节)。
2026-09-12 11:51:41 +08:00
parent 3e5e5e3f95
commit e3d432f3b5
62 changed files with 62 additions and 62 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);
+}
@@ -0,0 +1,49 @@
+#ifndef GIMBAL_H
+#define GIMBAL_H
+
+/* 云台双轴控制(串级 PID)
+ * Yaw  (2026-09 重写):双环都以陀螺仪为反馈——
+ *        位置环:QEKF 多圈 yaw(°) -> 目标角速度(°/s)
+ *        速度环:陀螺 z 轴角速度(°/s) -> GM6020 电压
+ *        装配符号由上电自检自动测定(gimbal_detect_yaw_sign),
+ *        两环共用同一符号,不可能再出现"两传感器符号打架"
+ * Pitch:位置环用 QEKF 的 Roll(安装方向所致,勿改),速度环转子转速
+ * 电机:  Yaw=GM6020 CAN2 0x205(电压,0x1FF CAN2 槽1)
+ *        Pitch=GM6020 CAN1 0x206(电压,与拨弹共帧 0x1FF CAN1 槽2/3,legacy 方案)
+ *        摩擦轮 M3508 0x201/0x202 与拨弹 M2006 0x207 的实例也在此初始化 */
+
+#include "dji_motor.h"
+
+/* 电机实例(由 gimbal_task 初始化,其他模块可读取反馈) */
+extern dji_motor_t motor_yaw_gimbal;    // Yaw GM6020,CAN2 0x205
+extern dji_motor_t motor_pitch_gimbal;  // Pitch GM6020,CAN1 0x206
+extern dji_motor_t motor_friction_l;    // 左摩擦轮 M3508,CAN1 0x201
+extern dji_motor_t motor_friction_r;    // 右摩擦轮 M3508,CAN1 0x202
+extern dji_motor_t motor_rammer;        // 拨弹 M2006,CAN1 0x207
+
+/* 初始化电机实例与 PID(can_start 之后调用) */
+extern void gimbal_motor_init(void);
+
+/* Yaw 装配符号自检:给 Yaw 电机正向小电压脉冲,按陀螺 z 轴响应方向
+ * 测定符号。阻塞约 200ms(HAL_Delay),检测期间云台会轻转几度。
+ * 返回 1-成功(gimbal_yaw_sign() 可用) 0-失败(被按住/电机离线/无响应) */
+extern uint8_t gimbal_detect_yaw_sign(void);
+
+/* 自检测得的装配符号(+1/-1;未检测时为 FALLBACK 参数值) */
+extern float gimbal_yaw_sign(void);
+
+/* 编码器 tick 增大方向对应云台旋转方向:+1=逆时针(CCW) -1=顺时针(CW)
+ * 0=尚未自检。随 0x1A9 flags bit1 下发,供底盘跟随符号自动综合 */
+extern int8_t gimbal_encoder_ccw(void);
+
+/* 云台轴控制:根据 cmd 增量更新目标角并计算电压输出(2ms 调用)
+ * 内部发送 CAN2 0x1FF(Yaw)与 CAN1 0x1FF(Pitch+拨弹) */
+extern void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag);
+
+/* 云台任一电机掉线返回 1 */
+extern uint8_t gimbal_motor_offline(void);
+
+/* 云台输出清零(停车) */
+extern void gimbal_output_zero(void);
+
+#endif
@@ -0,0 +1,101 @@
+/* =============================================================================
+ * shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006
+ * 逻辑(与 legacy 一致):
+ *   摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过),
+ *   开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开)
+ *   拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向
+ * 拨弹电流经 shoot_share_rammer_current() 交给云台共帧发送(0x1FF CAN1)
+ * ========================================================================== */
+#include "shoot.h"
+#include "gimbal.h"
+#include "pid.h"
+#include "tim.h"
+#include "robot_param.h"
+
+extern TIM_HandleTypeDef htim3;
+
+uint8_t shoot_friction_flag = 0;
+
+static PID_PositionTypeDef friction_l_pid, friction_r_pid, rammer_pid;
+
+/* 激光:PC8 = TIM3_CH3(10kHz PWM,ARR=8399),占空比 0/满 对应关/开 */
+#define LASER_ON() __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 8399)
+#define LASER_OFF() __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0)
+
+static void laser_init(void)
+{
+    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0);
+    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
+}
+
+void shoot_init(void)
+{
+    PID_PositionStructureInit(&friction_l_pid, 0);
+    PID_PositionSetParameter(&friction_l_pid, GIMBAL_FRICTION_WHEEL_KP, GIMBAL_FRICTION_WHEEL_KI, GIMBAL_FRICTION_WHEEL_KD);
+    PID_PositionSetEkRange(&friction_l_pid, -5, 5);
+    PID_PositionSetOUTRange(&friction_l_pid, -15000, 15000);
+
+    PID_PositionStructureInit(&friction_r_pid, 0);
+    PID_PositionSetParameter(&friction_r_pid, GIMBAL_FRICTION_WHEEL_KP, GIMBAL_FRICTION_WHEEL_KI, GIMBAL_FRICTION_WHEEL_KD);
+    PID_PositionSetEkRange(&friction_r_pid, -5, 5);
+    PID_PositionSetOUTRange(&friction_r_pid, -15000, 15000);
+
+    PID_PositionStructureInit(&rammer_pid, GIMBAL_RAMMER_SPEED);
+    PID_PositionSetParameter(&rammer_pid, GIMBAL_RAMMER_KP, GIMBAL_RAMMER_KI, GIMBAL_RAMMER_KD);
+    PID_PositionSetEkRange(&rammer_pid, -20, 20);
+    PID_PositionSetOUTRange(&rammer_pid, -30000, 30000);
+
+    laser_init();
+}
+
+void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on)
+{
+    /* 摩擦轮(与 legacy 逻辑一致:发射机构上电才允许开启;
+     * 台架调试时可经 DEBUG_SHOOT_WITHOUT_REFEREE 绕过裁判系统检查) */
+    if (friction_on && (shooter_power_on || DEBUG_SHOOT_WITHOUT_REFEREE))
+    {
+        friction_l_pid.Need_Value = -GIMBAL_FRICTION_WHEEL_SPEED;
+        friction_r_pid.Need_Value = GIMBAL_FRICTION_WHEEL_SPEED;
+        LASER_ON();
+        shoot_friction_flag = 1;
+    }
+    else
+    {
+        friction_l_pid.Need_Value = 0;
+        friction_r_pid.Need_Value = 0;
+        LASER_OFF();
+        shoot_friction_flag = 0;
+    }
+
+    /* 拨弹:仅在摩擦轮开启时允许 */
+    if (shoot_friction_flag)
+    {
+        if (fire)
+            rammer_pid.Need_Value = GIMBAL_RAMMER_SPEED;
+        else if (eject)
+            rammer_pid.Need_Value = -GIMBAL_RAMMER_SPEED;
+        else
+            rammer_pid.Need_Value = 0;
+    }
+    else
+        rammer_pid.Need_Value = 0;
+
+    PID_PositionCalc(&friction_l_pid, motor_friction_l.fdb.rotor_speed);
+    PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed);
+    PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed);
+
+    /* 拨弹电流交给云台控制帧统一发送(0x1FF CAN1 共帧) */
+    shoot_share_rammer_current((int16_t)rammer_pid.OUT);
+}
+
+void shoot_send_cmd(void)
+{
+    dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0);
+}
+
+void shoot_output_zero(void)
+{
+    dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
+    LASER_OFF();
+    shoot_friction_flag = 0;
+}
@@ -0,0 +1,28 @@
+#ifndef SHOOT_H
+#define SHOOT_H
+
+#include <stdint.h>
+
+/* 发射机构:摩擦轮 2×M3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(电流由云台共帧发送)
+ * 摩擦轮开启时联动激光(PC8,经 TIM3 CH3 PWM) */
+
+/* 初始化摩擦轮/拨弹 PID(can_start 之后调用) */
+extern void shoot_init(void);
+
+/* 发射控制(2ms 调用)
+ * friction_on:摩擦轮开关;fire:发射;eject:退弹
+ * shooter_power_on:发射机构上电状态(来自底盘 0x019) */
+extern void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on);
+
+/* 发送摩擦轮电流帧(0x200 CAN1) */
+extern void shoot_send_cmd(void);
+
+/* 摩擦轮输出清零 */
+extern void shoot_output_zero(void);
+
+/* 摩擦轮当前开关状态(供激光联动与调试) */
+extern uint8_t shoot_friction_flag;
+
+/* 拨弹电流写入共享变量(由 shoot 模块调用,云台共帧发送) */
+extern void shoot_share_rammer_current(int16_t current);
+#endif