修复bmi088读取零飘错误,为instask增加初始化四元数以防止零飘发散，修复cancomm顺序

HAL_N_Middlewares/Src/can.c

@@ -49,7 +49,7 @@ void MX_CAN1_Init(void)
   hcan1.Init.AutoWakeUp = DISABLE;
   hcan1.Init.AutoRetransmission = DISABLE;
   hcan1.Init.ReceiveFifoLocked = DISABLE;
-  hcan1.Init.TransmitFifoPriority = DISABLE;
+  hcan1.Init.TransmitFifoPriority = ENABLE;
   if (HAL_CAN_Init(&hcan1) != HAL_OK)
   {
     Error_Handler();
@@ -81,7 +81,7 @@ void MX_CAN2_Init(void)
   hcan2.Init.AutoWakeUp = DISABLE;
   hcan2.Init.AutoRetransmission = DISABLE;
   hcan2.Init.ReceiveFifoLocked = DISABLE;
-  hcan2.Init.TransmitFifoPriority = DISABLE;
+  hcan2.Init.TransmitFifoPriority = ENABLE;
   if (HAL_CAN_Init(&hcan2) != HAL_OK)
   {
     Error_Handler();

modules/BMI088/bmi088.c

@@ -261,9 +261,9 @@ BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088)
 
         first_read_flag = 1; // 最后在这里,完成一次读取,标志第一次读取完成
     }                        // 别担心,初始化调用的时候offset(即零飘bias)是0
-    bmi088->gyro[0] = (float)(int16_t)(((buf[1]) << 8) | buf[0]) * gyrosen - bias1 * dt_imu;
+    bmi088->gyro[0] = (float)(int16_t)(((buf[1]) << 8) | buf[0]) * gyrosen - bias1;
-    bmi088->gyro[0] = (float)(int16_t)(((buf[3]) << 8) | buf[2]) * gyrosen - bias2 * dt_imu;
+    bmi088->gyro[0] = (float)(int16_t)(((buf[3]) << 8) | buf[2]) * gyrosen - bias2;
-    bmi088->gyro[0] = (float)(int16_t)(((buf[5]) << 8) | buf[4]) * gyrosen - bias3 * dt_imu;
+    bmi088->gyro[0] = (float)(int16_t)(((buf[5]) << 8) | buf[4]) * gyrosen - bias3;
 
     BMI088AccelRead(bmi088, BMI088_TEMP_M, buf, 2); // 读温度,温度传感器在accel上
     bmi088->temperature = (float)(int16_t)(((buf[0] << 3) | (buf[1] >> 5))) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;

modules/algorithm/QuaternionEKF.c

@@ -47,7 +47,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
  * @param[in] lambda         fading coefficient          0.9996
  * @param[in] lpf            lowpass filter coefficient  0
  */
-void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf)
+void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf)
 {
     QEKF_INS.Initialized = 1;
     QEKF_INS.Q1 = process_noise1;
@@ -69,10 +69,10 @@ void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float me
     Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
 
     // 姿态初始化
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[0] = 1;
+    for(int i = 0; i < 4; i++)
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[1] = 0;
+    {
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[2] = 0;
+        QEKF_INS.IMU_QuaternionEKF.xhat_data[i] = init_quaternion[i];
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[3] = 0;
+    }
 
     // 自定义函数初始化,用于扩展或增加kf的基础功能
     QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
@@ -99,10 +99,6 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
     // 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
     static float halfgxdt, halfgydt, halfgzdt;
     static float accelInvNorm;
-    if (!QEKF_INS.Initialized)
-    {
-        IMU_QuaternionEKF_Init(10, 0.001, 1000000 * 10, 0.9996 * 0 + 1, 0);
-    }
 
     /*   F, number with * represent vals to be set
      0      1*     2*     3*     4     5

modules/algorithm/QuaternionEKF.h

@@ -69,7 +69,7 @@ typedef struct
 extern QEKF_INS_t QEKF_INS;
 extern float chiSquare;
 extern float ChiSquareTestThreshold;
-void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
+void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
 void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
 
 #endif

modules/algorithm/user_lib.c

@@ -163,3 +163,27 @@ int float_rounding(float raw)
         integer++;
     return integer;
 }
+
+// 三维向量归一化
+float* Norm3d(float* v)
+{
+    float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+    v[0] /= len;
+    v[1] /= len;
+    v[2] /= len;
+    return v;
+}
+
+// 三维向量叉乘v1 x v2
+void Cross3d(float* v1, float* v2,float* res)
+{
+    res[0] = v1[1] * v2[2] - v1[2] * v2[1];
+    res[1] = v1[2] * v2[0] - v1[0] * v2[2];
+    res[2] = v1[0] * v2[1] - v1[1] * v2[0];
+}
+
+// 三维向量点乘
+float Dot3d(float* v1, float* v2)
+{
+    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+}

modules/algorithm/user_lib.h

@@ -88,7 +88,7 @@ extern uint8_t GlobalDebugMode;
 #define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
 
 /**
- * @brief 返回一块干净的内存,不过仍然需要强制转换为你需要的类型
+ * @brief 返回一å<EFBFBD>—干净的内å­?,ä¸<EFBFBD>过ä»<EFBFBD>然需è¦<EFBFBD>强制转æ<EFBFBD>?为你需è¦<EFBFBD>的类型
  * 
  * @param size 分配大小
  * @return void* 
@@ -114,6 +114,12 @@ float theta_format(float Ang);
 
 int float_rounding(float raw);
 
+float* Norm3d(float* v);
+
+void Cross3d(float* v1, float* v2, float* res);
+
+float Dot3d(float* v1, float* v2);
+
 //<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
 #define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
 

modules/imu/BMI088driver.c

@@ -347,11 +347,11 @@ void BMI088_Read(IMU_Data_t *bmi088)
         if (caliOffset)
         {
             bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
-            bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[0] * dt;
+            bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[0];
             bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
-            bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1] * dt;
+            bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1];
             bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
-            bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2] * dt;
+            bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2];
         }
         else
         {

modules/imu/ins_task.c

@@ -43,6 +43,33 @@ static void IMU_Temperature_Ctrl(void)
     IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
 }
 
+// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
+static void InitQuaternion(float* init_q4)
+{
+    float acc_init[3] = {0};
+    float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
+    float axis_rot[3] = {0};           // 旋转轴
+    // 读取100次加速度计数据,取平均值作为初始值
+    for (uint8_t i = 0; i < 100; ++i)
+    {
+        BMI088_Read(&BMI088);
+        acc_init[X] += BMI088.Accel[X];
+        acc_init[Y] += BMI088.Accel[Y];
+        acc_init[Z] += BMI088.Accel[Z];
+        DWT_Delay(0.001);
+    }
+    for (uint8_t i = 0; i < 3; ++i)
+        acc_init[i] /= 100;
+    Norm3d(acc_init);
+    // 计算原始加速度矢量和导航系重力加速度矢量的夹角
+    float angle = acosf(Dot3d(acc_init, gravity_norm));
+    Cross3d(acc_init, gravity_norm, axis_rot);
+    Norm3d(axis_rot);
+    init_q4[0] = cosf(angle / 2.0f);
+    for(uint8_t i = 0; i < 2; ++i)
+        init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
+}
+
 attitude_t *INS_Init(void)
 {
     while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
@@ -55,7 +82,9 @@ attitude_t *INS_Init(void)
     IMU_Param.Roll = 0;
     IMU_Param.flag = 1;
 
-    IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
+    float init_quaternion[4] = {0};
+    InitQuaternion(init_quaternion);
+    IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
     // imu heat init
     PID_Init_Config_s config = {.MaxOut = 2000,
                                 .IntegralLimit = 300,
@@ -68,6 +97,7 @@ attitude_t *INS_Init(void)
 
     // noise of accel is relatively big and of high freq,thus lpf is used
     INS.AccelLPF = 0.0085;
+    DWT_GetDeltaT64(&INS_DWT_Count);
     return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
 }