修复bmi088读取零飘错误,为instask增加初始化四元数以防止零飘发散,修复cancomm顺序
This commit is contained in:
parent
95fca40700
commit
6523bbd6a9
|
@ -49,7 +49,7 @@ void MX_CAN1_Init(void)
|
||||||
hcan1.Init.AutoWakeUp = DISABLE;
|
hcan1.Init.AutoWakeUp = DISABLE;
|
||||||
hcan1.Init.AutoRetransmission = DISABLE;
|
hcan1.Init.AutoRetransmission = DISABLE;
|
||||||
hcan1.Init.ReceiveFifoLocked = DISABLE;
|
hcan1.Init.ReceiveFifoLocked = DISABLE;
|
||||||
hcan1.Init.TransmitFifoPriority = DISABLE;
|
hcan1.Init.TransmitFifoPriority = ENABLE;
|
||||||
if (HAL_CAN_Init(&hcan1) != HAL_OK)
|
if (HAL_CAN_Init(&hcan1) != HAL_OK)
|
||||||
{
|
{
|
||||||
Error_Handler();
|
Error_Handler();
|
||||||
|
@ -81,7 +81,7 @@ void MX_CAN2_Init(void)
|
||||||
hcan2.Init.AutoWakeUp = DISABLE;
|
hcan2.Init.AutoWakeUp = DISABLE;
|
||||||
hcan2.Init.AutoRetransmission = DISABLE;
|
hcan2.Init.AutoRetransmission = DISABLE;
|
||||||
hcan2.Init.ReceiveFifoLocked = DISABLE;
|
hcan2.Init.ReceiveFifoLocked = DISABLE;
|
||||||
hcan2.Init.TransmitFifoPriority = DISABLE;
|
hcan2.Init.TransmitFifoPriority = ENABLE;
|
||||||
if (HAL_CAN_Init(&hcan2) != HAL_OK)
|
if (HAL_CAN_Init(&hcan2) != HAL_OK)
|
||||||
{
|
{
|
||||||
Error_Handler();
|
Error_Handler();
|
||||||
|
|
|
@ -261,9 +261,9 @@ BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088)
|
||||||
|
|
||||||
first_read_flag = 1; // 最后在这里,完成一次读取,标志第一次读取完成
|
first_read_flag = 1; // 最后在这里,完成一次读取,标志第一次读取完成
|
||||||
} // 别担心,初始化调用的时候offset(即零飘bias)是0
|
} // 别担心,初始化调用的时候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上
|
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;
|
bmi088->temperature = (float)(int16_t)(((buf[0] << 3) | (buf[1] >> 5))) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
|
||||||
|
|
|
@ -47,7 +47,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
|
||||||
* @param[in] lambda fading coefficient 0.9996
|
* @param[in] lambda fading coefficient 0.9996
|
||||||
* @param[in] lpf lowpass filter coefficient 0
|
* @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.Initialized = 1;
|
||||||
QEKF_INS.Q1 = process_noise1;
|
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);
|
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的基础功能
|
// 自定义函数初始化,用于扩展或增加kf的基础功能
|
||||||
QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
|
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矩阵
|
// 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
|
||||||
static float halfgxdt, halfgydt, halfgzdt;
|
static float halfgxdt, halfgydt, halfgzdt;
|
||||||
static float accelInvNorm;
|
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
|
/* F, number with * represent vals to be set
|
||||||
0 1* 2* 3* 4 5
|
0 1* 2* 3* 4 5
|
||||||
|
|
|
@ -69,7 +69,7 @@ typedef struct
|
||||||
extern QEKF_INS_t QEKF_INS;
|
extern QEKF_INS_t QEKF_INS;
|
||||||
extern float chiSquare;
|
extern float chiSquare;
|
||||||
extern float ChiSquareTestThreshold;
|
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);
|
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -162,4 +162,28 @@ int float_rounding(float raw)
|
||||||
if (decimal > 0.5f)
|
if (decimal > 0.5f)
|
||||||
integer++;
|
integer++;
|
||||||
return 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];
|
||||||
}
|
}
|
|
@ -88,7 +88,7 @@ extern uint8_t GlobalDebugMode;
|
||||||
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
|
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 返回一块干净的内存,不过仍然需要强制转换为你需要的类型
|
* @brief 返回一å<EFBFBD>—干净的内å?,ä¸<EFBFBD>过ä»<EFBFBD>然需è¦<EFBFBD>强制转æ<EFBFBD>?ä¸ºä½ éœ€è¦<EFBFBD>的类型
|
||||||
*
|
*
|
||||||
* @param size 分配大小
|
* @param size 分配大小
|
||||||
* @return void*
|
* @return void*
|
||||||
|
@ -114,6 +114,12 @@ float theta_format(float Ang);
|
||||||
|
|
||||||
int float_rounding(float raw);
|
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
|
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
|
||||||
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
|
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
|
||||||
|
|
||||||
|
|
|
@ -347,11 +347,11 @@ void BMI088_Read(IMU_Data_t *bmi088)
|
||||||
if (caliOffset)
|
if (caliOffset)
|
||||||
{
|
{
|
||||||
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
|
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_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_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
|
else
|
||||||
{
|
{
|
||||||
|
|
|
@ -43,6 +43,33 @@ static void IMU_Temperature_Ctrl(void)
|
||||||
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
|
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)
|
attitude_t *INS_Init(void)
|
||||||
{
|
{
|
||||||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
|
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
|
||||||
|
@ -55,19 +82,22 @@ attitude_t *INS_Init(void)
|
||||||
IMU_Param.Roll = 0;
|
IMU_Param.Roll = 0;
|
||||||
IMU_Param.flag = 1;
|
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
|
// imu heat init
|
||||||
PID_Init_Config_s config = {.MaxOut = 2000,
|
PID_Init_Config_s config = {.MaxOut = 2000,
|
||||||
.IntegralLimit = 300,
|
.IntegralLimit = 300,
|
||||||
.DeadBand = 0,
|
.DeadBand = 0,
|
||||||
.Kp = 1000,
|
.Kp = 1000,
|
||||||
.Ki = 20,
|
.Ki = 20,
|
||||||
.Kd = 0,
|
.Kd = 0,
|
||||||
.Improve = 0x01}; // enable integratiaon limit
|
.Improve = 0x01}; // enable integratiaon limit
|
||||||
PIDInit(&TempCtrl, &config);
|
PIDInit(&TempCtrl, &config);
|
||||||
|
|
||||||
// noise of accel is relatively big and of high freq,thus lpf is used
|
// noise of accel is relatively big and of high freq,thus lpf is used
|
||||||
INS.AccelLPF = 0.0085;
|
INS.AccelLPF = 0.0085;
|
||||||
|
DWT_GetDeltaT64(&INS_DWT_Count);
|
||||||
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue