diff --git a/HAL_N_Middlewares/Src/can.c b/HAL_N_Middlewares/Src/can.c index 292573f..fe60482 100644 --- a/HAL_N_Middlewares/Src/can.c +++ b/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(); diff --git a/modules/BMI088/bmi088.c b/modules/BMI088/bmi088.c index 433734f..91ab6c9 100644 --- a/modules/BMI088/bmi088.c +++ b/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[3]) << 8) | buf[2]) * gyrosen - bias2 * dt_imu; - bmi088->gyro[0] = (float)(int16_t)(((buf[5]) << 8) | buf[4]) * gyrosen - bias3 * 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; + 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; diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 5513486..27c06e5 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/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; - QEKF_INS.IMU_QuaternionEKF.xhat_data[1] = 0; - QEKF_INS.IMU_QuaternionEKF.xhat_data[2] = 0; - QEKF_INS.IMU_QuaternionEKF.xhat_data[3] = 0; + for(int i = 0; i < 4; i++) + { + QEKF_INS.IMU_QuaternionEKF.xhat_data[i] = init_quaternion[i]; + } // 自定义函数初始化,用于扩展或增加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 diff --git a/modules/algorithm/QuaternionEKF.h b/modules/algorithm/QuaternionEKF.h index f7b4d7e..26ab0a9 100644 --- a/modules/algorithm/QuaternionEKF.h +++ b/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 diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index 78d451a..5fe5042 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -162,4 +162,28 @@ int float_rounding(float raw) if (decimal > 0.5f) 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]; } \ No newline at end of file diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 6e76eac..0197d2e 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -88,7 +88,7 @@ extern uint8_t GlobalDebugMode; #define VAL_MAX(a, b) ((a) > (b) ? (a) : (b)) /** - * @brief 返回一块干净的内存,不过仍然需要强制转换为你需要的类型 + * @brief 返回一块干净的内?,不过仍然需要强制转?为你需要的类型 * * @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); + //���ȸ�ʽ��Ϊ-PI~PI #define rad_format(Ang) loop_float_constrain((Ang), -PI, PI) diff --git a/modules/imu/BMI088driver.c b/modules/imu/BMI088driver.c index c4d148e..4772204 100644 --- a/modules/imu/BMI088driver.c +++ b/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 { diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index a64a78b..8fd18d7 100644 --- a/modules/imu/ins_task.c +++ b/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,19 +82,22 @@ 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, - .DeadBand = 0, - .Kp = 1000, - .Ki = 20, - .Kd = 0, - .Improve = 0x01}; // enable integratiaon limit + .IntegralLimit = 300, + .DeadBand = 0, + .Kp = 1000, + .Ki = 20, + .Kd = 0, + .Improve = 0x01}; // enable integratiaon limit PIDInit(&TempCtrl, &config); // 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结构体可能会导致异常,待修复. }