添加了大量调试log，新增了dwt计时宏，增加了USB软件复位防止主控复位后上位机无法连接usb

2026-07-24 03:27:45 +08:00 · 2023-06-22 21:52:46 +08:00
parent ea6163a48d
commit 4a45331d31
31 changed files with 247 additions and 275 deletions
--- a/modules/motor/DJImotor/dji_motor.c
+++ b/modules/motor/DJImotor/dji_motor.c
@@ -4,14 +4,15 @@
 #include "bsp_log.h"

 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
-
 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
-static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL};
+static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在control任务中遍历该指针数组进行pid计算

 /**
 * @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
 *        该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
 *
+ * @note  因为只用于发送,所以不需要在bsp_can中注册
+ *
 * C610(m2006)/C620(m3508):0x1ff,0x200;
 * GM6020:0x1ff,0x2ff
 * 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
@@ -69,9 +70,10 @@ 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."); // 后续可以把id和CAN打印出来
-                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 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) // 后续可以把id和CAN打印出来 // 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;
@@ -98,16 +100,16 @@ 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.");
-                while (1)
-                    ; // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8)
+                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
+                while (1) // 后续可以把id和CAN打印出来 // 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;

    default: // other motors should not be registered here
-        LOGERROR("You must not register other motors using the API of DJI motor.");
        while (1)
-            ; // 其他电机不应该在这里注册
+            LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
    }
 }

@@ -148,6 +150,9 @@ static void DecodeDJIMotor(CANInstance *_instance)

 static void DJIMotorLostCallback(void *motor_ptr)
 {
+    DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
+    uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
+    LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
 }

 // 电机初始化,返回一个电机实例
@@ -182,7 +187,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
    Daemon_Init_Config_s daemon_config = {
        .callback = DJIMotorLostCallback,
        .owner_id = instance,
-        .reload_count = 1, // 10ms未收到数据则丢失
+        .reload_count = 2, // 20ms未收到数据则丢失
    };
    instance->daemon = DaemonRegister(&daemon_config);

@@ -195,17 +200,11 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
 void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
 {
    if (loop == ANGLE_LOOP)
-    {
        motor->motor_settings.angle_feedback_source = type;
-    }
    else if (loop == SPEED_LOOP)
-    {
        motor->motor_settings.speed_feedback_source = type;
-    }
    else
-    {
        LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
-    }
 }

 void DJIMotorStop(DJIMotorInstance *motor)
@@ -299,11 +298,9 @@ void DJIMotorControl()
        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)
-        { // 若该电机处于停止状态,直接将buff置零
            memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
-        }
    }

    // 遍历flag,检查是否要发送这一帧报文
--- a/modules/motor/HTmotor/HT04.c
+++ b/modules/motor/HTmotor/HT04.c
@@ -6,6 +6,7 @@
 #include "string.h"
 #include "daemon.h"
 #include "stdlib.h"
+#include "bsp_log.h"

 static uint8_t idx;
 static HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
@@ -64,17 +65,14 @@ static void HTMotorDecode(CANInstance *motor_can)
    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
    measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
                            (1 - CURRENT_SMOOTH_COEF) * measure->real_current;
-    
-    
 }

 static void HTMotorLostCallback(void *motor_ptr)
 {
    HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
-    if (motor->stop_flag == MOTOR_STOP)
-        return;
+    LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
    if (++motor->lost_cnt % 10 != 0)
-        HTMotorSetMode(CMD_MOTOR_MODE, motor); // 若不在停止模式,尝试重新让电机进入控制模式
+        HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
 }

 /* 海泰电机一生黑,什么垃圾协议! */
@@ -99,7 +97,7 @@ void HTMotorCalibEncoder(HTMotorInstance *motor)
    memcpy(zero_buff, buf, 6); // 初始化的时候至少调用一次,故将其他指令为0时发送的报文保存一下,详见ht04电机说明
    CANTransmit(motor->motor_can_instace, 1);
    DWT_Delay(0.005);
-    HTMotorSetMode(CMD_ZERO_POSITION, motor);
+    HTMotorSetMode(CMD_ZERO_POSITION, motor); // sb 玩意校准完了编码器也不为0
    DWT_Delay(0.005);
    // HTMotorSetMode(CMD_MOTOR_MODE, motor);
 }
@@ -215,7 +213,7 @@ void HTMotorControlInit()
        char ht_id_buff[2] = {0};
        __itoa(i, ht_id_buff, 10);
        strcat(ht_task_name, ht_id_buff); // 似乎没什么吊用,osthreaddef会把第一个变量当作宏字符串传入,作为任务名
-        // todo 还需要一个更优雅的方案来区分不同的电机任务
+        // @todo 还需要一个更优雅的方案来区分不同的电机任务
        osThreadDef(ht_task_name, HTMotorTask, osPriorityNormal, 0, 128);
        ht_task_handle[i] = osThreadCreate(osThread(ht_task_name), ht_motor_instance[i]);
    }
--- a/modules/motor/HTmotor/HT04.md
+++ b/modules/motor/HTmotor/HT04.md
@@ -40,3 +40,5 @@ static void HTMotorDecode(CANInstance *motor_can)
    }
 }
 ```
+
+第一次收到数据时默认电机处于限位处,将速度和角度都设置为零,记录当前的编码器数据,之后每次收到都减去该值.
--- a/modules/motor/LKmotor/LK9025.c
+++ b/modules/motor/LKmotor/LK9025.c
@@ -3,6 +3,7 @@
 #include "general_def.h"
 #include "daemon.h"
 #include "bsp_dwt.h"
+#include "bsp_log.h"

 static uint8_t idx;
 static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
@@ -43,6 +44,12 @@ static void LKMotorDecode(CANInstance *_instance)
    measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
 }

+static void LKMotorLostCallback(void *motor_ptr)
+{
+    LKMotorInstance *motor = (LKMotorInstance *)motor_ptr;
+    LOGWARNING("[LKMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
+}
+
 LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
 {
    LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
@@ -75,7 +82,7 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
    Daemon_Init_Config_s daemon_config = {
        .callback = NULL,
        .owner_id = motor,
-        .reload_count = 5, // 0.05秒
+        .reload_count = 5, // 50ms
    };
    motor->daemon = DaemonRegister(&daemon_config);