关节电机measure

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);
-    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
-    CANTransmit(motor->motor_can_instace, 1);
-	DWT_Delay(DM_SEND_DELAY);
+    if (motor->rx_only && cmd != DM_CMD_RESET_MODE)
+        return;
+
+    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,7 +222,8 @@ 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);
 
-		CANTransmit(motor->motor_can_instace, 1);
+		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,8 +77,10 @@ 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);
 void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
-#endif // !DMMOTOR
+#endif // !DMMOTOR