speed_feedforward

modules/motor/DJImotor/dji_motor.c

@@ -240,7 +240,8 @@ void DJIMotorControl()
     Motor_Controller_s *motor_controller;   // 电机控制器
     DJI_Motor_Measure_s *measure;           // 电机测量值
     float pid_measure, pid_ref;             // 电机PID测量值和设定值
-
+    float speed_acceleration;               // 加速度
+    
     // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
     for (size_t i = 0; i < idx; ++i)
     { // 减小访存开销,先保存指针引用
@@ -268,7 +269,14 @@ void DJIMotorControl()
         if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
         {
             if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
+            {
+                // 计算加速度，并作为前馈数据
+                speed_acceleration = motor_controller->speed_PID.Measure - motor_controller->speed_PID.Last_Measure;
+                if(speed_acceleration >= SPEED_FEEDFORWARD_MAX_DIFFERENTIABLE) // 防止微分无穷大
+                    speed_acceleration = SPEED_FEEDFORWARD_MAX_DIFFERENTIABLE;
+                motor_controller->speed_feedforward_ptr = &speed_acceleration;
                 pid_ref += *motor_controller->speed_feedforward_ptr;
+            }
 
             if (motor_setting->speed_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_speed_feedback_ptr;

modules/motor/DJImotor/dji_motor.h

@@ -26,7 +26,9 @@
 /* 滤波系数设置为1的时候即关闭滤波 */
 #define SPEED_SMOOTH_COEF 0.85f      // 最好大于0.85
 #define CURRENT_SMOOTH_COEF 0.9f     // 必须大于0.9
-#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制
+#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制、
+
+#define SPEED_FEEDFORWARD_MAX_DIFFERENTIABLE 3000
 
 /* DJI电机CAN反馈信息*/
 typedef struct