damiao motor driver init

application/gimbal/gimbal.c

@@ -79,12 +79,20 @@ void GimbalInit()
             },
             .speed_PID = {
                 .Kp = 50,  // 50
-                .Ki = 350, // 350
+                .Ki = 175, // 350
                 .Kd = 0,   // 0
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 2500,
-                .MaxOut = 20000,
+                .MaxOut = 12000,
             },
+            // .current_PID = {  // 添加current_PID配置
+            //     .Kp = 0,
+            //     .Ki = 0,
+            //     .Kd = 0,
+            //     .Improve = PID_Integral_Limit,
+            //     .IntegralLimit = 5000,
+            //     .MaxOut = 15000,
+            // },
             .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
             // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
             .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
@@ -95,6 +103,7 @@ void GimbalInit()
             .outer_loop_type = ANGLE_LOOP,
             .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
             .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
         },
         .motor_type = DM4310,
     };

application/robot_def.h

@@ -208,6 +208,10 @@ typedef struct
     attitude_t gimbal_imu_data;
     uint16_t yaw_motor_single_round_angle;
     uint16_t yaw_motor_total_angle;
+    //uint16_t pitch_motor_single_round_angle;
+    //uint16_t pitch_motor_total_angle;
+        // 后续增加云台的其他数据
+        // ...
 } Gimbal_Upload_Data_s;
 
 typedef struct

modules/motor/DJImotor/dji_motor.c

@@ -20,6 +20,7 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
  * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
  */
 static CANInstance sender_assignment[6] = {
+    //[0] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
     [0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
     [1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
     [2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
@@ -51,12 +52,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
         if (motor_id < 4) // 根据ID分组
         {
             motor_send_num = motor_id;
-            motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
+            motor_grouping = config->can_handle == &hcan2 ? 1 : 4;
         }
         else
         {
             motor_send_num = motor_id - 4;
-            motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
+            motor_grouping = config->can_handle == &hcan2 ? 0 : 3;
         }
 
         // 计算接收id并设置分组发送id
@@ -71,7 +72,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
             if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
             {
                 LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
-                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
+                uint16_t can_bus = config->can_handle == &hcan2 ? 1 : 2;
                 while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
                     LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
             }
@@ -90,22 +91,6 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
             motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
         }
 
-        config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
-        sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
-        motor->message_num = motor_send_num;
-        motor->sender_group = motor_grouping;
-
-        for (size_t i = 0; i < idx; ++i)
-        {
-            if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
-            {
-                LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
-                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
-                while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
-                    LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
-            }
-        }
-        break;
         case DM4310:
             if (motor_id < 4) // 根据ID分组
             {
@@ -307,7 +292,7 @@ void DJIMotorControl()
             pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
         }
 
-        // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
+        //计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
          if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
              pid_ref += *motor_controller->current_feedforward_ptr;
          if (motor_setting->close_loop_type & CURRENT_LOOP)
@@ -321,95 +306,21 @@ void DJIMotorControl()
         // 获取最终输出
         set = (int16_t)pid_ref;
 
-
         // 分组填入发送数据
         group = motor->sender_group;
         num = motor->message_num;
+        //DM电机单独处理,因为它的发送格式与DJI电机不同,但仍然使用sender_assignment进行分组发送
+        if (i == 1)
+        {
+            sender_assignment[4].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位
+            sender_assignment[4].tx_buff[7] = (uint8_t)(set >> 8); // 低八位
+        }
+
+        else
+        {
             sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8);         // 低八位
              sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
-
-        // 若该电机处于停止状态,直接将buff置零
-        if (motor->stop_flag == MOTOR_STOP)
-            memset(sender_assignment[group].tx_buff + 2 * num, 0, 2u);
-    }
-
-    // 遍历flag,检查是否要发送这一帧报文
-    for (size_t i = 0; i < 6; ++i)
-    {
-        if (sender_enable_flag[i])
-        {
-            CANTransmit(&sender_assignment[i], 1);
-        }
-    }
-}
-void DM_MotorControl()
-{
-    // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
-    uint8_t group, num; // 电机组号和组内编号
-    int16_t set;        // 电机控制CAN发送设定值
-    DJIMotorInstance *motor;
-    Motor_Control_Setting_s *motor_setting; // 电机控制参数
-    Motor_Controller_s *motor_controller;   // 电机控制器
-    DJI_Motor_Measure_s *measure;           // 电机测量值
-    float pid_measure, pid_ref;             // 电机PID测量值和设定值
-
-    // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
-    for (size_t i = 0; i < idx; ++i)
-    { // 减小访存开销,先保存指针引用
-        motor = dji_motor_instance[i];
-        motor_setting = &motor->motor_settings;
-        motor_controller = &motor->motor_controller;
-        measure = &motor->measure;
-        pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
-        if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
-            pid_ref *= -1; // 设置反转
-
-        // pid_ref会顺次通过被启用的闭环充当数据的载体
-        // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
-        if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
-        {
-            if (motor_setting->angle_feedback_source == OTHER_FEED)
-                pid_measure = *motor_controller->other_angle_feedback_ptr;
-            else
-                pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
-            // 更新pid_ref进入下一个环
-            pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
-        }
-
-        // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
-        if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
-        {
-            if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
-                pid_ref += *motor_controller->speed_feedforward_ptr;
-
-            if (motor_setting->speed_feedback_source == OTHER_FEED)
-                pid_measure = *motor_controller->other_speed_feedback_ptr;
-            else // MOTOR_FEED
-                pid_measure = measure->speed_aps;
-            // 更新pid_ref进入下一个环
-            pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
-        }
-
-        // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
-        if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
-            pid_ref += *motor_controller->current_feedforward_ptr;
-        if (motor_setting->close_loop_type & CURRENT_LOOP)
-        {
-            pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
-        }
-
-        if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
-            pid_ref *= -1;
-
-        // 获取最终输出
-        set = (int16_t)pid_ref;
-
-
-        // 分组填入发送数据
-        group = motor->sender_group;
-        num = motor->message_num;
-        sender_assignment[group].tx_buff[num + 6] = (uint8_t)(set & 0x00ff); // 高八位
-        sender_assignment[group].tx_buff[num + 7] = (uint8_t)(set >> 8); // 低八位
+        }
 
         // 若该电机处于停止状态,直接将buff置零
         if (motor->stop_flag == MOTOR_STOP)

modules/motor/motor_task.c

@@ -13,7 +13,6 @@ void MotorControlTask()
     if(cnt%2==0) //500hz
     {
         DJIMotorControl();
-        DM_MotorControl();
     }
 
     /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */