From 490b9570457605b5fb8fc13344a091ecbb9f65d4 Mon Sep 17 00:00:00 2001 From: NeoZeng Date: Wed, 23 Nov 2022 22:10:44 +0800 Subject: [PATCH] fix can rx fifo bug --- .vscode/settings.json | 5 +++- HAL_N_Middlewares/Src/freertos.c | 31 ++++++++++++++++++- HAL_N_Middlewares/Src/main.c | 13 ++++---- bsp/bsp_can.c | 19 +++++++++++- modules/algorithm/QuaternionEKF.c | 1 + modules/imu/BMI088driver.c | 42 ++++++++++---------------- modules/imu/BMI088driver.h | 50 +++++++++++++++++++------------ modules/imu/ins_task.c | 15 +++++----- modules/imu/ins_task.h | 13 ++++---- modules/imu/ins_task.md | 5 +++- modules/motor/dji_motor.c | 2 +- 11 files changed, 123 insertions(+), 73 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e7f09d4..944a8bb 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,7 +4,10 @@ "stdint-gcc.h": "c", "led_task.h": "c", "stdinf.h": "c", - "usart.h": "c" + "usart.h": "c", + "controller.h": "c", + "ins_task.h": "c", + "task.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index 6d65919..bf3a360 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -25,7 +25,8 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ - +#include "ins_task.h" +#include "motor_task.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -51,6 +52,9 @@ osThreadId defaultTaskHandle; /* Private function prototypes -----------------------------------------------*/ /* USER CODE BEGIN FunctionPrototypes */ +void StartINSTASK(void const * argument); + +void StartMOTORTASK(void const * argument); /* USER CODE END FunctionPrototypes */ @@ -106,6 +110,12 @@ void MX_FREERTOS_Init(void) { osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128); defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); + osThreadDef(instask, StartINSTASK, osPriorityNormal, 0, 1024); + defaultTaskHandle = osThreadCreate(osThread(instask), NULL); + + osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256); + defaultTaskHandle = osThreadCreate(osThread(motortask), NULL); + /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ /* USER CODE END RTOS_THREADS */ @@ -134,5 +144,24 @@ void StartDefaultTask(void const * argument) /* Private application code --------------------------------------------------*/ /* USER CODE BEGIN Application */ +void StartINSTASK(void const * argument) +{ + while (1) + { + INS_Task(); + osDelay(1); + } + +} + +void StartMOTORTASK(void const * argument) +{ + while (1) + { + MotorControlTask(); + osDelay(1); + } +} + /* USER CODE END Application */ diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index f98c2b0..d29b583 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -77,8 +77,7 @@ void MX_FREERTOS_Init(void); * @brief The application entry point. * @retval int */ -int main(void) -{ +int main(void){ /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ @@ -119,12 +118,12 @@ int main(void) RC_init(&huart3); DWT_Init(168); Motor_Init_Config_s config = { - .motor_type = M2006, + .motor_type = M3508, .can_init_config = { .can_handle = &hcan1, - .tx_id = 1}, - .controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL}, - .controller_param_init_config = {.current_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}}; + .tx_id = 2}, + .controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL}, + .controller_param_init_config = {.angle_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}}; dji_motor_instance *djimotor = DJIMotorInit(config); RefereeInit(&huart6); @@ -143,7 +142,7 @@ int main(void) { /* USER CODE END WHILE */ - DJIMotorSetRef(djimotor, get_remote_control_point()->rc.ch[0]); + DJIMotorSetRef(djimotor, 10); MotorControlTask(); HAL_Delay(100); /* USER CODE BEGIN 3 */ diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 104c075..6e06ceb 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -29,13 +29,30 @@ static void CANAddFilter(can_instance *_instance) can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; - can_filter_conf.FilterFIFOAssignment = (_instance->rx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; + can_filter_conf.FilterFIFOAssignment = CAN_RX_FIFO1; can_filter_conf.SlaveStartFilterBank = 14; can_filter_conf.FilterIdLow = _instance->rx_id << 5; + can_filter_conf.FilterIdHigh = _instance->rx_id << 5; + can_filter_conf.FilterMaskIdLow = _instance->rx_id << 5; + can_filter_conf.FilterMaskIdHigh = _instance->rx_id << 5; can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf); + + // CAN_FilterTypeDef can_filter_st; + // can_filter_st.FilterActivation = ENABLE; + // can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK; + // can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT; + // can_filter_st.FilterIdHigh = 0x0000; + // can_filter_st.FilterIdLow = 0x0000; + // can_filter_st.FilterMaskIdHigh = 0x0000; + // can_filter_st.FilterMaskIdLow = 0x0000; + // can_filter_st.FilterBank = 0; + // can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0; + // HAL_CAN_ConfigFilter(&hcan1, &can_filter_st); + // HAL_CAN_Start(&hcan1); + // HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); } /** diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 6c69918..08a33df 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/modules/algorithm/QuaternionEKF.c @@ -11,6 +11,7 @@ * 1 * ——————— * as + 1 + * ****************************************************************************** */ #include "QuaternionEKF.h" diff --git a/modules/imu/BMI088driver.c b/modules/imu/BMI088driver.c index e2139dd..4a008d2 100644 --- a/modules/imu/BMI088driver.c +++ b/modules/imu/BMI088driver.c @@ -1,15 +1,3 @@ -/** - ****************************************************************************** - * @file BMI088driver.c - * @author - * @version V1.2.0 - * @date 2022/3/8 - * @brief - ****************************************************************************** - * @attention - * - ****************************************************************************** - */ #include "BMI088driver.h" #include "BMI088reg.h" #include "BMI088Middleware.h" @@ -82,30 +70,30 @@ static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len); #endif static uint8_t write_BMI088_accel_reg_data_error[BMI088_WRITE_ACCEL_REG_NUM][3] = -{ - {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR}, - {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR}, - {BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR}, - {BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR}, - {BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR}, - {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR} + { + {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR}, + {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR}, + {BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR}, + {BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR}, + {BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR}, + {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR} }; static uint8_t write_BMI088_gyro_reg_data_error[BMI088_WRITE_GYRO_REG_NUM][3] = -{ - {BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR}, - {BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR}, - {BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR}, - {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR}, - {BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR}, - {BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR} + { + {BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR}, + {BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR}, + {BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR}, + {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR}, + {BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR}, + {BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR} }; static void Calibrate_MPU_Offset(IMU_Data_t *bmi088); -uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate) +uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate) { BMI088_SPI = bmi088_SPI; error = BMI088_NO_ERROR; diff --git a/modules/imu/BMI088driver.h b/modules/imu/BMI088driver.h index 43ece02..2fd30fb 100644 --- a/modules/imu/BMI088driver.h +++ b/modules/imu/BMI088driver.h @@ -1,16 +1,3 @@ -/** - ****************************************************************************** - * @file BMI088driver.h - * @author - * @version V1.1.2 - * @version V1.2.0 - * @date 2022/3/8 - * @brief - ****************************************************************************** - * @attention - * - ****************************************************************************** - */ #ifndef BMI088DRIVER_H #define BMI088DRIVER_H @@ -44,7 +31,7 @@ #define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f #define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f -// ֶ޸ +// ���ֶ��޸� #if INFANTRY_ID == 0 #define GxOFFSET 0.00247530174f #define GyOFFSET 0.000393082853f @@ -72,6 +59,7 @@ #define gNORM 9.876785f #endif +/* IMU数据结构体 */ typedef struct { float Accel[3]; @@ -87,6 +75,7 @@ typedef struct float gNorm; } IMU_Data_t; +/* BMI088错误码枚举 */ enum { BMI088_NO_ERROR = 0x00, @@ -109,13 +98,36 @@ enum BMI088_NO_SENSOR = 0xFF, }; -void BMI088_Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate); -extern uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate); -extern uint8_t bmi088_accel_init(void); -extern uint8_t bmi088_gyro_init(void); - extern IMU_Data_t BMI088; +/** + * @brief 初始化BMI088,传入连接的SPI总线handle,以及是否进行在线标定 + * + * @param bmi088_SPI handle + * @param calibrate 1为进行在线标定,0使用离线数据 + * @return uint8_t 成功则返回BMI088_NO_ERROR + */ +extern uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate); + +/** + * @brief 加速计初始化 + * + * @return uint8_t + */ +extern uint8_t bmi088_accel_init(void); + +/** + * @brief 陀螺仪初始化 + * + * @return uint8_t + */ +extern uint8_t bmi088_gyro_init(void); + +/** + * @brief 读取一次BMI088的数据,包括gyro和accel + * + * @param bmi088 传入BMI088实例(结构体) + */ extern void BMI088_Read(IMU_Data_t *bmi088); #endif diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 8134f7b..7aed32f 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -26,13 +26,14 @@ const float xb[3] = {1, 0, 0}; const float yb[3] = {0, 1, 0}; const float zb[3] = {0, 0, 1}; +// 用于获取两次采样之间的时间间隔 uint32_t INS_DWT_Count = 0; static float dt = 0, t = 0; -uint8_t ins_debug_mode = 0; -float RefTemp = 40; +float RefTemp = 40; // 恒温设定温度 static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]); +/* 调用此函数获得姿态数据指针 */ attitude_t *GetINSptr() { return (attitude_t *)&INS.Roll; @@ -45,13 +46,12 @@ attitude_t *GetINSptr() static void IMU_Temperature_Ctrl(void) { PID_Calculate(&TempCtrl, BMI088.Temperature, RefTemp); - imu_pwm_set(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX)); } void INS_Init(void) { - // while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR); + while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR); IMU_Param.scale[X] = 1; IMU_Param.scale[Y] = 1; IMU_Param.scale[Z] = 1; @@ -75,6 +75,7 @@ void INS_Init(void) INS.AccelLPF = 0.0085; } +/* 注意以1kHz的频率运行此任务 */ void INS_Task(void) { static uint32_t count = 0; @@ -134,12 +135,10 @@ void INS_Task(void) IMU_Temperature_Ctrl(); } - if ((count % 1000) == 0) + if ((count++ % 1000) == 0) { - // 200hz + // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线 } - - count++; } /** diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index 71d7779..56ab9b2 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -2,12 +2,14 @@ ****************************************************************************** * @file ins_task.h * @author Wang Hongxi + * @author annotation and modification by NeoZeng * @version V2.0.0 * @date 2022/2/23 * @brief ****************************************************************************** - * @attention - * + * @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在 + * 应用层调用初始化函数. + * ****************************************************************************** */ #ifndef __INS_TASK_H @@ -29,7 +31,7 @@ typedef struct float Pitch; float Yaw; float YawTotalAngle; -} attitude_t; +} attitude_t; //最终解算得到的角度,以及yaw转动的总角度(方便多圈控制) typedef struct { @@ -57,10 +59,7 @@ typedef struct float YawTotalAngle; } INS_t; -/** - * @brief 用于修正安装误差的参数 - * - */ +/* 用于修正安装误差的参数 */ typedef struct { uint8_t flag; diff --git a/modules/imu/ins_task.md b/modules/imu/ins_task.md index b68bc99..4eb801a 100644 --- a/modules/imu/ins_task.md +++ b/modules/imu/ins_task.md @@ -4,4 +4,7 @@ ## 硬触发流程 -![image-20221113212706633](assets\image-20221113212706633.png) \ No newline at end of file +![image-20221113212706633](assets\image-20221113212706633.png) + +## 算法解析 +介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643) diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 1e04fb8..77c6339 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -242,7 +242,7 @@ void DJIMotorControl() group = motor->sender_group; num = motor->message_num; sender_assignment[group].tx_buff[num] = 0xff & set >> 8; - sender_assignment[group].tx_buff[num + 1] = 0xff & set; + sender_assignment[group].tx_buff[2*num + 1] = 0xff & set; } else // 遇到空指针说明所有遍历结束,退出循环 break;