From ed6d509c725b4f6ce5dfdbd7bd0be65204d39ab5 Mon Sep 17 00:00:00 2001
From: SkyLi <SkyLiA7Mr@gmail.com>
Date: Sun, 7 Jun 2026 16:22:16 +0800
Subject: [PATCH] feat: add direction flag to feedforward controller with init
 config struct

- add DM_Feedforward_Init_Config_s with Kp and dir fields
- replace plain DM_Feedforward_Kp in DM_Motor_Init_Config_s
- apply direction flag in DMMotorControl to negate feedforward when reverse
- set yaw feedforward: Kp=0.9, dir=REVERSE
---
 application/gimbal/gimbal.c     |  5 ++++-
 modules/motor/DMmotor/dmmotor.c |  8 +++++---
 modules/motor/DMmotor/dmmotor.h | 10 +++++++++-
 3 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c
index 5e7759b..9d9e6d4 100644
--- a/application/gimbal/gimbal.c
+++ b/application/gimbal/gimbal.c
@@ -52,7 +52,10 @@ void GimbalInit()
             .motor_type = DM_MOTOR,
         },
         .ctrl_mode = DM_MODE_VELOCITY,
-        .DM_Feedforward_Kp = 0.9f,
+        .feedforward_config = {
+            .Kp  = 0.9f,
+            .dir = OUTPUT_DIRECTION_REVERSE,
+        },
         .mit_kp = 0, .mit_kd = 0,
     };
     yaw_motor = DMMotorInit(&yaw_config);
diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c
index 027e0b0..4a43eb8 100644
--- a/modules/motor/DMmotor/dmmotor.c
+++ b/modules/motor/DMmotor/dmmotor.c
@@ -186,8 +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;
+    /* 设置前馈控制器 */
+    motor->feedforward_controller.Kp  = config->feedforward_config.Kp;
+    motor->feedforward_controller.dir = config->feedforward_config.dir;
 
     /* register daemon */
     Daemon_Init_Config_s conf = {
@@ -237,7 +238,8 @@ void DMMotorControl(void)
         if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE)
             pid_out *= -1;
 
-        pid_out += feedforward_controller.Kp * feedforward_controller.feed_input;
+        float ff = feedforward_controller.Kp * feedforward_controller.feed_input;
+        pid_out += (feedforward_controller.dir == OUTPUT_DIRECTION_REVERSE) ? -ff : ff;
 
         float vel_cmd = pid_out * DEGREE_2_RAD;
         LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h
index e595e6b..da16c1c 100644
--- a/modules/motor/DMmotor/dmmotor.h
+++ b/modules/motor/DMmotor/dmmotor.h
@@ -56,6 +56,7 @@ typedef struct
 
 typedef struct{
     float Kp;
+    Output_Reverse_Flag_e dir;
     float feed_input;
 } DM_Feedforward_Controller_s;
 
@@ -76,12 +77,19 @@ typedef struct
 
 } DMMotorInstance;
 
+/* Feedforward controller init config */
+typedef struct
+{
+    float Kp;
+    Output_Reverse_Flag_e dir;
+} DM_Feedforward_Init_Config_s;
+
 /* DM motor init config */
 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;
+    DM_Feedforward_Init_Config_s feedforward_config;
     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;