From 81099d79f1282b09bb997043e5f4eebb7d1a41b7 Mon Sep 17 00:00:00 2001
From: LTS <631571182@qq.com>
Date: Sun, 15 Oct 2023 18:06:23 +0800
Subject: [PATCH] speed_feedforward

---
 modules/motor/DJImotor/dji_motor.c | 10 +++++++++-
 modules/motor/DJImotor/dji_motor.h |  4 +++-
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c
index e9345cc..20137e0 100644
--- a/modules/motor/DJImotor/dji_motor.c
+++ b/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;
diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h
index bd07c1a..73a91de 100644
--- a/modules/motor/DJImotor/dji_motor.h
+++ b/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