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

application/cmd/robot_cmd.c

@@ -29,11 +29,16 @@ static float filtered_y_measure;
 static float gimbal_yaw_ref;
 static float gimbal_pitch_ref;
 
+/* 手动模式角度速率 */
+static float yaw_degree_velocity = 0;
+static float pitch_degree_velocity = 0;
+
 /* ---- pub-sub ---- */
 static Publisher_t *gimbal_cmd_pub;
 static Subscriber_t *gimbal_feed_sub;
 static Publisher_t *motor_enable_pub;
 static Publisher_t *motor_pid_pub;
+static Publisher_t *feedforward_pub;
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;
 static Gimbal_Upload_Data_s gimbal_fetch_data;
 
@@ -94,6 +99,8 @@ static void SwitchToAutoMode(void)
     vision_data.auto_updated = 0;
     VisionPIDReset();
 
+    vision_data.feedforward_updated = 0;
+
     /* 从调试模式切回时重新使能电机 */
     if (current_mode == VISION_MODE_DEBUG)
     {
@@ -114,6 +121,10 @@ static void SwitchToManualMode(void)
     // gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch;
 
     vision_data.manual_updated = 0;
+    yaw_degree_velocity = 0;
+    pitch_degree_velocity = 0;
+
+    vision_data.feedforward_updated = 0;
 
     /* 从调试模式切回时重新使能电机 */
     if (current_mode == VISION_MODE_DEBUG)
@@ -209,6 +220,7 @@ void RobotCMDInit()
     gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     motor_enable_pub = PubRegister("motor_enable", sizeof(Motor_Enable_s));
     motor_pid_pub    = PubRegister("motor_pid", sizeof(Motor_PID_Tune_s));
+    feedforward_pub  = PubRegister("gimbal_feedforward", sizeof(Gimbal_Feedforward_s));
 
     static PID_Init_Config_s yaw_vpid_config = {
         .Kp = 100.0f, .Ki = 0.0f, .Kd = 0.0f,
@@ -272,11 +284,16 @@ static void VisionTracking()
 
     gimbal_cmd_send.yaw   = gimbal_yaw_ref;
     gimbal_cmd_send.pitch = gimbal_pitch_ref;
+
+    if (vision_data.feedforward_updated)
+    {
+        vision_data.feedforward_updated = 0;
+        Gimbal_Feedforward_s ff = { .feedforward = vision_data.feedforward };
+        PubPushMessage(feedforward_pub, &ff);
+    }
 }
 
 /* ---- 手动模式: 上位机角度参考 ---- */
-static float yaw_degree_velocity = 0;
-static float pitch_degree_velocity = 0;
 static void ManualMode()
 {
     if (vision_data.manual_updated)
@@ -295,6 +312,13 @@ static void ManualMode()
 
     gimbal_cmd_send.yaw   = gimbal_yaw_ref;
     gimbal_cmd_send.pitch = gimbal_pitch_ref;
+
+    if (vision_data.feedforward_updated)
+    {
+        vision_data.feedforward_updated = 0;
+        Gimbal_Feedforward_s ff = { .feedforward = vision_data.feedforward };
+        PubPushMessage(feedforward_pub, &ff);
+    }
 }
 
 /* ---- 调试模式: 上位机下发 PID 参数调参 ---- */

application/gimbal/gimbal.c

@@ -11,6 +11,7 @@ static Publisher_t *gimbal_pub;
 static Subscriber_t *gimbal_sub;
 static Subscriber_t *motor_enable_sub;
 static Subscriber_t *motor_pid_sub;
+static Subscriber_t *feedforward_sub;
 static Gimbal_Upload_Data_s gimbal_feedback_data;
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;
 
@@ -25,6 +26,7 @@ void GimbalInit()
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
     motor_enable_sub = SubRegister("motor_enable", sizeof(Motor_Enable_s));
     motor_pid_sub    = SubRegister("motor_pid", sizeof(Motor_PID_Tune_s));
+    feedforward_sub  = SubRegister("gimbal_feedforward", sizeof(Gimbal_Feedforward_s));
 
     /* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
     DM_Motor_Init_Config_s yaw_config = {
@@ -50,6 +52,7 @@ void GimbalInit()
             .motor_type = DM_MOTOR,
         },
         .ctrl_mode = DM_MODE_VELOCITY,
+        .DM_Feedforward_Kp = 0.9f,
         .mit_kp = 0, .mit_kd = 0,
     };
     yaw_motor = DMMotorInit(&yaw_config);
@@ -87,10 +90,19 @@ void GimbalInit()
 
 void GimbalTask()
 {
-    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
+    /* 角度环参考值设定 */
-
+    if(SubGetMessage(gimbal_sub, &gimbal_cmd_recv))
-    DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
+    {
-    DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+        DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
+        DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+    }
+    
+    /* 前馈输入 */
+    static Gimbal_Feedforward_s feedforward_msg;
+    if (SubGetMessage(feedforward_sub, &feedforward_msg))
+    {
+        yaw_motor->feedforward_controller.feed_input = feedforward_msg.feedforward;
+    }
 
     /* 电机使能/失能控制 */
     static Motor_Enable_s enable_msg;

application/robot_def.h

@@ -63,6 +63,12 @@ typedef struct
     float pitch_Kp, pitch_Ki, pitch_Kd;
 } Motor_PID_Tune_s;
 
+/* ---- CMD 发布给 gimbal 的前馈输入 ---- */
+typedef struct
+{
+    float feedforward;
+} Gimbal_Feedforward_s;
+
 #pragma pack()
 
 #endif // !ROBOT_DEF_H

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_AUTO_LEN         18
-#define VISION_MANUAL_LEN  10
+#define VISION_MANUAL_LEN       10
-#define VISION_CMD_LEN     3
+#define VISION_CMD_LEN          3
-#define VISION_DEBUG_LEN   50
+#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;