fix can rx fifo bug

.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",
 }

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 */

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},
+			.tx_id = 2},
-		.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_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 = {.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}}};
+		.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 */

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);
 }
 
 /**

modules/algorithm/QuaternionEKF.c

@@ -11,6 +11,7 @@
  *     1
  *  ———————
  *  as + 1
+ * 
  ******************************************************************************
  */
 #include "QuaternionEKF.h"

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_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_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_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_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_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_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_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_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_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
-    {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_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_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_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;

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
 
-// ÐèÊÖ¶¯ÐÞ¸Ä
+// <EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD><EFBFBD>޸<EFBFBD>
 #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

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++;
 }
 
 /**

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;

modules/imu/ins_task.md

@@ -4,4 +4,7 @@
 
 ## 硬触发流程
 
-![image-20221113212706633](assets\image-20221113212706633.png)
+![image-20221113212706633](assets\image-20221113212706633.png)
+
+## 算法解析
+介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643)

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;