HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

This commit is contained in:
NeoZng 2023-04-13 10:31:06 +08:00
parent 95fca40700
commit 6523bbd6a9
8 changed files with 82 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
HAL_N_Middlewares/Src/can.c
View File

@ -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();

6
modules/BMI088/bmi088.c
View File

@ -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;

14
modules/algorithm/QuaternionEKF.c
View File

@ -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

2
modules/algorithm/QuaternionEKF.h
View File

@ -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

24
modules/algorithm/user_lib.c
View File

@ -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];
}

8
modules/algorithm/user_lib.h
View File

@ -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)

6
modules/imu/BMI088driver.c
View File

@ -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
{

44
modules/imu/ins_task.c
View File

@ -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结构体可能会导致异常,待修复.
}