From 77a3aeb25c22796bef4a593ff18dd8a7311607bc Mon Sep 17 00:00:00 2001 From: SkyLi Date: Sun, 31 May 2026 00:58:45 +0800 Subject: [PATCH] fix: set txconf.Identifier from tx_id before CANTransmit, enable motor task - DM motor send functions now fill txconf.Identifier from tx_id before CANTransmit (MIT=tid, PosVel=tid+0x100, Velocity=tid+0x200, SetMode=tid) - Enable motor task with stack size 1024 1000Hz - Increase FDCAN1 StdFiltersNbr from 1 to 14 - Comment out INS temperature log, DM error handling and PID for testing - Remove test_robot_cmd.c --- Basic_Framework_MC02.ioc | 4 +- Core/Src/fdcan.c | 2 +- application/cmd/test_robot_cmd.c | 140 ------------------------------- application/robot_task.h | 12 +-- bsp/can/bsp_can.c | 2 +- modules/imu/ins_task.c | 3 +- modules/motor/DMmotor/dmmotor.c | 14 ++-- 7 files changed, 18 insertions(+), 159 deletions(-) delete mode 100644 application/cmd/test_robot_cmd.c diff --git a/Basic_Framework_MC02.ioc b/Basic_Framework_MC02.ioc index 38e1019..0af885b 100644 --- a/Basic_Framework_MC02.ioc +++ b/Basic_Framework_MC02.ioc @@ -349,7 +349,7 @@ FDCAN1.NominalTimeSeg2=10 FDCAN1.ProtocolException=ENABLE FDCAN1.RxFifo0ElmtsNbr=4 FDCAN1.RxFifo1ElmtsNbr=4 -FDCAN1.StdFiltersNbr=1 +FDCAN1.StdFiltersNbr=14 FDCAN1.TxFifoQueueElmtsNbr=4 FDCAN2.CalculateBaudRateNominal=1000000 FDCAN2.CalculateTimeBitNominal=1000 @@ -781,7 +781,7 @@ ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,6-MX_TIM12_Init-TIM12-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_SPI2_Init-SPI2-false-HAL-true,9-MX_TIM3_Init-TIM3-false-HAL-true,10-MX_USART1_UART_Init-USART1-false-HAL-true,11-MX_USART2_UART_Init-USART2-false-HAL-true,12-MX_USART3_UART_Init-USART3-false-HAL-true,13-MX_UART7_Init-UART7-false-HAL-true,14-MX_USART10_UART_Init-USART10-false-HAL-true,15-MX_FDCAN1_Init-FDCAN1-false-HAL-true,16-MX_FDCAN2_Init-FDCAN2-false-HAL-true,17-MX_FDCAN3_Init-FDCAN3-false-HAL-true,18-MX_TIM1_Init-TIM1-false-HAL-true,19-MX_TIM2_Init-TIM2-false-HAL-true,20-MX_OCTOSPI2_Init-OCTOSPI2-false-HAL-true,21-MX_UART5_Init-UART5-false-HAL-true,22-MX_UART8_Init-UART8-false-HAL-true,23-MX_UART9_Init-UART9-false-HAL-true,24-MX_I2C2_Init-I2C2-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_TIM12_Init-TIM12-false-HAL-true,6-MX_SPI1_Init-SPI1-false-HAL-true,7-MX_SPI2_Init-SPI2-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_USART1_UART_Init-USART1-false-HAL-true,10-MX_USART2_UART_Init-USART2-false-HAL-true,11-MX_USART3_UART_Init-USART3-false-HAL-true,12-MX_UART7_Init-UART7-false-HAL-true,13-MX_USART10_UART_Init-USART10-false-HAL-true,14-MX_FDCAN1_Init-FDCAN1-false-HAL-true,15-MX_FDCAN2_Init-FDCAN2-false-HAL-true,16-MX_FDCAN3_Init-FDCAN3-false-HAL-true,17-MX_TIM1_Init-TIM1-false-HAL-true,18-MX_TIM2_Init-TIM2-false-HAL-true,19-MX_OCTOSPI2_Init-OCTOSPI2-false-HAL-true,20-MX_UART5_Init-UART5-false-HAL-true,21-MX_UART8_Init-UART8-false-HAL-true,22-MX_UART9_Init-UART9-false-HAL-true,23-MX_I2C2_Init-I2C2-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.ADCFreq_Value=96000000 RCC.AHB12Freq_Value=240000000 RCC.AHB4Freq_Value=240000000 diff --git a/Core/Src/fdcan.c b/Core/Src/fdcan.c index 3a41870..2584d4b 100644 --- a/Core/Src/fdcan.c +++ b/Core/Src/fdcan.c @@ -54,7 +54,7 @@ void MX_FDCAN1_Init(void) hfdcan1.Init.DataTimeSeg1 = 29; hfdcan1.Init.DataTimeSeg2 = 10; hfdcan1.Init.MessageRAMOffset = 0; - hfdcan1.Init.StdFiltersNbr = 1; + hfdcan1.Init.StdFiltersNbr = 14; hfdcan1.Init.ExtFiltersNbr = 0; hfdcan1.Init.RxFifo0ElmtsNbr = 4; hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8; diff --git a/application/cmd/test_robot_cmd.c b/application/cmd/test_robot_cmd.c deleted file mode 100644 index c866bf3..0000000 --- a/application/cmd/test_robot_cmd.c +++ /dev/null @@ -1,140 +0,0 @@ -#include "kalman_filter.h" - -// 添加卡尔曼滤波器实例 -static KalmanFilter_t yaw_error_kf; -static KalmanFilter_t pitch_error_kf; -static float filtered_yaw_error = 0.0f; -static float filtered_pitch_error = 0.0f; - -// 在RobotCMDInit()中添加卡尔曼滤波器初始化 -static void VisionKalmanFilterInit() -{ - // yaw误差卡尔曼滤波器初始化 - Kalman_Filter_Init(&yaw_error_kf, 2, 0, 1); // 状态2维(位置,速度), 控制0维, 观测1维 - - // 减小初始协方差矩阵P，增加收敛速度 - static float P_init_yaw[4] = { - 5.0f, 0.0f, // 减小初始不确定性 - 0.0f, 5.0f - }; - memcpy(yaw_error_kf.P_data, P_init_yaw, sizeof(P_init_yaw)); - - // 状态转移矩阵F (dt = 1/200 = 0.005s, 200Hz控制频率) - static float F_init_yaw[4] = { - 1.0f, 0.005f, // 修正时间步长 - 0.0f, 1.0f - }; - memcpy(yaw_error_kf.F_data, F_init_yaw, sizeof(F_init_yaw)); - - // 增大过程噪声协方差矩阵Q，允许更快的状态变化 - static float Q_init_yaw[4] = { - 0.1f, 0.0f, // 增大位置过程噪声 - 0.0f, 1.0f // 增大速度过程噪声 - }; - memcpy(yaw_error_kf.Q_data, Q_init_yaw, sizeof(Q_init_yaw)); - - // 观测矩阵H - static uint8_t measurement_map_yaw[1] = {1}; // 观测第1个状态(位置) - static float measurement_degree_yaw[1] = {1.0f}; - static float mat_R_diagonal_yaw[1] = {1.0f}; // 减小观测噪声，增加对观测的信任 - static float state_min_variance_yaw[2] = {0.001f, 0.001f}; // 减小最小方差 - - yaw_error_kf.UseAutoAdjustment = 1; - memcpy(yaw_error_kf.MeasurementMap, measurement_map_yaw, sizeof(measurement_map_yaw)); - memcpy(yaw_error_kf.MeasurementDegree, measurement_degree_yaw, sizeof(measurement_degree_yaw)); - memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal_yaw, sizeof(mat_R_diagonal_yaw)); - memcpy(yaw_error_kf.StateMinVariance, state_min_variance_yaw, sizeof(state_min_variance_yaw)); - - // pitch误差卡尔曼滤波器初始化 - 使用相同的优化参数 - Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1); - - static float P_init_pitch[4] = { - 5.0f, 0.0f, - 0.0f, 5.0f - }; - memcpy(pitch_error_kf.P_data, P_init_pitch, sizeof(P_init_pitch)); - - static float F_init_pitch[4] = { - 1.0f, 0.005f, - 0.0f, 1.0f - }; - memcpy(pitch_error_kf.F_data, F_init_pitch, sizeof(F_init_pitch)); - - static float Q_init_pitch[4] = { - 0.1f, 0.0f, - 0.0f, 1.0f - }; - memcpy(pitch_error_kf.Q_data, Q_init_pitch, sizeof(Q_init_pitch)); - - static uint8_t measurement_map_pitch[1] = {1}; - static float measurement_degree_pitch[1] = {1.0f}; - static float mat_R_diagonal_pitch[1] = {1.0f}; - static float state_min_variance_pitch[2] = {0.001f, 0.001f}; - - pitch_error_kf.UseAutoAdjustment = 1; - memcpy(pitch_error_kf.MeasurementMap, measurement_map_pitch, sizeof(measurement_map_pitch)); - memcpy(pitch_error_kf.MeasurementDegree, measurement_degree_pitch, sizeof(measurement_degree_pitch)); - memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal_pitch, sizeof(mat_R_diagonal_pitch)); - memcpy(pitch_error_kf.StateMinVariance, state_min_variance_pitch, sizeof(state_min_variance_pitch)); -} - -// 更新RemoteControlSet()函数中的视觉处理部分 -static void RemoteControlSet() -{ - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - - if (switch_is_up(rc_data[TEMP].rc.switch_left)) - { - if (Vison_revc_flag) - { - // 限制视觉数据范围 - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - - // 更新卡尔曼滤波器观测值 - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - } - else - { - // 无视觉数据时，观测值设为0(表示无效) - yaw_error_kf.MeasuredVector[0] = 0.0f; - pitch_error_kf.MeasuredVector[0] = 0.0f; - } - - // 执行卡尔曼滤波更新 - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - - filtered_yaw_error = yaw_filtered[0]; - filtered_pitch_error = pitch_filtered[0]; - - // 使用滤波后的误差进行PID控制 - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 0); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - - gimbal_cmd_send.yaw += 0.001f * yaw_tracking_pid_output; - gimbal_cmd_send.pitch -= 0.001f * pitch_tracking_pid_output; - - // 调试输出 - vofa_debug[0] = filtered_yaw_error; - vofa_debug[1] = filtered_pitch_error; - vofa_debug[2] = yaw_tracking_pid.Err; - vofa_debug[3] = yaw_tracking_pid.dt; - vofa_debug[4] = yaw_tracking_pid.Iout; - vofa_debug[5] = yaw_tracking_pid.Dout; - vofa_debug[6] = yaw_tracking_pid.Output; - vofa_justfloat_output(vofa_debug, 7, &huart7); - } - else - { - gimbal_cmd_send.yaw += 0.0025f * (float)rc_data[TEMP].rc.rocker_r_; - gimbal_cmd_send.pitch -= 0.00125f * (float)rc_data[TEMP].rc.rocker_r1; - } - - if (gimbal_cmd_send.pitch > 30.0f) - gimbal_cmd_send.pitch = 30.0f; - else if (gimbal_cmd_send.pitch < -30.0f) - gimbal_cmd_send.pitch = -30.0f; -} \ No newline at end of file diff --git a/application/robot_task.h b/application/robot_task.h index d87e5f8..18c08a5 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -32,12 +32,12 @@ void OSTaskInit() }; insTaskHandle = osThreadNew(StartINSTASK, NULL, &insTask_attr); - // const osThreadAttr_t motorTask_attr = { - // .name = "motortask", - // .priority = osPriorityNormal, - // .stack_size = 256, - // }; - // motorTaskHandle = osThreadNew(StartMOTORTASK, NULL, &motorTask_attr); + const osThreadAttr_t motorTask_attr = { + .name = "motortask", + .priority = osPriorityNormal, + .stack_size = 1024, + }; + motorTaskHandle = osThreadNew(StartMOTORTASK, NULL, &motorTask_attr); const osThreadAttr_t daemonTask_attr = { .name = "daemontask", diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index aa68763..3d42dcd 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -195,7 +195,7 @@ CANInstance *CANRegister(CAN_Init_Config_s *config) #endif // 设置回调函数和接收发送id instance->can_handle = config->can_handle; - instance->tx_id = config->tx_id; // 好像没用,可以删掉 + instance->tx_id = config->tx_id; instance->rx_id = config->rx_id; instance->can_module_callback = config->can_module_callback; instance->id = config->id; diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index d460c56..9853c31 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -179,7 +179,8 @@ void INS_Task(void) if ((count % 1000) == 0) { // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线 - LOGINFO("[IMU] Temperature: %d C, PWM: %d", (int)(BMI088.Temperature * 100), (int)TempCtrl.Output); + //LOGINFO("[IMU] Temperature: %d C, PWM: %d", (int)(BMI088.Temperature * 100), (int)TempCtrl.Output); + //LOGINFO("[IMU] Yaw: %d, Pitch: %d, Roll: %d", (int)(INS.Yaw*100), (int)(INS.Pitch*100), (int)(INS.Roll*100)); } count++; diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 6f680a3..c473307 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -46,6 +46,7 @@ void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, buf[6] = (uint8_t)(((kd_uint & 0xF) << 4) | (torq_uint >> 8)); buf[7] = (uint8_t)(torq_uint); + can->txconf.Identifier = can->tx_id; CANTransmit(can, 1); } @@ -65,11 +66,8 @@ void DMMotorSendPosVel(CANInstance *can, float pos, float vel) buf[6] = vel_bytes[2]; buf[7] = vel_bytes[3]; - /* Temporarily change tx_id to motor_id + 0x100 for this transmission */ - uint32_t saved_id = can->tx_id; - can->tx_id = saved_id + 0x100; + can->txconf.Identifier = can->tx_id + 0x100; CANTransmit(can, 1); - can->tx_id = saved_id; } /* Velocity mode: CAN ID = motor_id + 0x200 */ @@ -87,10 +85,8 @@ void DMMotorSendVelocity(CANInstance *can, float vel) buf[6] = 0; buf[7] = 0; - uint32_t saved_id = can->tx_id; - can->tx_id = saved_id + 0x200; + can->txconf.Identifier = can->tx_id + 0x200; CANTransmit(can, 1); - can->tx_id = saved_id; } /* ---- Motor commands (enable/disable/zero/clear error) ---- */ @@ -99,6 +95,7 @@ void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) { memset(motor->motor_can_instance->tx_buff, 0xFF, 7); motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; + motor->motor_can_instance->txconf.Identifier = motor->motor_can_instance->tx_id; CANTransmit(motor->motor_can_instance, 1); } @@ -228,9 +225,10 @@ void DMMotorControl(void) switch (motor->ctrl_mode) { - case DM_MODE_VELOCITY: + case DM_MODE_VELOCITY:{ DMMotorSendVelocity(can, vel_cmd); break; + } case DM_MODE_MIT: { float pos_cmd = pid_ref * DEGREE_2_RAD; LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);