fix can rx fifo bug

2026-07-24 03:27:45 +08:00 · 2022-11-23 22:10:44 +08:00
parent 62b1783b59
commit 490b957045
11 changed files with 123 additions and 73 deletions
--- a/modules/algorithm/QuaternionEKF.c
+++ b/modules/algorithm/QuaternionEKF.c
@@ -11,6 +11,7 @@
 *     1
 *  ———————
 *  as + 1
+ * 
 ******************************************************************************
 */
 #include "QuaternionEKF.h"
--- 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;
--- 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

-// <20><><EFBFBD>ֶ<EFBFBD><D6B6>޸<EFBFBD>
+// <20><><EFBFBD>ֶ<EFBFBD><D6B6>޸<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
--- 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++;
 }

 /**
--- 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;
--- a/modules/imu/ins_task.md
+++ b/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)
--- 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;