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);
-
-    DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
-    DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+    /* 角度环参考值设定 */
+    if(SubGetMessage(gimbal_sub, &gimbal_cmd_recv))
+    {
+        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