新增通用定义头文件,主要内容是角度转换.统一控制的输入,为后续model-based控制做准备

modules/general_def.h

@@ -0,0 +1,13 @@
+#ifndef GENERAL_DEF_H
+#define GENERAL_DEF_H
+
+#define PI 3.1415926535f
+#define PI2 (PI * 2.0f) // 2 pi
+
+#define RAD_2_ANGLE (180.0f / PI)
+#define ANGLE_2_RAD (PI / 180.0f)
+
+#define RPM_2_ANGLE_PER_SEC (360.0f/60.0f) // ×360°/60sec
+
+
+#endif // !GENERAL_DEF_H

modules/motor/dji_motor.c

@@ -1,6 +1,6 @@
 #include "dji_motor.h"
+#include "general_def.h"
 
-#define PI2 (3.141592f * 2)  // 2pi
 #define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
 
 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
@@ -129,20 +129,24 @@ static void DecodeDJIMotor(can_instance *_instance)
         if (dji_motor_info[i]->motor_can_instance == _instance)
         {
             rxbuff = _instance->rx_buff;
-            measure = &dji_motor_info[i]->motor_measure;
+            measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销
+
             // resolve data and apply filter to current and speed
             measure->last_ecd = measure->ecd;
             measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
-            // 增加滤波
-            measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
-            measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
+            measure->angle_single_round = ECD_ANGLE_COEF* measure->ecd;
+            measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + 
+                                           RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
+            measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + 
+                                     RPM_2_ANGLE_PER_SEC * CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
             measure->temperate = rxbuff[6];
-            // multi round calc
-            if (measure->ecd - measure->last_ecd > 4096)
+
+            // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
+            if (measure->ecd - measure->last_ecd > 4096) 
                 measure->total_round--;
             else if (measure->ecd - measure->last_ecd < -4096)
                 measure->total_round++;
-            measure->total_angle = measure->total_round * 360 + measure->ecd * ECD_ANGLE_COEF; // @todo simplify the calculation
+            measure->total_angle = measure->total_round * 360 + measure->angle_single_round; 
             break;
         }
     }
@@ -164,8 +168,10 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config)
     PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
     dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
     dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+    
     // group motors, because 4 motors share the same CAN control message
     MotorSenderGrouping(&config->can_init_config);
+
     // register motor to CAN bus
     config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
     dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
@@ -173,7 +179,6 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config)
     return dji_motor_info[idx++];
 }
 
-// 改变反馈来源
 void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
 {
     if (loop == ANGLE_LOOP)
@@ -186,12 +191,28 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba
     }
 }
 
+void DJIMotorStop(dji_motor_instance *motor)
+{
+    motor->stop_flag = MOTOR_STOP;
+}
+
+void DJIMotorEnable(dji_motor_instance *motor)
+{
+    motor->stop_flag = MOTOR_ENALBED;
+}
+
+void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop)
+{
+    motor->motor_settings.outer_loop_type = outer_loop;
+}
+
 // 设置参考值
 void DJIMotorSetRef(dji_motor_instance *motor, float ref)
 {
     motor->motor_controller.pid_ref = ref;
 }
-// 计算三环PID,发送控制报文
+
+// 为所有电机实例计算三环PID,发送控制报文
 void DJIMotorControl()
 {
     // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
@@ -231,7 +252,7 @@ void DJIMotorControl()
                 if (motor_setting->speed_feedback_source == OTHER_FEED)
                     pid_measure = *motor_controller->other_speed_feedback_ptr;
                 else
-                    pid_measure = motor_measure->speed_rpm;
+                    pid_measure = motor_measure->speed_angle_per_sec;
                 // 更新pid_ref进入下一个环
                 motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
             }
@@ -272,18 +293,3 @@ void DJIMotorControl()
         }
     }
 }
-
-void DJIMotorStop(dji_motor_instance *motor)
-{
-    motor->stop_flag = MOTOR_STOP;
-}
-
-void DJIMotorEnable(dji_motor_instance *motor)
-{
-    motor->stop_flag = MOTOR_ENALBED;
-}
-
-void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop)
-{
-    motor->motor_settings.outer_loop_type = outer_loop;
-}

modules/motor/dji_motor.h

@@ -22,19 +22,20 @@
 #define DJI_MOTOR_CNT 12
 
 /* 滤波系数设置为1的时候即关闭滤波 */
-#define SPEED_SMOOTH_COEF 0.85f   // better to be greater than 0.8
-#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
+#define SPEED_SMOOTH_COEF 0.9f    // better to be greater than 0.85
+#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95
 
 /* DJI电机CAN反馈信息*/
 typedef struct
 {
-    uint16_t ecd; // 0-8192
-    uint16_t last_ecd;
-    int16_t speed_rpm;     // rounds per minute
-    int16_t given_current; // 实际电流
-    uint8_t temperate;
-    int16_t total_round; // 总圈数,注意方向
-    int32_t total_angle; // 总角度,注意方向
+    uint16_t ecd;              // 0-8191,刻度总共有8192格
+    uint16_t last_ecd;         // 上一次读取的编码器值
+    float angle_single_round;  // 单圈角度
+    float speed_angle_per_sec; // 度/秒 rounds per minute
+    int16_t given_current;     // 实际电流
+    uint8_t temperate;         // 温度 Celsius
+    int16_t total_round;       // 总圈数,注意方向
+    int32_t total_angle;       // 总角度,注意方向
 } dji_motor_measure;
 
 /**
@@ -59,9 +60,8 @@ typedef struct
     uint8_t sender_group;
     uint8_t message_num;
 
-    Motor_Type_e motor_type;
-
-    Motor_Working_Type_e stop_flag;
+    Motor_Type_e motor_type;        // 电机类型
+    Motor_Working_Type_e stop_flag; // 启停标志
 
 } dji_motor_instance;
 
@@ -111,22 +111,21 @@ void DJIMotorControl();
  * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
  *
  */
-void DJIMotorStop(dji_motor_instance* motor);
+void DJIMotorStop(dji_motor_instance *motor);
 
 /**
  * @brief 启动电机,此时电机会响应设定值
  *        初始化时不需要此函数,因为stop_flag的默认值为0
  *
  */
-void DJIMotorEnable(dji_motor_instance* motor);
+void DJIMotorEnable(dji_motor_instance *motor);
 
 /**
  * @brief 修改电机闭环目标(外层闭环)
- * 
+ *
  * @param motor  要修改的电机实例指针
  * @param outer_loop 外层闭环类型
  */
 void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop);
 
-
 #endif // !DJI_MOTOR_H

modules/motor/dji_motor.md

@@ -15,9 +15,17 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
 
 **==设定值的单位==**
 
-1. 位置环为角度，角度制。
-2. 速度环为角速度，单位为度/每秒
-3. 
+1. ==位置环为**角度制**（0-360，total_angle可以为任意值）==
+
+2. ==速度环为角速度，单位为**度/每秒**（deg/sec）==
+
+3. ==电流环为mA==
+
+4. ==GM6020的输入设定为**力矩**，待测量（-30000~30000）==
+
+   ==M3508的输入设定为-20A~20A （-16384~16384）==
+
+   ==M2006的输入设定为-10A~10A （-10000~10000）==
 
 如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。
 
@@ -419,6 +427,12 @@ static void DecodeDJIMotor(can_instance *_instance)
 - `DecodeDJIMotor()`是解析电机反馈报文的函数，在`DJIMotorInit()`中会将其注册到该电机实例对应的`can_instance`中（即`can_instance`的`can_module_callback()`）。这样，当该电机的反馈报文到达时，`bsp_can.c`中的回调函数会调用解包函数进行反馈数据解析。
 
   该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。
+  
+  **电机反馈的电流值为说明书中的映射值，需转换为实际值。**
+  
+  **反馈的速度单位是rpm（转每分钟），转换为角度每秒。**
+  
+  **反馈的位置是编码器值（0~8191），转换为角度。**
 
 ## 使用范例