diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 611276b..86bd6d4 100644 --- a/application/chassis/balance.c +++ b/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.rx_id = 14; + joint_conf.can_init_config.tx_id = JOINT_LF_MOTOR_ID; + 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.rx_id = 13; + joint_conf.can_init_config.tx_id = JOINT_LB_MOTOR_ID; + 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.rx_id = 12; + joint_conf.can_init_config.tx_id = JOINT_RF_MOTOR_ID; + 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.rx_id = 11; + joint_conf.can_init_config.tx_id = JOINT_RB_MOTOR_ID; + 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 || diff --git a/application/chassis/balance.h b/application/chassis/balance.h index 698c675..9abcbd5 100644 --- a/application/chassis/balance.h +++ b/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 diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 43591b6..c941786 100644 --- a/bsp/can/bsp_can.c +++ b/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; } } } diff --git a/bsp/can/bsp_can.h b/bsp/can/bsp_can.h index 62b7f5c..24fef43 100644 --- a/bsp/can/bsp_can.h +++ b/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 diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 11d104f..06889a4 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/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(¤tTime, 4); // 4ms 的绝对延时 } diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index d4e77a9..fe28840 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/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 \ No newline at end of file +#endif // !DMMOTOR