关节电机measure

application/chassis/balance.c

@@ -40,6 +40,18 @@
 #define STOOL_RC_DEADBAND 40
 #define CHASSIS_UNDERVOLTAGE_LIMIT 15.0f
 #define REFEREE_VOLTAGE_VALID_MIN 5.0f
+// Stage 1 permits only 0xFD safety-disable probes.
+#define JOINT_RX_ONLY_TEST 1u
+#define JOINT_RX_PROBE_INTERVAL 0.01f
+#define JOINT_RX_PROBE_START_DELAY 0.5f
+#define JOINT_RB_MOTOR_ID 0x01u
+#define JOINT_RF_MOTOR_ID 0x02u
+#define JOINT_LB_MOTOR_ID 0x03u
+#define JOINT_LF_MOTOR_ID 0x04u
+#define JOINT_RB_MASTER_ID 0x11u
+#define JOINT_RF_MASTER_ID 0x12u
+#define JOINT_LB_MASTER_ID 0x13u
+#define JOINT_LF_MASTER_ID 0x14u
 // 计时变量
 static uint32_t balance_dwt_cnt;
 static float del_t;
@@ -52,6 +64,22 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
 static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
 static LKMotorInstance *l_driven, *r_driven, *driven[2];
 
+volatile JointMotorRxOnlyDebug_s joint_rx_debug = {
+    .rx_only = JOINT_RX_ONLY_TEST,
+    .left_front = {
+        .expected_motor_id = JOINT_LF_MOTOR_ID,
+        .expected_feedback_can_id = JOINT_LF_MASTER_ID},
+    .left_back = {
+        .expected_motor_id = JOINT_LB_MOTOR_ID,
+        .expected_feedback_can_id = JOINT_LB_MASTER_ID},
+    .right_front = {
+        .expected_motor_id = JOINT_RF_MOTOR_ID,
+        .expected_feedback_can_id = JOINT_RF_MASTER_ID},
+    .right_back = {
+        .expected_motor_id = JOINT_RB_MOTOR_ID,
+        .expected_feedback_can_id = JOINT_RB_MASTER_ID},
+};
+
 // 两个腿的参数,0为左腿,1为右腿
 static LinkNPodParam l_side, r_side;
 static ChassisParam chassis;
@@ -77,6 +105,83 @@ static float stool_stop_dist;
 static float stool_target_wz;
 static uint8_t stool_spin_enabled;
 static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
+static float joint_rx_probe_start_elapsed;
+static float joint_rx_probe_elapsed;
+static uint8_t joint_rx_probe_index;
+
+static void UpdateJointRxDebugMotor(volatile JointMotorRxDebug_s *debug, DMMotorInstance *motor)
+{
+    DM_Motor_Measure_s *measure = &motor->measure;
+
+    debug->feedback_motor_id = measure->id;
+    debug->state = measure->state;
+    debug->feedback_can_id = measure->feedback_can_id;
+    debug->feedback_count = measure->feedback_count;
+    debug->received = measure->feedback_count > 0u;
+    debug->online = debug->received && DaemonIsOnline(motor->motor_daemon);
+    debug->id_matches = debug->received &&
+                        measure->id == debug->expected_motor_id;
+    debug->feedback_can_id_matches = debug->received &&
+                                     measure->feedback_can_id == debug->expected_feedback_can_id;
+    debug->position_rad = measure->position;
+    debug->velocity_rad_s = measure->velocity;
+    debug->torque = measure->torque;
+    debug->mos_temperature_c = measure->T_Mos;
+    debug->rotor_temperature_c = measure->T_Rotor;
+}
+
+static void UpdateJointRxDebug(void)
+{
+    UpdateJointRxDebugMotor(&joint_rx_debug.left_front, lf);
+    UpdateJointRxDebugMotor(&joint_rx_debug.left_back, lb);
+    UpdateJointRxDebugMotor(&joint_rx_debug.right_front, rf);
+    UpdateJointRxDebugMotor(&joint_rx_debug.right_back, rb);
+
+    joint_rx_debug.rx_only = lf->rx_only && lb->rx_only &&
+                             rf->rx_only && rb->rx_only;
+    joint_rx_debug.all_received = joint_rx_debug.left_front.received &&
+                                  joint_rx_debug.left_back.received &&
+                                  joint_rx_debug.right_front.received &&
+                                  joint_rx_debug.right_back.received;
+    joint_rx_debug.all_online = joint_rx_debug.left_front.online &&
+                                joint_rx_debug.left_back.online &&
+                                joint_rx_debug.right_front.online &&
+                                joint_rx_debug.right_back.online;
+    joint_rx_debug.all_ids_match = joint_rx_debug.left_front.id_matches &&
+                                   joint_rx_debug.left_back.id_matches &&
+                                   joint_rx_debug.right_front.id_matches &&
+                                   joint_rx_debug.right_back.id_matches;
+    joint_rx_debug.all_feedback_can_ids_match =
+        joint_rx_debug.left_front.feedback_can_id_matches &&
+        joint_rx_debug.left_back.feedback_can_id_matches &&
+        joint_rx_debug.right_front.feedback_can_id_matches &&
+        joint_rx_debug.right_back.feedback_can_id_matches;
+}
+
+static void JointRxOnlyProbe(void)
+{
+    if (!lf->rx_only || !lb->rx_only || !rf->rx_only || !rb->rx_only)
+        return;
+
+    if (joint_rx_probe_start_elapsed < JOINT_RX_PROBE_START_DELAY)
+    {
+        joint_rx_probe_start_elapsed += del_t;
+        return;
+    }
+
+    joint_rx_probe_elapsed += del_t;
+    if (joint_rx_probe_elapsed < JOINT_RX_PROBE_INTERVAL)
+        return;
+
+    joint_rx_probe_elapsed = 0.0f;
+    joint_rx_debug.disable_probe_count++;
+    if (!DMMotorSendDisable(joint[joint_rx_probe_index]))
+        joint_rx_debug.disable_probe_fail_count++;
+
+    joint_rx_probe_index++;
+    if (joint_rx_probe_index >= JOINT_CNT)
+        joint_rx_probe_index = 0u;
+}
 
 static void ClearMotionTargets(void)
 {
@@ -213,18 +318,20 @@ void BalanceInit()
             .speed_feedback_source = MOTOR_FEED,
         },
         .motor_type = DM8009P};
-    joint_conf.can_init_config.tx_id = 3;
+    joint_conf.can_init_config.tx_id = JOINT_LF_MOTOR_ID;
-    joint_conf.can_init_config.rx_id = 14;
+    joint_conf.can_init_config.rx_id = JOINT_LF_MASTER_ID;
     joint[LF] = lf = DMMotorInit(&joint_conf);
-    joint_conf.can_init_config.tx_id = 2;
+    joint_conf.can_init_config.tx_id = JOINT_LB_MOTOR_ID;
-    joint_conf.can_init_config.rx_id = 13;
+    joint_conf.can_init_config.rx_id = JOINT_LB_MASTER_ID;
     joint[LB] = lb = DMMotorInit(&joint_conf);
-    joint_conf.can_init_config.tx_id = 1;
+    joint_conf.can_init_config.tx_id = JOINT_RF_MOTOR_ID;
-    joint_conf.can_init_config.rx_id = 12;
+    joint_conf.can_init_config.rx_id = JOINT_RF_MASTER_ID;
     joint[RF] = rf = DMMotorInit(&joint_conf);
-    joint_conf.can_init_config.tx_id = 0;
+    joint_conf.can_init_config.tx_id = JOINT_RB_MOTOR_ID;
-    joint_conf.can_init_config.rx_id = 11;
+    joint_conf.can_init_config.rx_id = JOINT_RB_MASTER_ID;
     joint[RB] = rb = DMMotorInit(&joint_conf);
+    for (uint8_t i = 0; i < JOINT_CNT; i++)
+        DMMotorSetRxOnly(joint[i], JOINT_RX_ONLY_TEST);
 
     // 驱动轮电机
     Motor_Init_Config_s driven_conf = {
@@ -313,6 +420,7 @@ void BalanceInit()
     chassis_status = ROBOT_STOP;
     ResetStoolRuntime();
     DisableAllJointMotor();
+    // Clear stale enable state if the MCU resets without cycling motor power.
     for (uint8_t i = 0; i < JOINT_CNT; i++)
         DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]);
     StopDrivenMotor();
@@ -373,7 +481,7 @@ static void ControlSwitch()
     }
 }
 
-/* Joint motors must remain disabled on this demo chassis. */
+/* Stage 1 keeps all joint motors disabled and receive-only. */
 static void ResetChassis()
 {
     ClearMotionTargets();
@@ -565,6 +673,7 @@ static void CommNPower()
 void BalanceTask()
 {
     del_t = DWT_GetDeltaT(&balance_dwt_cnt);
+    JointRxOnlyProbe();
 
     BuzzerOn();
     // 切换遥控器控制or云台板控制
@@ -575,6 +684,7 @@ void BalanceTask()
     CommNPower();
     // 参数组装
     ParamAssemble();
+    UpdateJointRxDebug();
 
     if (chassis_status == ROBOT_STOP ||
         chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||

application/chassis/balance.h

@@ -38,6 +38,44 @@
 #define LD 0u
 #define RD 1u
 
+typedef struct
+{
+    uint8_t expected_motor_id;
+    uint8_t feedback_motor_id;
+    uint8_t state;
+    uint8_t received;
+    uint8_t online;
+    uint8_t id_matches;
+    uint8_t feedback_can_id_matches;
+    uint8_t reserved;
+    uint32_t expected_feedback_can_id;
+    uint32_t feedback_can_id;
+    uint32_t feedback_count;
+    float position_rad;
+    float velocity_rad_s;
+    float torque;
+    float mos_temperature_c;
+    float rotor_temperature_c;
+} JointMotorRxDebug_s;
+
+typedef struct
+{
+    uint8_t rx_only;
+    uint8_t all_received;
+    uint8_t all_online;
+    uint8_t all_ids_match;
+    uint8_t all_feedback_can_ids_match;
+    uint8_t reserved[3];
+    uint32_t disable_probe_count;
+    uint32_t disable_probe_fail_count;
+    JointMotorRxDebug_s left_front;
+    JointMotorRxDebug_s left_back;
+    JointMotorRxDebug_s right_front;
+    JointMotorRxDebug_s right_back;
+} JointMotorRxOnlyDebug_s;
+
+extern volatile JointMotorRxOnlyDebug_s joint_rx_debug;
+
 typedef struct
 {
     // joint

bsp/can/bsp_can.c

@@ -105,6 +105,7 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
     instance->can_handle = config->can_handle;
     instance->tx_id = config->tx_id; // 好像没用,可以删掉
     instance->rx_id = config->rx_id;
+    instance->rx_id_match_any = config->rx_id_match_any;
     instance->can_module_callback = config->can_module_callback;
     instance->id = config->id;
 
@@ -141,6 +142,17 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout)
     return 1; // 发送成功
 }
 
+uint8_t CANTryTransmit(CANInstance *_instance)
+{
+    if (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0u)
+        return 0u;
+
+    return HAL_CAN_AddTxMessage(_instance->can_handle,
+                                &_instance->txconf,
+                                _instance->tx_buff,
+                                &_instance->tx_mailbox) == HAL_OK;
+}
+
 void CANSetDLC(CANInstance *_instance, uint8_t length)
 {
     // 发送长度错误!检查调用参数是否出错,或出现野指针/越界访问
@@ -168,15 +180,16 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
         HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); // 从FIFO中获取数据
         for (size_t i = 0; i < idx; ++i)
         { // 两者相等说明这是要找的实例
-            if (_hcan == can_instance[i]->can_handle && rxconf.StdId == can_instance[i]->rx_id)
+            if (_hcan == can_instance[i]->can_handle &&
+                (can_instance[i]->rx_id_match_any || rxconf.StdId == can_instance[i]->rx_id))
             {
                 if (can_instance[i]->can_module_callback != NULL) // 回调函数不为空就调用
                 {
+                    can_instance[i]->rx_std_id = rxconf.StdId;
                     can_instance[i]->rx_len = rxconf.DLC;                      // 保存接收到的数据长度
                     memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例
                     can_instance[i]->can_module_callback(can_instance[i]);     // 触发回调进行数据解析和处理
                 }
-                break;
             }
         }
     }

bsp/can/bsp_can.h

@@ -21,7 +21,9 @@ typedef struct _
     uint8_t tx_buff[8];            // 发送缓存,发送消息长度可以通过CANSetDLC()设定,最大为8
     uint8_t rx_buff[8];            // 接收缓存,最大消息长度为8
     uint32_t rx_id;                // 接收id
+    uint32_t rx_std_id;            // 实际收到的标准帧id
     uint8_t rx_len;                // 接收长度,可能为0-8
+    uint8_t rx_id_match_any;       // 软件层接收该CAN总线上的任意标准帧
     // 接收的回调函数,用于解析接收到的数据
     void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
     void *id;                                // 使用can外设的模块指针(即id指向的模块拥有此can实例,是父子关系)
@@ -34,6 +36,7 @@ typedef struct
     CAN_HandleTypeDef *can_handle;              // can句柄
     uint32_t tx_id;                             // 发送id
     uint32_t rx_id;                             // 接收id
+    uint8_t rx_id_match_any;                    // 忽略rx_id,由模块自行解析报文id
     void (*can_module_callback)(CANInstance *); // 处理接收数据的回调函数
     void *id;                                   // 拥有can实例的模块地址,用于区分不同的模块(如果有需要的话),如果不需要可以不传入
 } CAN_Init_Config_s;
@@ -65,4 +68,6 @@ void CANSetDLC(CANInstance *_instance, uint8_t length);
  */
 uint8_t CANTransmit(CANInstance *_instance,float timeout);
 
+uint8_t CANTryTransmit(CANInstance *_instance);
+
 #endif

modules/motor/DMmotor/dmmotor.c

@@ -7,7 +7,7 @@
 #include "stdlib.h"
 #include "bsp_log.h"
 
-#define DM_SEND_DELAY 0.2
+#define DM_MODE_SEND_DELAY_S 0.01f
 
 static uint8_t idx;
 static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
@@ -27,12 +27,20 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
     return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
 }
 
+static uint8_t DMMotorTransmitMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
+{
+    memset(motor->motor_can_instace->tx_buff, 0xff, 7u);
+    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
+    return CANTransmit(motor->motor_can_instace, 1);
+}
+
 void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
 {
-    memset(motor->motor_can_instace->tx_buff, 0xff, 7);
+    if (motor->rx_only && cmd != DM_CMD_RESET_MODE)
-    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
+        return;
-    CANTransmit(motor->motor_can_instace, 1);
+
-	DWT_Delay(DM_SEND_DELAY);
+    DMMotorTransmitMode(cmd, motor);
+	DWT_Delay(DM_MODE_SEND_DELAY_S);
 }
 
 static void DMMotorDecode(CANInstance *motor_can)
@@ -42,9 +50,17 @@ static void DMMotorDecode(CANInstance *motor_can)
     DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
     DM_Motor_Measure_s *measure = &(motor->measure);
 
+    if (motor_can->rx_len != 8u)
+        return;
+
+    uint8_t feedback_id = rxbuff[0] & 0x0fu;
+    if (feedback_id != (motor_can->tx_id & 0x0fu))
+        return;
+
     DaemonReload(motor->motor_daemon);
-    measure->id = rxbuff[0];
+    measure->id = feedback_id;
     measure->state = (rxbuff[0] >> 4) & 0xf;
+    measure->feedback_can_id = motor_can->rx_std_id;
     measure->last_position = measure->position;
     tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
     measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
@@ -60,6 +76,7 @@ static void DMMotorDecode(CANInstance *motor_can)
     measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
     measure->T_Mos = (float)rxbuff[6];
     measure->T_Rotor = (float)rxbuff[7];
+    measure->feedback_count++;
 }
 
 static void DMMotorLostCallback(void *motor_ptr)
@@ -88,6 +105,7 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
     motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
 
     config->can_init_config.can_module_callback = DMMotorDecode;
+    config->can_init_config.rx_id_match_any = 1u;
     config->can_init_config.id = motor;
     motor->motor_can_instace = CANRegister(&config->can_init_config);
 
@@ -109,6 +127,9 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref)
 
 void DMMotorEnable(DMMotorInstance *motor)
 {
+    if (motor->rx_only)
+        return;
+
     motor->stop_flag = MOTOR_ENALBED;
 }
 
@@ -117,6 +138,21 @@ void DMMotorStop(DMMotorInstance *motor)
     motor->stop_flag = MOTOR_STOP;
 }
 
+uint8_t DMMotorSendDisable(DMMotorInstance *motor)
+{
+    DMMotorStop(motor);
+    memset(motor->motor_can_instace->tx_buff, 0xff, 7u);
+    motor->motor_can_instace->tx_buff[7] = (uint8_t)DM_CMD_RESET_MODE;
+    return CANTryTransmit(motor->motor_can_instace);
+}
+
+void DMMotorSetRxOnly(DMMotorInstance *motor, uint8_t rx_only)
+{
+    motor->rx_only = rx_only != 0u;
+    if (motor->rx_only)
+        DMMotorStop(motor);
+}
+
 void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
 {
     motor->motor_settings.outer_loop_type = type;
@@ -186,6 +222,7 @@ void DMMotorTask(void const *argument)
         motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
         motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
 
+		if (!motor->rx_only)
 			CANTransmit(motor->motor_can_instace, 1);
 
     	osDelayUntil(&currentTime, 4); // 4ms 的绝对延时

modules/motor/DMmotor/dmmotor.h

@@ -21,6 +21,8 @@ typedef struct
 {
     uint8_t id;
     uint8_t state;
+    volatile uint32_t feedback_count;
+    uint32_t feedback_can_id;
     float velocity;
     float last_position;
     float position;
@@ -55,6 +57,7 @@ typedef struct
     float *current_feedforward_ptr;
     float pid_ref;
     Motor_Working_Type_e stop_flag;
+    uint8_t rx_only; // Blocks enable, calibration, and periodic control frames.
     CANInstance *motor_can_instace;
     DaemonInstance *motor_daemon;
     uint32_t lost_cnt;
@@ -74,6 +77,8 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref);
 void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e closeloop_type);
 void DMMotorEnable(DMMotorInstance *motor);
 void DMMotorStop(DMMotorInstance *motor);
+uint8_t DMMotorSendDisable(DMMotorInstance *motor);
+void DMMotorSetRxOnly(DMMotorInstance *motor, uint8_t rx_only);
 void DMMotorCaliEncoder(DMMotorInstance *motor);
 DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
 void DMMotorControlInit(void);