feat: add 0xA9 feedforward frame and wire feedforward controller to motor output

- host 0xA9 frame → master_machine decode → robot_cmd publish
  via gimbal_feedforward topic → gimbal subscribe → yaw_motor.feed_input
- add DM_Feedforward_Controller_s with Kp and feed_input fields
- add feedforward term (Kp * feed_input) to motor control output
- add DM_Feedforward_Kp to motor init config

modules/master_machine/master_machine.c

@@ -62,6 +62,13 @@ static void DecodeVision()
         memcpy(&vision_data.motor_pitch_Kd,  p, 4);
         vision_data.debug_updated = 1;
     }
+    else if (sof == VISION_SOF_FEEDFORWARD && len == VISION_FEEDFORWARD_LEN)
+    {
+        if (calc_checksum(buf, VISION_FEEDFORWARD_LEN) != buf[VISION_FEEDFORWARD_LEN - 1])
+            return;
+        memcpy(&vision_data.feedforward, buf + 1, 4);
+        vision_data.feedforward_updated = 1;
+    }
     else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN)
     {
         if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1])

modules/master_machine/master_machine.h

@@ -31,18 +31,24 @@
  *   [motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4][checksum:1]
  *   全部 float, 依次为视觉环 yaw/pitch 和电机角度环 yaw/pitch 的 PID 参数
  *
+ * ---- 前馈输入帧 0xA9 (6 字节) ----
+ *   [SOF:0xA9][feedforward:4][checksum:1]
+ *   feedforward: float, 前馈输入值
+ *
  * checksum: 帧头到数据末尾前一字节累加和取低8位
  */
 
 #define VISION_SOF_AUTO    0xA5
 #define VISION_SOF_MANUAL  0xA6
 #define VISION_SOF_CMD     0xA7
-#define VISION_SOF_DEBUG   0xA8
+#define VISION_SOF_DEBUG       0xA8
+#define VISION_SOF_FEEDFORWARD 0xA9
 
-#define VISION_AUTO_LEN    18
-#define VISION_MANUAL_LEN  10
-#define VISION_CMD_LEN     3
-#define VISION_DEBUG_LEN   50
+#define VISION_AUTO_LEN         18
+#define VISION_MANUAL_LEN       10
+#define VISION_CMD_LEN          3
+#define VISION_DEBUG_LEN        50
+#define VISION_FEEDFORWARD_LEN  6
 
 /* 命令码 */
 #define VISION_CMD_SWITCH_AUTO   0x01
@@ -80,6 +86,10 @@ typedef struct
     float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
     uint8_t debug_updated;
 
+    /* 前馈输入 */
+    float feedforward;
+    uint8_t feedforward_updated;
+
     /* 命令 */
     uint8_t cmd;            // 最新命令码
     uint8_t cmd_pending;    // 有待处理命令标志

modules/motor/DMmotor/dmmotor.c

@@ -107,6 +107,9 @@ void DMMotorSetMode(DM_Motor_Mode_e cmd, DMMotorInstance *motor)
         motor->motor_controller.angle_PID.Dout      = 0;
         motor->motor_controller.angle_PID.Last_Dout = 0;
         motor->motor_controller.pid_ref = 0;
+
+        /* 清理前馈控制器 */
+        motor->feedforward_controller.feed_input    = 0;
     }
 
     memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
@@ -183,6 +186,9 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
     config->motor_config.can_init_config.id = motor;
     motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config);
 
+    /* 设置前馈控制器增益 */
+    motor->feedforward_controller.Kp = config->DM_Feedforward_Kp;
+
     /* register daemon */
     Daemon_Init_Config_s conf = {
         .callback = DMMotorLostCallback,
@@ -213,7 +219,7 @@ void DMMotorControl(void)
         if(motor->motor_mode == DM_CMD_DISABLE_MODE)
             continue;
         CANInstance *can = motor->motor_can_instance;
-
+        DM_Feedforward_Controller_s feedforward_controller = motor->feedforward_controller;
         /* handle motor error states */
         if (motor->measure.state != 0x00 && motor->measure.state != 0x01)
         {
@@ -224,14 +230,16 @@ void DMMotorControl(void)
         }
 
         /* angle PID: IMU feedback → velocity output */
-        float pid_ref = PIDCalculate(&motor->motor_controller.angle_PID,
+        float pid_out = PIDCalculate(&motor->motor_controller.angle_PID,
                                      *motor->motor_controller.other_angle_feedback_ptr,
                                      motor->motor_controller.pid_ref);
 
         if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE)
-            pid_ref *= -1;
+            pid_out *= -1;
 
-        float vel_cmd = pid_ref * DEGREE_2_RAD;
+        pid_out += feedforward_controller.Kp * feedforward_controller.feed_input;
+
+        float vel_cmd = pid_out * DEGREE_2_RAD;
         LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
 
         switch (motor->ctrl_mode)
@@ -241,13 +249,13 @@ void DMMotorControl(void)
             break;
         }
         case DM_MODE_MIT: {
-            float pos_cmd = pid_ref * DEGREE_2_RAD;
+            float pos_cmd = pid_out * DEGREE_2_RAD;
             LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
             DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f);
             break;
         }
         case DM_MODE_POS_VEL: {
-            float pos_cmd = pid_ref * DEGREE_2_RAD;
+            float pos_cmd = pid_out * DEGREE_2_RAD;
             LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
             DMMotorSendPosVel(can, pos_cmd, vel_cmd);
             break;

modules/motor/DMmotor/dmmotor.h

@@ -54,11 +54,17 @@ typedef struct
     float total_angle;     // multi-turn angle (degrees)
 } DM_Motor_Measure_s;
 
+typedef struct{
+    float Kp;
+    float feed_input;
+} DM_Feedforward_Controller_s;
+
 /* DM motor instance (runtime state) */
 typedef struct
 {
     DM_Motor_Measure_s measure;
     Motor_Controller_s motor_controller;
+    DM_Feedforward_Controller_s feedforward_controller;
 
     CANInstance *motor_can_instance;  // CAN communication instance
     DaemonInstance *motor_daemon;     // daemon watchdog
@@ -75,6 +81,7 @@ typedef struct
 {
     Motor_Init_Config_s motor_config;   // generic motor PID and setting config
     DM_Motor_Ctrl_Mode_e ctrl_mode;          // default control mode
+    float DM_Feedforward_Kp;
     float mit_kp;                       // MIT mode KP (only for MIT mode)
     float mit_kd;                       // MIT mode KD (only for MIT mode)
 } DM_Motor_Init_Config_s;