AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: set txconf.Identifier from tx_id before CANTransmit, enable motor task

Browse Source 
- 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
This commit is contained in:
李傲琦
2026-05-31 00:58:45 +08:00
parent 5e662f8458
commit 77a3aeb25c
7 changed files with 18 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Basic_Framework_MC02.ioc
+2 -2
View File
@@ -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
Core/Src/fdcan.c
+1 -1
View File
@@ -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;
application/cmd/test_robot_cmd.c
-140
View File
@@ -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;
}
application/robot_task.h
+6 -6
View File
@@ -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",
bsp/can/bsp_can.c
+1 -1
View File
@@ -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;
modules/imu/ins_task.c
+2 -1
View File
@@ -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++;
modules/motor/DMmotor/dmmotor.c
+6 -8
View File
@@ -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);