TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 19:47:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

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>?为你需要的类型
*
* @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)