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

init commit

Browse Source
This commit is contained in:
NeoZng
2022-10-20 17:13:02 +08:00
commit 78f85b7fda
518 changed files with 409143 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

492
modules/algorithm/QuaternionEKF.c Normal file
View File

@@ -0,0 +1,492 @@
/**
******************************************************************************
* @file QuaternionEKF.c
* @author Wang Hongxi
* @version V1.2.0
* @date 2022/3/8
* @brief attitude update with gyro bias estimate and chi-square test
******************************************************************************
* @attention
* 1st order LPF transfer function:
* 1
* ———————
* as + 1
******************************************************************************
*/
#include "QuaternionEKF.h"
QEKF_INS_t QEKF_INS;
const float IMU_QuaternionEKF_F[36] = {1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1};
float IMU_QuaternionEKF_P[36] = {100000, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 100000, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 100000, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 100000, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 100, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 100};
float IMU_QuaternionEKF_K[18];
float IMU_QuaternionEKF_H[18];
static float invSqrt(float x);
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
/**
* @brief Quaternion EKF initialization and some reference value
* @param[in] process_noise1 quaternion process noise 10
* @param[in] process_noise2 gyro bias process noise 0.001
* @param[in] measure_noise accel measure noise 1000000
* @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)
{
QEKF_INS.Initialized = 1;
QEKF_INS.Q1 = process_noise1;
QEKF_INS.Q2 = process_noise2;
QEKF_INS.R = measure_noise;
QEKF_INS.ChiSquareTestThreshold = 1e-8;
QEKF_INS.ConvergeFlag = 0;
QEKF_INS.ErrorCount = 0;
QEKF_INS.UpdateCount = 0;
if (lambda > 1)
{
lambda = 1;
}
QEKF_INS.lambda = lambda;
QEKF_INS.accLPFcoef = lpf;
// 初始化矩阵维度信息
Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3);
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;
// 自定义函数初始化,用于扩展或增加kf的基础功能
QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading;
QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH;
QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate;
// 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合)
QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE;
QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE;
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P));
}
/**
* @brief Quaternion EKF update
* @param[in] gyro x y z in rad/s
* @param[in] accel x y z in m/s²
* @param[in] update period in s
*/
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt)
{
// 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
6* 7 8* 9* 10 11
12* 13* 14 15* 16 17
18* 19* 20* 21 22 23
24 25 26 27 28 29
30 31 32 33 34 35
*/
QEKF_INS.dt = dt;
QEKF_INS.Gyro[0] = gx - QEKF_INS.GyroBias[0];
QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];
// set F
halfgxdt = 0.5f * QEKF_INS.Gyro[0] * dt;
halfgydt = 0.5f * QEKF_INS.Gyro[1] * dt;
halfgzdt = 0.5f * QEKF_INS.Gyro[2] * dt;
// 此部分设定状态转移矩阵F的左上角部分 4x4子矩阵,即0.5(Ohm-Ohm^bias)*deltaT,右下角有一个2x2单位阵已经初始化好了
// 注意在predict步F的右上角是4x2的零矩阵,因此每次predict的时候都会调用memcpy用单位阵覆盖前一轮线性化后的矩阵
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
{
QEKF_INS.Accel[0] = ax;
QEKF_INS.Accel[1] = ay;
QEKF_INS.Accel[2] = az;
}
QEKF_INS.Accel[0] = QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
// set z,单位化重力加速度向量
accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]);
for (uint8_t i = 0; i < 3; i++)
{
QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
}
// get body state
QEKF_INS.gyro_norm = 1.0f / invSqrt(QEKF_INS.Gyro[0] * QEKF_INS.Gyro[0] +
QEKF_INS.Gyro[1] * QEKF_INS.Gyro[1] +
QEKF_INS.Gyro[2] * QEKF_INS.Gyro[2]);
QEKF_INS.accl_norm = 1.0f / accelInvNorm;
// 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
// 稍后在最后的姿态更新部分会利用StableFlag来确定
if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f && QEKF_INS.accl_norm < 9.8f + 0.5f)
{
QEKF_INS.StableFlag = 1;
}
else
{
QEKF_INS.StableFlag = 0;
}
// set Q R,过程噪声和观测噪声矩阵
QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R;
QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;
// 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);
// 获取融合后的数据,包括四元数和xy零飘值
QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];
QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移
// 利用四元数反解欧拉角
QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
{
QEKF_INS.YawRoundCount--;
}
else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f)
{
QEKF_INS.YawRoundCount++;
}
QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
}
/**
* @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新;
* 并对零漂的方差进行限制,防止过度收敛并限幅防止发散
*
* @param kf
*/
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
{
static float q0, q1, q2, q3;
static float qInvNorm;
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
// quaternion normalize
qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
for (uint8_t i = 0; i < 4; i++)
{
kf->xhatminus_data[i] *= qInvNorm;
}
/* F, number with * represent vals to be set
0 1 2 3 4* 5*
6 7 8 9 10* 11*
12 13 14 15 16* 17*
18 19 20 21 22* 23*
24 25 26 27 28 29
30 31 32 33 34 35
*/
// set F
kf->F_data[4] = q1 * QEKF_INS.dt / 2;
kf->F_data[5] = q2 * QEKF_INS.dt / 2;
kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
kf->F_data[11] = q3 * QEKF_INS.dt / 2;
kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
kf->F_data[22] = q2 * QEKF_INS.dt / 2;
kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
// fading filter,防止零飘参数过度收敛
kf->P_data[28] /= QEKF_INS.lambda;
kf->P_data[35] /= QEKF_INS.lambda;
// 限幅,防止发散
if (kf->P_data[28] > 10000)
{
kf->P_data[28] = 10000;
}
if (kf->P_data[35] > 10000)
{
kf->P_data[35] = 10000;
}
}
/**
* @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
*
* @param kf
*/
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf)
{
static float doubleq0, doubleq1, doubleq2, doubleq3;
/* H
0 1 2 3 4 5
6 7 8 9 10 11
12 13 14 15 16 17
last two cols are zero
*/
// set H
doubleq0 = 2 * kf->xhatminus_data[0];
doubleq1 = 2 * kf->xhatminus_data[1];
doubleq2 = 2 * kf->xhatminus_data[2];
doubleq3 = 2 * kf->xhatminus_data[3];
memset(kf->H_data, 0, sizeof_float * kf->zSize * kf->xhatSize);
kf->H_data[0] = -doubleq2;
kf->H_data[1] = doubleq3;
kf->H_data[2] = -doubleq0;
kf->H_data[3] = doubleq1;
kf->H_data[6] = doubleq1;
kf->H_data[7] = doubleq0;
kf->H_data[8] = doubleq3;
kf->H_data[9] = doubleq2;
kf->H_data[12] = doubleq0;
kf->H_data[13] = -doubleq1;
kf->H_data[14] = -doubleq2;
kf->H_data[15] = doubleq3;
}
/**
* @brief 利用观测值和先验估计得到最优的后验估计
* 加入了卡方检验以判断融合加速度的条件是否满足
* 同时引入发散保护保证恶劣工况下的必要量测更新
*
* @param kf
*/
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
{
static float q0, q1, q2, q3;
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
// 计算预测得到的重力加速度方向(通过姿态获取的)
kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
// 计算预测值和各个轴的方向余弦
for (uint8_t i = 0; i < 3; i++)
{
QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
}
// 利用加速度计数据修正
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k))
// chi-square test,卡方检验
kf->temp_matrix.numRows = kf->temp_vector1.numRows;
kf->temp_matrix.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix); // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k)))
kf->temp_vector.numRows = 1;
kf->temp_vector.numCols = kf->temp_vector1.numRows;
kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))'
kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare);
// rk is small,filter converged/converging
if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold)
{
QEKF_INS.ConvergeFlag = 1;
}
// rk is bigger than thre but once converged
if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
{
if (QEKF_INS.StableFlag)
{
QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验
}
else
{
QEKF_INS.ErrorCount = 0;
}
if (QEKF_INS.ErrorCount > 50)
{
// 滤波器发散
QEKF_INS.ConvergeFlag = 0;
kf->SkipEq5 = FALSE; // step-5 is cov mat P updating
}
else
{
// 残差未通过卡方检验 仅预测
// xhat(k) = xhat'(k)
// P(k) = P'(k)
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
kf->SkipEq5 = TRUE; // part5 is P updating
return;
}
}
else // if divergent or rk is not that big/acceptable,use adaptive gain
{
// scale adaptive,rk越小则增益越大,否则更相信预测值
if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
{
QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (0.9f * QEKF_INS.ChiSquareTestThreshold);
}
else
{
QEKF_INS.AdaptiveGainScale = 1;
}
QEKF_INS.ErrorCount = 0;
kf->SkipEq5 = FALSE;
}
// cal kf-gain K
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
// implement adaptive
for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++)
{
kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
}
for (uint8_t i = 4; i < 6; i++)
{
for (uint8_t j = 0; j < 3; j++)
{
kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
}
}
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
// 零漂修正限幅,一般不会有过大的漂移
if (QEKF_INS.ConvergeFlag)
{
for (uint8_t i = 4; i < 6; i++)
{
if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt)
{
kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
}
if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt)
{
kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
}
}
}
// 不修正yaw轴数据
kf->temp_vector.pData[3] = 0;
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
/**
* @brief EKF观测环节,其实就是把数据复制一下
*
* @param kf kf类型定义
*/
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf)
{
memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P));
memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K));
memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H));
}
/**
* @brief 自定义1/sqrt(x),速度更快
*
* @param x x
* @return float
*/
static float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f375a86 - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}

75
modules/algorithm/QuaternionEKF.h Normal file
View File

@@ -0,0 +1,75 @@
/**
******************************************************************************
* @file QuaternionEKF.h
* @author Wang Hongxi
* @version V1.2.0
* @date 2022/3/8
* @brief attitude update with gyro bias estimate and chi-square test
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _QUAT_EKF_H
#define _QUAT_EKF_H
#include "kalman_filter.h"
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
#endif
#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif
typedef struct
{
uint8_t Initialized;
KalmanFilter_t IMU_QuaternionEKF;
uint8_t ConvergeFlag;
uint8_t StableFlag;
uint64_t ErrorCount;
uint64_t UpdateCount;
float q[4]; // 四元数估计值
float GyroBias[3]; // 陀螺仪零偏估计值
float Gyro[3];
float Accel[3];
float OrientationCosine[3];
float accLPFcoef;
float gyro_norm;
float accl_norm;
float AdaptiveGainScale;
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
float Q1; // 四元数更新过程噪声
float Q2; // 陀螺仪零偏过程噪声
float R; // 加速度计量测噪声
float dt; // 姿态更新周期
mat ChiSquare;
float ChiSquare_Data[1]; // 卡方检验检测函数
float ChiSquareTestThreshold; // 卡方检验阈值
float lambda; // 渐消因子
int16_t YawRoundCount;
float YawAngleLast;
} QEKF_INS_t;
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_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
#endif

504
modules/algorithm/controller.c Normal file
View File

@@ -0,0 +1,504 @@
/**
******************************************************************************
* @file controller.c
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief DWT定时器用于计算控制周期 OLS用于提取信号微分
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "controller.h"
/******************************* PID CONTROL *********************************/
// PID优化环节函数声明
static void f_Trapezoid_Intergral(PID_t *pid);
static void f_Integral_Limit(PID_t *pid);
static void f_Derivative_On_Measurement(PID_t *pid);
static void f_Changing_Integration_Rate(PID_t *pid);
static void f_Output_Filter(PID_t *pid);
static void f_Derivative_Filter(PID_t *pid);
static void f_Output_Limit(PID_t *pid);
static void f_Proportion_Limit(PID_t *pid);
static void f_PID_ErrorHandle(PID_t *pid);
/**
* @brief PID初始化 PID initialize
* @param[in] PID结构体 PID structure
* @param[in] 略
* @retval 返回空 null
*/
void PID_Init(
PID_t *pid,
float max_out,
float intergral_limit,
float deadband,
float kp,
float Ki,
float Kd,
float A,
float B,
float output_lpf_rc,
float derivative_lpf_rc,
uint16_t ols_order,
uint8_t improve)
{
pid->DeadBand = deadband;
pid->IntegralLimit = intergral_limit;
pid->MaxOut = max_out;
pid->Ref = 0;
pid->Kp = kp;
pid->Ki = Ki;
pid->Kd = Kd;
pid->ITerm = 0;
// 变速积分参数
// coefficient of changing integration rate
pid->CoefA = A;
pid->CoefB = B;
pid->Output_LPF_RC = output_lpf_rc;
pid->Derivative_LPF_RC = derivative_lpf_rc;
// 最小二乘提取信号微分初始化
// differential signal is distilled by OLS
pid->OLS_Order = ols_order;
OLS_Init(&pid->OLS, ols_order);
// DWT定时器计数变量清零
// reset DWT Timer count counter
pid->DWT_CNT = 0;
// 设置PID优化环节
pid->Improve = improve;
// 设置PID异常处理 目前仅包含电机堵转保护
pid->ERRORHandler.ERRORCount = 0;
pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
pid->Output = 0;
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float PID_Calculate(PID_t *pid, float measure, float ref)
{
if (pid->Improve & ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT);
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
if (pid->User_Func1_f != NULL)
pid->User_Func1_f(pid);
if (abs(pid->Err) > pid->DeadBand)
{
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
if (pid->OLS_Order > 2)
pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, pid->Err);
else
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
if (pid->User_Func2_f != NULL)
pid->User_Func2_f(pid);
// 梯形积分
if (pid->Improve & Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm;
pid->Output = pid->Pout + pid->Iout + pid->Dout;
// 输出滤波
if (pid->Improve & OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
// 无关紧要
f_Proportion_Limit(pid);
}
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}
static void f_Trapezoid_Intergral(PID_t *pid)
{
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
static void f_Changing_Integration_Rate(PID_t *pid)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
// Integral still increasing
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
else
pid->ITerm = 0;
}
}
static void f_Integral_Limit(PID_t *pid)
{
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
// Integral still increasing
pid->ITerm = 0;
}
}
if (temp_Iout > pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = pid->IntegralLimit;
}
if (temp_Iout < -pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = -pid->IntegralLimit;
}
}
static void f_Derivative_On_Measurement(PID_t *pid)
{
if (pid->OLS_Order > 2)
pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, -pid->Measure);
else
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
static void f_Derivative_Filter(PID_t *pid)
{
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}
static void f_Output_Filter(PID_t *pid)
{
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
static void f_Output_Limit(PID_t *pid)
{
if (pid->Output > pid->MaxOut)
{
pid->Output = pid->MaxOut;
}
if (pid->Output < -(pid->MaxOut))
{
pid->Output = -(pid->MaxOut);
}
}
static void f_Proportion_Limit(PID_t *pid)
{
if (pid->Pout > pid->MaxOut)
{
pid->Pout = pid->MaxOut;
}
if (pid->Pout < -(pid->MaxOut))
{
pid->Pout = -(pid->MaxOut);
}
}
// PID ERRORHandle Function
static void f_PID_ErrorHandle(PID_t *pid)
{
/*Motor Blocked Handle*/
if (pid->Output < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
{
// Motor blocked counting
pid->ERRORHandler.ERRORCount++;
}
else
{
pid->ERRORHandler.ERRORCount = 0;
}
if (pid->ERRORHandler.ERRORCount > 500)
{
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = Motor_Blocked;
}
}
/*************************** FEEDFORWARD CONTROL *****************************/
/**
* @brief 前馈控制初始化
* @param[in] 前馈控制结构体
* @param[in] 略
* @retval 返回空
*/
void Feedforward_Init(
Feedforward_t *ffc,
float max_out,
float *c,
float lpf_rc,
uint16_t ref_dot_ols_order,
uint16_t ref_ddot_ols_order)
{
ffc->MaxOut = max_out;
// 设置前馈控制器参数 详见前馈控制结构体定义
// set parameters of feed-forward controller (see struct definition)
if (c != NULL && ffc != NULL)
{
ffc->c[0] = c[0];
ffc->c[1] = c[1];
ffc->c[2] = c[2];
}
else
{
ffc->c[0] = 0;
ffc->c[1] = 0;
ffc->c[2] = 0;
ffc->MaxOut = 0;
}
ffc->LPF_RC = lpf_rc;
// 最小二乘提取信号微分初始化
// differential signal is distilled by OLS
ffc->Ref_dot_OLS_Order = ref_dot_ols_order;
ffc->Ref_ddot_OLS_Order = ref_ddot_ols_order;
if (ref_dot_ols_order > 2)
OLS_Init(&ffc->Ref_dot_OLS, ref_dot_ols_order);
if (ref_ddot_ols_order > 2)
OLS_Init(&ffc->Ref_ddot_OLS, ref_ddot_ols_order);
ffc->DWT_CNT = 0;
ffc->Output = 0;
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float Feedforward_Calculate(Feedforward_t *ffc, float ref)
{
ffc->dt = DWT_GetDeltaT((void *)&ffc->DWT_CNT);
ffc->Ref = ref * ffc->dt / (ffc->LPF_RC + ffc->dt) +
ffc->Ref * ffc->LPF_RC / (ffc->LPF_RC + ffc->dt);
// 计算一阶导数
// calculate first derivative
if (ffc->Ref_dot_OLS_Order > 2)
ffc->Ref_dot = OLS_Derivative(&ffc->Ref_dot_OLS, ffc->dt, ffc->Ref);
else
ffc->Ref_dot = (ffc->Ref - ffc->Last_Ref) / ffc->dt;
// 计算二阶导数
// calculate second derivative
if (ffc->Ref_ddot_OLS_Order > 2)
ffc->Ref_ddot = OLS_Derivative(&ffc->Ref_ddot_OLS, ffc->dt, ffc->Ref_dot);
else
ffc->Ref_ddot = (ffc->Ref_dot - ffc->Last_Ref_dot) / ffc->dt;
// 计算前馈控制输出
// calculate feed-forward controller output
ffc->Output = ffc->c[0] * ffc->Ref + ffc->c[1] * ffc->Ref_dot + ffc->c[2] * ffc->Ref_ddot;
ffc->Output = float_constrain(ffc->Output, -ffc->MaxOut, ffc->MaxOut);
ffc->Last_Ref = ffc->Ref;
ffc->Last_Ref_dot = ffc->Ref_dot;
return ffc->Output;
}
/*************************LINEAR DISTURBANCE OBSERVER *************************/
void LDOB_Init(
LDOB_t *ldob,
float max_d,
float deadband,
float *c,
float lpf_rc,
uint16_t measure_dot_ols_order,
uint16_t measure_ddot_ols_order)
{
ldob->Max_Disturbance = max_d;
ldob->DeadBand = deadband;
// 设置线性扰动观测器参数 详见LDOB结构体定义
// set parameters of linear disturbance observer (see struct definition)
if (c != NULL && ldob != NULL)
{
ldob->c[0] = c[0];
ldob->c[1] = c[1];
ldob->c[2] = c[2];
}
else
{
ldob->c[0] = 0;
ldob->c[1] = 0;
ldob->c[2] = 0;
ldob->Max_Disturbance = 0;
}
// 设置Q(s)带宽 Q(s)选用一阶惯性环节
// set bandwidth of Q(s) Q(s) is chosen as a first-order low-pass form
ldob->LPF_RC = lpf_rc;
// 最小二乘提取信号微分初始化
// differential signal is distilled by OLS
ldob->Measure_dot_OLS_Order = measure_dot_ols_order;
ldob->Measure_ddot_OLS_Order = measure_ddot_ols_order;
if (measure_dot_ols_order > 2)
OLS_Init(&ldob->Measure_dot_OLS, measure_dot_ols_order);
if (measure_ddot_ols_order > 2)
OLS_Init(&ldob->Measure_ddot_OLS, measure_ddot_ols_order);
ldob->DWT_CNT = 0;
ldob->Disturbance = 0;
}
float LDOB_Calculate(LDOB_t *ldob, float measure, float u)
{
ldob->dt = DWT_GetDeltaT((void *)&ldob->DWT_CNT);
ldob->Measure = measure;
ldob->u = u;
// 计算一阶导数
// calculate first derivative
if (ldob->Measure_dot_OLS_Order > 2)
ldob->Measure_dot = OLS_Derivative(&ldob->Measure_dot_OLS, ldob->dt, ldob->Measure);
else
ldob->Measure_dot = (ldob->Measure - ldob->Last_Measure) / ldob->dt;
// 计算二阶导数
// calculate second derivative
if (ldob->Measure_ddot_OLS_Order > 2)
ldob->Measure_ddot = OLS_Derivative(&ldob->Measure_ddot_OLS, ldob->dt, ldob->Measure_dot);
else
ldob->Measure_ddot = (ldob->Measure_dot - ldob->Last_Measure_dot) / ldob->dt;
// 估计总扰动
// estimate external disturbances and internal disturbances caused by model uncertainties
ldob->Disturbance = ldob->c[0] * ldob->Measure + ldob->c[1] * ldob->Measure_dot + ldob->c[2] * ldob->Measure_ddot - ldob->u;
ldob->Disturbance = ldob->Disturbance * ldob->dt / (ldob->LPF_RC + ldob->dt) +
ldob->Last_Disturbance * ldob->LPF_RC / (ldob->LPF_RC + ldob->dt);
ldob->Disturbance = float_constrain(ldob->Disturbance, -ldob->Max_Disturbance, ldob->Max_Disturbance);
// 扰动输出死区
// deadband of disturbance output
if (abs(ldob->Disturbance) > ldob->DeadBand * ldob->Max_Disturbance)
ldob->Output = ldob->Disturbance;
else
ldob->Output = 0;
ldob->Last_Measure = ldob->Measure;
ldob->Last_Measure_dot = ldob->Measure_dot;
ldob->Last_Disturbance = ldob->Disturbance;
return ldob->Output;
}
/*************************** Tracking Differentiator ***************************/
void TD_Init(TD_t *td, float r, float h0)
{
td->r = r;
td->h0 = h0;
td->x = 0;
td->dx = 0;
td->ddx = 0;
td->last_dx = 0;
td->last_ddx = 0;
}
float TD_Calculate(TD_t *td, float input)
{
static float d, a0, y, a1, a2, a, fhan;
td->dt = DWT_GetDeltaT((void *)&td->DWT_CNT);
if (td->dt > 0.5f)
return 0;
td->Input = input;
d = td->r * td->h0 * td->h0;
a0 = td->dx * td->h0;
y = td->x - td->Input + a0;
a1 = sqrt(d * (d + 8 * abs(y)));
a2 = a0 + sign(y) * (a1 - d) / 2;
a = (a0 + y) * (sign(y + d) - sign(y - d)) / 2 + a2 * (1 - (sign(y + d) - sign(y - d)) / 2);
fhan = -td->r * a / d * (sign(a + d) - sign(a - d)) / 2 -
td->r * sign(a) * (1 - (sign(a + d) - sign(a - d)) / 2);
td->ddx = fhan;
td->dx += (td->ddx + td->last_ddx) * td->dt / 2;
td->x += (td->dx + td->last_dx) * td->dt / 2;
td->last_ddx = td->ddx;
td->last_dx = td->dx;
return td->x;
}

234
modules/algorithm/controller.h Normal file
View File

@@ -0,0 +1,234 @@
/**
******************************************************************************
* @file controller.h
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _CONTROLLER_H
#define _CONTROLLER_H
#include "main.h"
#include "stdint.h"
#include "string.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "user_lib.h"
#include "arm_math.h"
#include <math.h>
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
/******************************* PID CONTROL *********************************/
typedef enum pid_Improvement_e
{
NONE = 0X00, //0000 0000
Integral_Limit = 0x01, //0000 0001
Derivative_On_Measurement = 0x02, //0000 0010
Trapezoid_Intergral = 0x04, //0000 0100
Proportional_On_Measurement = 0x08, //0000 1000
OutputFilter = 0x10, //0001 0000
ChangingIntegrationRate = 0x20, //0010 0000
DerivativeFilter = 0x40, //0100 0000
ErrorHandle = 0x80, //1000 0000
} PID_Improvement_e;
typedef enum errorType_e
{
PID_ERROR_NONE = 0x00U,
Motor_Blocked = 0x01U
} ErrorType_e;
typedef __packed struct
{
uint64_t ERRORCount;
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
typedef __packed struct pid_t
{
float Ref;
float Kp;
float Ki;
float Kd;
float Measure;
float Last_Measure;
float Err;
float Last_Err;
float Last_ITerm;
float Pout;
float Iout;
float Dout;
float ITerm;
float Output;
float Last_Output;
float Last_Dout;
float MaxOut;
float IntegralLimit;
float DeadBand;
float ControlPeriod;
float CoefA; //For Changing Integral
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
uint16_t OLS_Order;
Ordinary_Least_Squares_t OLS;
uint32_t DWT_CNT;
float dt;
uint8_t Improve;
PID_ErrorHandler_t ERRORHandler;
void (*User_Func1_f)(struct pid_t *pid);
void (*User_Func2_f)(struct pid_t *pid);
} PID_t;
void PID_Init(
PID_t *pid,
float max_out,
float intergral_limit,
float deadband,
float kp,
float ki,
float kd,
float A,
float B,
float output_lpf_rc,
float derivative_lpf_rc,
uint16_t ols_order,
uint8_t improve);
float PID_Calculate(PID_t *pid, float measure, float ref);
/*************************** FEEDFORWARD CONTROL *****************************/
typedef __packed struct
{
float c[3]; // G(s) = 1/(c2s^2 + c1s + c0)
float Ref;
float Last_Ref;
float DeadBand;
uint32_t DWT_CNT;
float dt;
float LPF_RC; // RC = 1/omegac
float Ref_dot;
float Ref_ddot;
float Last_Ref_dot;
uint16_t Ref_dot_OLS_Order;
Ordinary_Least_Squares_t Ref_dot_OLS;
uint16_t Ref_ddot_OLS_Order;
Ordinary_Least_Squares_t Ref_ddot_OLS;
float Output;
float MaxOut;
} Feedforward_t;
void Feedforward_Init(
Feedforward_t *ffc,
float max_out,
float *c,
float lpf_rc,
uint16_t ref_dot_ols_order,
uint16_t ref_ddot_ols_order);
float Feedforward_Calculate(Feedforward_t *ffc, float ref);
/************************* LINEAR DISTURBANCE OBSERVER *************************/
typedef __packed struct
{
float c[3]; // G(s) = 1/(c2s^2 + c1s + c0)
float Measure;
float Last_Measure;
float u; // system input
float DeadBand;
uint32_t DWT_CNT;
float dt;
float LPF_RC; // RC = 1/omegac
float Measure_dot;
float Measure_ddot;
float Last_Measure_dot;
uint16_t Measure_dot_OLS_Order;
Ordinary_Least_Squares_t Measure_dot_OLS;
uint16_t Measure_ddot_OLS_Order;
Ordinary_Least_Squares_t Measure_ddot_OLS;
float Disturbance;
float Output;
float Last_Disturbance;
float Max_Disturbance;
} LDOB_t;
void LDOB_Init(
LDOB_t *ldob,
float max_d,
float deadband,
float *c,
float lpf_rc,
uint16_t measure_dot_ols_order,
uint16_t measure_ddot_ols_order);
float LDOB_Calculate(LDOB_t *ldob, float measure, float u);
/*************************** Tracking Differentiator ***************************/
typedef __packed struct
{
float Input;
float h0;
float r;
float x;
float dx;
float ddx;
float last_dx;
float last_ddx;
uint32_t DWT_CNT;
float dt;
} TD_t;
void TD_Init(TD_t *td, float r, float h0);
float TD_Calculate(TD_t *td, float input);
#endif

480
modules/algorithm/kalman_filter.c Normal file
View File

@@ -0,0 +1,480 @@
/**
******************************************************************************
* @file kalman filter.c
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief C implementation of kalman filter
******************************************************************************
* @attention
* 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。
* This implementation of kalman filter can dynamically adjust dimension and
* value of matrix H R and K according to the measurement validity under any
* circumstance that the sampling rate of component sensors are different.
*
* 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
* 入传感器量测所对应的状态与这个量测的方式，详情请见例程
* Therefore, the initialization of matrix P, F, and Q is sometimes different
* from that of matrices H R. when initialization. Additionally, the corresponding
* state and the method of the measurement should be provided when initializing
* measurement vector z. For more details, please see the example.
*
* 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初
* 始化矩阵P那样用常规方式初始化z H R即可。
* If automatic adjustment is not required, assign zero to the UseAutoAdjustment
* and initialize z H R in the normal way as matrix P.
*
* 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼
* 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
* MeasuredVector and ControlVector are required to be updated in the sensor
* callback function. Integer 0 in measurement vector z indicates the invalidity
* of current measurement, so MeasuredVector and ControlVector will be reset
* (to 0) during each update.
*
* 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法
* 通过限制矩阵P最小值的方法，可有效抑制滤波器的过度收敛，详情请见例程。
* Additionally, the excessive convergence of matrix P will make filter incapable
* of adopting the slowly changing state. This implementation can effectively
* suppress filter excessive convergence through boundary limiting for matrix P.
* For more details, please see the example.
*
* @example:
* x =
* | height |
* | velocity |
* |acceleration|
*
* KalmanFilter_t Height_KF;
*
* void INS_Task_Init(void)
* {
* static float P_Init[9] =
* {
* 10, 0, 0,
* 0, 30, 0,
* 0, 0, 10,
* };
* static float F_Init[9] =
* {
* 1, dt, 0.5*dt*dt,
* 0, 1, dt,
* 0, 0, 1,
* };
* static float Q_Init[9] =
* {
* 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
* 0.5*dt*dt*dt, dt*dt, dt,
* 0.5*dt*dt, dt, 1,
* };
*
* // 设置最小方差
* static float state_min_variance[3] = {0.03, 0.005, 0.1};
*
* // 开启自动调整
* Height_KF.UseAutoAdjustment = 1;
*
* // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度
* static uint8_t measurement_reference[3] = {1, 1, 3}
*
* static float measurement_degree[3] = {1, 1, 1}
* // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下）
* |1 0 0|
* |1 0 0|
* |0 0 1|
*
* static float mat_R_diagonal_elements = {30, 25, 35}
* //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下）
* |30 0 0|
* | 0 25 0|
* | 0 0 35|
*
* Kalman_Filter_Init(&Height_KF, 3, 0, 3);
*
* // 设置矩阵值
* memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
* memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
* memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
* memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference));
* memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
* memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements));
* memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance));
* }
*
* void INS_Task(void const *pvParameters)
* {
* // 循环更新
* Kalman_Filter_Update(&Height_KF);
* vTaskDelay(ts);
* }
*
* // 测量数据更新应按照以下形式 即向MeasuredVector赋值
* void Barometer_Read_Over(void)
* {
* ......
* INS_KF.MeasuredVector[0] = baro_height;
* }
* void GPS_Read_Over(void)
* {
* ......
* INS_KF.MeasuredVector[1] = GPS_height;
* }
* void Acc_Data_Process(void)
* {
* ......
* INS_KF.MeasuredVector[2] = acc.z;
* }
******************************************************************************
*/
#include "kalman_filter.h"
uint16_t sizeof_float, sizeof_double;
static void H_K_R_Adjustment(KalmanFilter_t *kf);
/**
* @brief 初始化矩阵维度信息并为矩阵分配空间
*
* @param kf kf类型定义
* @param xhatSize 状态变量维度
* @param uSize 控制变量维度
* @param zSize 观测量维度
*/
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize)
{
sizeof_float = sizeof(float);
sizeof_double = sizeof(double);
kf->xhatSize = xhatSize;
kf->uSize = uSize;
kf->zSize = zSize;
kf->MeasurementValidNum = 0;
// measurement flags
kf->MeasurementMap = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize);
kf->MeasurementDegree = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MeasurementDegree, 0, sizeof_float * zSize);
kf->MatR_DiagonalElements = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize);
kf->StateMinVariance = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->StateMinVariance, 0, sizeof_float * xhatSize);
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
memset(kf->temp, 0, sizeof(uint8_t) * zSize);
// filter data
kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->FilteredValue, 0, sizeof_float * xhatSize);
kf->MeasuredVector = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MeasuredVector, 0, sizeof_float * zSize);
kf->ControlVector = (float *)user_malloc(sizeof_float * uSize);
memset(kf->ControlVector, 0, sizeof_float * uSize);
// xhat x(k|k)
kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->xhat_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *)kf->xhat_data);
// xhatminus x(k|k-1)
kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data);
if (uSize != 0)
{
// control vector u
kf->u_data = (float *)user_malloc(sizeof_float * uSize);
memset(kf->u_data, 0, sizeof_float * uSize);
Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data);
}
// measurement vector z
kf->z_data = (float *)user_malloc(sizeof_float * zSize);
memset(kf->z_data, 0, sizeof_float * zSize);
Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data);
// covariance matrix P(k|k)
kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data);
// create covariance matrix P(k|k-1)
kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data);
// state transition matrix F FT
kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize);
memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data);
Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data);
if (uSize != 0)
{
// control matrix B
kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize);
memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
}
// measurement matrix H
kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize);
kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
memset(kf->H_data, 0, sizeof_float * zSize * xhatSize);
memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize);
Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data);
Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data);
// process noise covariance matrix Q
kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data);
// measurement noise covariance matrix R
kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize);
memset(kf->R_data, 0, sizeof_float * zSize * zSize);
Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data);
// kalman gain K
kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data);
kf->S_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_matrix_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize);
kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize);
Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data);
Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data);
Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1);
Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data);
Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1);
kf->SkipEq1 = 0;
kf->SkipEq2 = 0;
kf->SkipEq3 = 0;
kf->SkipEq4 = 0;
kf->SkipEq5 = 0;
}
void Kalman_Filter_Measure(KalmanFilter_t *kf)
{
// 矩阵H K R根据量测情况自动调整
// matrix H K R auto adjustment
if (kf->UseAutoAdjustment != 0)
H_K_R_Adjustment(kf);
else
{
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
}
memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize);
}
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq1)
{
if (kf->uSize > 0)
{
kf->temp_vector.numRows = kf->xhatSize;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
kf->temp_vector1.numRows = kf->xhatSize;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1);
kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
}
else
{
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus);
}
}
}
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq2)
{
kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT);
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus);
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->FT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT
kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus);
}
}
void Kalman_Filter_SetK(KalmanFilter_t *kf)
{
if (!kf->SkipEq3)
{
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
}
}
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq4)
{
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k)
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H·xhat'(k)
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
}
void Kalman_Filter_P_Update(KalmanFilter_t *kf)
{
if (!kf->SkipEq5)
{
kf->temp_matrix.numRows = kf->K.numRows;
kf->temp_matrix.numCols = kf->H.numCols;
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)·H·P'(k)
kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P);
}
}
/**
* @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
*
* @param kf kf类型定义
* @return float* 返回滤波值
*/
float *Kalman_Filter_Update(KalmanFilter_t *kf)
{
// 0. 获取量测信息
Kalman_Filter_Measure(kf);
if (kf->User_Func0_f != NULL)
kf->User_Func0_f(kf);
// 先验估计
// 1. xhat'(k)= A·xhat(k-1) + B·u
Kalman_Filter_xhatMinusUpdate(kf);
if (kf->User_Func1_f != NULL)
kf->User_Func1_f(kf);
// 预测更新
// 2. P'(k) = A·P(k-1)·AT + Q
Kalman_Filter_PminusUpdate(kf);
if (kf->User_Func2_f != NULL)
kf->User_Func2_f(kf);
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
{
// 量测更新
// 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
Kalman_Filter_SetK(kf);
if (kf->User_Func3_f != NULL)
kf->User_Func3_f(kf);
// 融合
// 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k))
Kalman_Filter_xhatUpdate(kf);
if (kf->User_Func4_f != NULL)
kf->User_Func4_f(kf);
// 修正方差
// 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k)
Kalman_Filter_P_Update(kf);
}
else
{
// 无有效量测,仅预测
// xhat(k) = xhat'(k)
// P(k) = P'(k)
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
}
// 自定义函数,可以提供后处理等
if (kf->User_Func5_f != NULL)
kf->User_Func5_f(kf);
// 避免滤波器过度收敛
// suppress filter excessive convergence
for (uint8_t i = 0; i < kf->xhatSize; i++)
{
if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
}
memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize);
if (kf->User_Func6_f != NULL)
kf->User_Func6_f(kf);
return kf->FilteredValue;
}
static void H_K_R_Adjustment(KalmanFilter_t *kf)
{
kf->MeasurementValidNum = 0;
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
// 识别量测数据有效性并调整矩阵H R K
// recognize measurement validity and adjust matrices H R K
memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
for (uint8_t i = 0; i < kf->zSize; i++)
{
if (kf->z_data[i] != 0)
{
// 重构向量z
// rebuild vector z
kf->z_data[kf->MeasurementValidNum] = kf->z_data[i];
kf->temp[kf->MeasurementValidNum] = i;
// 重构矩阵H
// rebuild matrix H
kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i];
kf->MeasurementValidNum++;
}
}
for (uint8_t i = 0; i < kf->MeasurementValidNum; i++)
{
// 重构矩阵R
// rebuild matrix R
kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]];
}
// 调整矩阵维数
// adjust the dimensions of system matrices
kf->H.numRows = kf->MeasurementValidNum;
kf->H.numCols = kf->xhatSize;
kf->HT.numRows = kf->xhatSize;
kf->HT.numCols = kf->MeasurementValidNum;
kf->R.numRows = kf->MeasurementValidNum;
kf->R.numCols = kf->MeasurementValidNum;
kf->K.numRows = kf->xhatSize;
kf->K.numCols = kf->MeasurementValidNum;
kf->z.numRows = kf->MeasurementValidNum;
}

121
modules/algorithm/kalman_filter.h Normal file
View File

@@ -0,0 +1,121 @@
/**
******************************************************************************
* @file kalman filter.h
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
// cortex-m4 DSP lib
/*
#define __CC_ARM // Keil
#define ARM_MATH_CM4
#define ARM_MATH_MATRIX_CHECK
#define ARM_MATH_ROUNDING
#define ARM_MATH_DSP // define in arm_math.h
*/
#include "stm32f407xx.h"
#include "arm_math.h"
//#include "dsp/matrix_functions.h"
#include "math.h"
#include "stdint.h"
#include "stdlib.h"
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
#define mat arm_matrix_instance_f32
#define Matrix_Init arm_mat_init_f32
#define Matrix_Add arm_mat_add_f32
#define Matrix_Subtract arm_mat_sub_f32
#define Matrix_Multiply arm_mat_mult_f32
#define Matrix_Transpose arm_mat_trans_f32
#define Matrix_Inverse arm_mat_inverse_f32
typedef struct kf_t
{
float *FilteredValue;
float *MeasuredVector;
float *ControlVector;
uint8_t xhatSize;
uint8_t uSize;
uint8_t zSize;
uint8_t UseAutoAdjustment;
uint8_t MeasurementValidNum;
uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state
float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H
float *MatR_DiagonalElements; // 量测方差 variance for each measurement
float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence
uint8_t *temp;
// 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个
uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5;
// definiion of struct mat: rows & cols & pointer to vars
mat xhat; // x(k|k)
mat xhatminus; // x(k|k-1)
mat u; // control vector u
mat z; // measurement vector z
mat P; // covariance matrix P(k|k)
mat Pminus; // covariance matrix P(k|k-1)
mat F, FT; // state transition matrix F FT
mat B; // control matrix B
mat H, HT; // measurement matrix H
mat Q; // process noise covariance matrix Q
mat R; // measurement noise covariance matrix R
mat K; // kalman gain K
mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1;
int8_t MatStatus;
// 用户定义函数,可以替换或扩展基准KF的功能
void (*User_Func0_f)(struct kf_t *kf);
void (*User_Func1_f)(struct kf_t *kf);
void (*User_Func2_f)(struct kf_t *kf);
void (*User_Func3_f)(struct kf_t *kf);
void (*User_Func4_f)(struct kf_t *kf);
void (*User_Func5_f)(struct kf_t *kf);
void (*User_Func6_f)(struct kf_t *kf);
// 矩阵存储空间指针
float *xhat_data, *xhatminus_data;
float *u_data;
float *z_data;
float *P_data, *Pminus_data;
float *F_data, *FT_data;
float *B_data;
float *H_data, *HT_data;
float *Q_data;
float *R_data;
float *K_data;
float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1;
} KalmanFilter_t;
extern uint16_t sizeof_float, sizeof_double;
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize);
void Kalman_Filter_Measure(KalmanFilter_t *kf);
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf);
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf);
void Kalman_Filter_SetK(KalmanFilter_t *kf);
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf);
void Kalman_Filter_P_Update(KalmanFilter_t *kf);
float *Kalman_Filter_Update(KalmanFilter_t *kf);
#endif //__KALMAN_FILTER_H

392
modules/algorithm/user_lib.c Normal file
View File

@@ -0,0 +1,392 @@
/**
******************************************************************************
* @file user_lib.c
* @author Wang Hongxi
* @version V1.0.0
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "stdlib.h"
#include "string.h"
#include "user_lib.h"
#include "math.h"
#include "main.h"
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
uint8_t GlobalDebugMode = 7;
//快速开方
float Sqrt(float x)
{
float y;
float delta;
float maxError;
if (x <= 0)
{
return 0;
}
// initial guess
y = x / 2;
// refine
maxError = x * 0.001f;
do
{
delta = (y * y) - x;
y -= delta / (2 * y);
} while (delta > maxError || delta < -maxError);
return y;
}
//快速求平方根倒数
/*
float invSqrt(float num)
{
float halfnum = 0.5f * num;
float y = num;
long i = *(long *)&y;
i = 0x5f375a86- (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfnum * y * y));
return y;
}*/
/**
* @brief 斜波函数初始化
* @author RM
* @param[in] 斜波函数结构体
* @param[in] 间隔的时间，单位 s
* @param[in] 最大值
* @param[in] 最小值
* @retval 返回空
*/
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min)
{
ramp_source_type->frame_period = frame_period;
ramp_source_type->max_value = max;
ramp_source_type->min_value = min;
ramp_source_type->input = 0.0f;
ramp_source_type->out = 0.0f;
}
/**
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
* @author RM
* @param[in] 斜波函数结构体
* @param[in] 输入值
* @retval 返回空
*/
float ramp_calc(ramp_function_source_t *ramp_source_type, float input)
{
ramp_source_type->input = input;
ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
if (ramp_source_type->out > ramp_source_type->max_value)
{
ramp_source_type->out = ramp_source_type->max_value;
}
else if (ramp_source_type->out < ramp_source_type->min_value)
{
ramp_source_type->out = ramp_source_type->min_value;
}
return ramp_source_type->out;
}
//绝对值限制
float abs_limit(float num, float Limit)
{
if (num > Limit)
{
num = Limit;
}
else if (num < -Limit)
{
num = -Limit;
}
return num;
}
//判断符号位
float sign(float value)
{
if (value >= 0.0f)
{
return 1.0f;
}
else
{
return -1.0f;
}
}
//浮点死区
float float_deadband(float Value, float minValue, float maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0.0f;
}
return Value;
}
//int26死区
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0;
}
return Value;
}
//限幅函数
float float_constrain(float Value, float minValue, float maxValue)
{
if (Value < minValue)
return minValue;
else if (Value > maxValue)
return maxValue;
else
return Value;
}
//限幅函数
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
{
if (Value < minValue)
return minValue;
else if (Value > maxValue)
return maxValue;
else
return Value;
}
//循环限幅函数
float loop_float_constrain(float Input, float minValue, float maxValue)
{
if (maxValue < minValue)
{
return Input;
}
if (Input > maxValue)
{
float len = maxValue - minValue;
while (Input > maxValue)
{
Input -= len;
}
}
else if (Input < minValue)
{
float len = maxValue - minValue;
while (Input < minValue)
{
Input += len;
}
}
return Input;
}
//弧度格式化为-PI~PI
//角度格式化为-180~180
float theta_format(float Ang)
{
return loop_float_constrain(Ang, -180.0f, 180.0f);
}
int float_rounding(float raw)
{
static int integer;
static float decimal;
integer = (int)raw;
decimal = raw - integer;
if (decimal > 0.5f)
integer++;
return integer;
}
/**
* @brief 最小二乘法初始化
* @param[in] 最小二乘法结构体
* @param[in] 样本数
* @retval 返回空
*/
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order)
{
OLS->Order = order;
OLS->Count = 0;
OLS->x = (float *)user_malloc(sizeof(float) * order);
OLS->y = (float *)user_malloc(sizeof(float) * order);
OLS->k = 0;
OLS->b = 0;
memset((void *)OLS->x, 0, sizeof(float) * order);
memset((void *)OLS->y, 0, sizeof(float) * order);
memset((void *)OLS->t, 0, sizeof(float) * 4);
}
/**
* @brief 最小二乘法拟合
* @param[in] 最小二乘法结构体
* @param[in] 信号新样本距上一个样本时间间隔
* @param[in] 信号值
*/
void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y)
{
static float temp = 0;
temp = OLS->x[1];
for (uint16_t i = 0; i < OLS->Order - 1; ++i)
{
OLS->x[i] = OLS->x[i + 1] - temp;
OLS->y[i] = OLS->y[i + 1];
}
OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
OLS->y[OLS->Order - 1] = y;
if (OLS->Count < OLS->Order)
{
OLS->Count++;
}
memset((void *)OLS->t, 0, sizeof(float) * 4);
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
{
OLS->t[0] += OLS->x[i] * OLS->x[i];
OLS->t[1] += OLS->x[i];
OLS->t[2] += OLS->x[i] * OLS->y[i];
OLS->t[3] += OLS->y[i];
}
OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
OLS->StandardDeviation = 0;
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
{
OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
}
OLS->StandardDeviation /= OLS->Order;
}
/**
* @brief 最小二乘法提取信号微分
* @param[in] 最小二乘法结构体
* @param[in] 信号新样本距上一个样本时间间隔
* @param[in] 信号值
* @retval 返回斜率k
*/
float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y)
{
static float temp = 0;
temp = OLS->x[1];
for (uint16_t i = 0; i < OLS->Order - 1; ++i)
{
OLS->x[i] = OLS->x[i + 1] - temp;
OLS->y[i] = OLS->y[i + 1];
}
OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
OLS->y[OLS->Order - 1] = y;
if (OLS->Count < OLS->Order)
{
OLS->Count++;
}
memset((void *)OLS->t, 0, sizeof(float) * 4);
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
{
OLS->t[0] += OLS->x[i] * OLS->x[i];
OLS->t[1] += OLS->x[i];
OLS->t[2] += OLS->x[i] * OLS->y[i];
OLS->t[3] += OLS->y[i];
}
OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
OLS->StandardDeviation = 0;
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
{
OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
}
OLS->StandardDeviation /= OLS->Order;
return OLS->k;
}
/**
* @brief 获取最小二乘法提取信号微分
* @param[in] 最小二乘法结构体
* @retval 返回斜率k
*/
float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS)
{
return OLS->k;
}
/**
* @brief 最小二乘法平滑信号
* @param[in] 最小二乘法结构体
* @param[in] 信号新样本距上一个样本时间间隔
* @param[in] 信号值
* @retval 返回平滑输出
*/
float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y)
{
static float temp = 0;
temp = OLS->x[1];
for (uint16_t i = 0; i < OLS->Order - 1; ++i)
{
OLS->x[i] = OLS->x[i + 1] - temp;
OLS->y[i] = OLS->y[i + 1];
}
OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
OLS->y[OLS->Order - 1] = y;
if (OLS->Count < OLS->Order)
{
OLS->Count++;
}
memset((void *)OLS->t, 0, sizeof(float) * 4);
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
{
OLS->t[0] += OLS->x[i] * OLS->x[i];
OLS->t[1] += OLS->x[i];
OLS->t[2] += OLS->x[i] * OLS->y[i];
OLS->t[3] += OLS->y[i];
}
OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
OLS->StandardDeviation = 0;
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
{
OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
}
OLS->StandardDeviation /= OLS->Order;
return OLS->k * OLS->x[OLS->Order - 1] + OLS->b;
}
/**
* @brief 获取最小二乘法平滑信号
* @param[in] 最小二乘法结构体
* @retval 返回平滑输出
*/
float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS)
{
return OLS->k * OLS->x[OLS->Order - 1] + OLS->b;
}

152
modules/algorithm/user_lib.h Normal file
View File

@@ -0,0 +1,152 @@
/**
******************************************************************************
* @file user_lib.h
* @author Wang Hongxi
* @version V1.0.0
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _USER_LIB_H
#define _USER_LIB_H
#include "stdint.h"
#include "main.h"
#include "cmsis_os.h"
enum
{
CHASSIS_DEBUG = 1,
GIMBAL_DEBUG,
INS_DEBUG,
RC_DEBUG,
IMU_HEAT_DEBUG,
SHOOT_DEBUG,
AIMASSIST_DEBUG,
};
extern uint8_t GlobalDebugMode;
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
#endif
#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif
/* math relevant */
/* radian coefficient */
#ifndef RADIAN_COEF
#define RADIAN_COEF 57.295779513f
#endif
/* circumference ratio */
#ifndef PI
#define PI 3.14159265354f
#endif
#define VAL_LIMIT(val, min, max) \
do \
{ \
if ((val) <= (min)) \
{ \
(val) = (min); \
} \
else if ((val) >= (max)) \
{ \
(val) = (max); \
} \
} while (0)
#define ANGLE_LIMIT_360(val, angle) \
do \
{ \
(val) = (angle) - (int)(angle); \
(val) += (int)(angle) % 360; \
} while (0)
#define ANGLE_LIMIT_360_TO_180(val) \
do \
{ \
if ((val) > 180) \
(val) -= 360; \
} while (0)
#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
typedef struct
{
float input; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float min_value; //<2F>޷<EFBFBD><DEB7><EFBFBD>Сֵ
float max_value; //<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֵ
float frame_period; //ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
} ramp_function_source_t;
typedef __packed struct
{
uint16_t Order;
uint32_t Count;
float *x;
float *y;
float k;
float b;
float StandardDeviation;
float t[4];
} Ordinary_Least_Squares_t;
//<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>
float Sqrt(float x);
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float abs_limit(float num, float Limit);
//<2F>жϷ<D0B6><CFB7><EFBFBD>λ
float sign(float value);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float float_deadband(float Value, float minValue, float maxValue);
// int26<32><36><EFBFBD><EFBFBD>
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
float float_constrain(float Value, float minValue, float maxValue);
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
//ѭ<><D1AD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
float loop_float_constrain(float Input, float minValue, float maxValue);
//<2F>Ƕ<EFBFBD> <20><><EFBFBD>޷<EFBFBD> 180 ~ -180
float theta_format(float Ang);
int float_rounding(float raw);
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order);
void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS);
float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS);
#endif

29
modules/imu/BMI088Middleware.c Normal file
View File

@@ -0,0 +1,29 @@
#include "BMI088Middleware.h"
#include "main.h"
SPI_HandleTypeDef *BMI088_SPI;
void BMI088_ACCEL_NS_L(void)
{
HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
}
void BMI088_ACCEL_NS_H(void)
{
HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}
void BMI088_GYRO_NS_L(void)
{
HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
}
void BMI088_GYRO_NS_H(void)
{
HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_SET);
}
uint8_t BMI088_read_write_byte(uint8_t txdata)
{
uint8_t rx_data;
HAL_SPI_TransmitReceive(BMI088_SPI, &txdata, &rx_data, 1, 1000);
return rx_data;
}

32
modules/imu/BMI088Middleware.h Normal file
View File

@@ -0,0 +1,32 @@
#ifndef BMI088MIDDLEWARE_H
#define BMI088MIDDLEWARE_H
#include "main.h"
#define BMI088_USE_SPI
//#define BMI088_USE_IIC
/*
#define CS1_ACCEL_GPIO_Port ACCEL_NSS_GPIO_Port
#define CS1_ACCEL_Pin ACCEL_NSS_Pin
#define CS1_GYRO_GPIO_Port GYRO_NSS_GPIO_Port
#define CS1_GYRO_Pin GYRO_NSS_Pin
*/
#if defined(BMI088_USE_SPI)
extern void BMI088_ACCEL_NS_L(void);
extern void BMI088_ACCEL_NS_H(void);
extern void BMI088_GYRO_NS_L(void);
extern void BMI088_GYRO_NS_H(void);
extern uint8_t BMI088_read_write_byte(uint8_t reg);
extern SPI_HandleTypeDef *BMI088_SPI;
#elif defined(BMI088_USE_IIC)
#endif
#endif

418
modules/imu/BMI088driver.c Normal file
View File

@@ -0,0 +1,418 @@
/**
******************************************************************************
* @file BMI088driver.c
* @author
* @version V1.2.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "BMI088driver.h"
#include "BMI088reg.h"
#include "BMI088Middleware.h"
#include "bsp_dwt.h"
#include <math.h>
float BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN;
float BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN;
static uint8_t res = 0;
static uint8_t write_reg_num = 0;
static uint8_t error = BMI088_NO_ERROR;
float gyroDiff[3], gNormDiff;
uint8_t caliOffset = 1;
int16_t caliCount = 0;
static uint32_t offset_cal_DWT_Count = 0;
IMU_Data_t BMI088;
#if defined(BMI088_USE_SPI)
#define BMI088_accel_write_single_reg(reg, data) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_write_single_reg((reg), (data)); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_accel_read_single_reg(reg, data) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_read_write_byte((reg) | 0x80); \
BMI088_read_write_byte(0x55); \
(data) = BMI088_read_write_byte(0x55); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_accel_read_muli_reg(reg, data, len) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_read_write_byte((reg) | 0x80); \
BMI088_read_muli_reg(reg, data, len); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_gyro_write_single_reg(reg, data) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_write_single_reg((reg), (data)); \
BMI088_GYRO_NS_H(); \
}
#define BMI088_gyro_read_single_reg(reg, data) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_read_single_reg((reg), &(data)); \
BMI088_GYRO_NS_H(); \
}
#define BMI088_gyro_read_muli_reg(reg, data, len) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_read_muli_reg((reg), (data), (len)); \
BMI088_GYRO_NS_H(); \
}
static void BMI088_write_single_reg(uint8_t reg, uint8_t data);
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data);
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len);
#elif defined(BMI088_USE_IIC)
#endif
static uint8_t write_BMI088_accel_reg_data_error[BMI088_WRITE_ACCEL_REG_NUM][3] =
{
{BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
{BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
{BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
{BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
{BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
{BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}
};
static uint8_t write_BMI088_gyro_reg_data_error[BMI088_WRITE_GYRO_REG_NUM][3] =
{
{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
{BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
{BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
{BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
{BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
{BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}
};
static void Calibrate_MPU_Offset(IMU_Data_t *bmi088);
uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate)
{
BMI088_SPI = bmi088_SPI;
error = BMI088_NO_ERROR;
error |= bmi088_accel_init();
error |= bmi088_gyro_init();
if (calibrate)
Calibrate_MPU_Offset(&BMI088);
else
{
BMI088.GyroOffset[0] = GxOFFSET;
BMI088.GyroOffset[1] = GyOFFSET;
BMI088.GyroOffset[2] = GzOFFSET;
BMI088.gNorm = gNORM;
BMI088.AccelScale = 9.81f / BMI088.gNorm;
BMI088.TempWhenCali = 40;
}
return error;
}
void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
{
static float startTime;
static uint16_t CaliTimes = 6000;
uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
int16_t bmi088_raw_temp;
float gyroMax[3], gyroMin[3];
float gNormTemp, gNormMax, gNormMin;
startTime = DWT_GetTimeline_s();
do
{
if (DWT_GetTimeline_s() - startTime > 10)
{
// <20><>????
bmi088->GyroOffset[0] = GxOFFSET;
bmi088->GyroOffset[1] = GyOFFSET;
bmi088->GyroOffset[2] = GzOFFSET;
bmi088->gNorm = gNORM;
bmi088->TempWhenCali = 40;
break;
}
DWT_Delay(0.005);
bmi088->gNorm = 0;
bmi088->GyroOffset[0] = 0;
bmi088->GyroOffset[1] = 0;
bmi088->GyroOffset[2] = 0;
for (uint16_t i = 0; i < CaliTimes; i++)
{
BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0];
bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN;
gNormTemp = sqrtf(bmi088->Accel[0] * bmi088->Accel[0] +
bmi088->Accel[1] * bmi088->Accel[1] +
bmi088->Accel[2] * bmi088->Accel[2]);
bmi088->gNorm += gNormTemp;
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
{
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[0] += bmi088->Gyro[0];
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[1] += bmi088->Gyro[1];
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[2] += bmi088->Gyro[2];
}
if (i == 0)
{
gNormMax = gNormTemp;
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; j++)
{
gyroMax[j] = bmi088->Gyro[j];
gyroMin[j] = bmi088->Gyro[j];
}
}
else
{
if (gNormTemp > gNormMax)
gNormMax = gNormTemp;
if (gNormTemp < gNormMin)
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; j++)
{
if (bmi088->Gyro[j] > gyroMax[j])
gyroMax[j] = bmi088->Gyro[j];
if (bmi088->Gyro[j] < gyroMin[j])
gyroMin[j] = bmi088->Gyro[j];
}
}
gNormDiff = gNormMax - gNormMin;
for (uint8_t j = 0; j < 3; j++)
gyroDiff[j] = gyroMax[j] - gyroMin[j];
if (gNormDiff > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f)
break;
DWT_Delay(0.0005);
}
bmi088->gNorm /= (float)CaliTimes;
for (uint8_t i = 0; i < 3; i++)
bmi088->GyroOffset[i] /= (float)CaliTimes;
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t)((buf[0] << 3) | (buf[1] >> 5));
if (bmi088_raw_temp > 1023)
bmi088_raw_temp -= 2048;
bmi088->TempWhenCali = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
caliCount++;
} while (gNormDiff > 0.5f ||
fabsf(bmi088->gNorm - 9.8f) > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f ||
fabsf(bmi088->GyroOffset[0]) > 0.01f ||
fabsf(bmi088->GyroOffset[1]) > 0.01f ||
fabsf(bmi088->GyroOffset[2]) > 0.01f);
bmi088->AccelScale = 9.81f / bmi088->gNorm;
}
uint8_t bmi088_accel_init(void)
{
// check commiunication
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
// accel software reset
BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check commiunication is normal after reset
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
// check the "who am I"
if (res != BMI088_ACC_CHIP_ID_VALUE)
return BMI088_NO_SENSOR;
// set accel sonsor config and check
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_ACCEL_REG_NUM; write_reg_num++)
{
BMI088_accel_write_single_reg(write_BMI088_accel_reg_data_error[write_reg_num][0], write_BMI088_accel_reg_data_error[write_reg_num][1]);
HAL_Delay(1);
BMI088_accel_read_single_reg(write_BMI088_accel_reg_data_error[write_reg_num][0], res);
HAL_Delay(1);
if (res != write_BMI088_accel_reg_data_error[write_reg_num][1])
{
// write_reg_num--;
// return write_BMI088_accel_reg_data_error[write_reg_num][2];
error |= write_BMI088_accel_reg_data_error[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
uint8_t bmi088_gyro_init(void)
{
// check commiunication
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
// reset the gyro sensor
BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check commiunication is normal after reset
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
// check the "who am I"
if (res != BMI088_GYRO_CHIP_ID_VALUE)
return BMI088_NO_SENSOR;
// set gyro sonsor config and check
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_GYRO_REG_NUM; write_reg_num++)
{
BMI088_gyro_write_single_reg(write_BMI088_gyro_reg_data_error[write_reg_num][0], write_BMI088_gyro_reg_data_error[write_reg_num][1]);
HAL_Delay(1);
BMI088_gyro_read_single_reg(write_BMI088_gyro_reg_data_error[write_reg_num][0], res);
HAL_Delay(1);
if (res != write_BMI088_gyro_reg_data_error[write_reg_num][1])
{
write_reg_num--;
// return write_BMI088_gyro_reg_data_error[write_reg_num][2];
error |= write_BMI088_accel_reg_data_error[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
void BMI088_Read(IMU_Data_t *bmi088)
{
static uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
static int16_t bmi088_raw_temp;
static float dt=1.0;
static uint8_t first_read_flag=0;
if(!first_read_flag)
{
first_read_flag=1;
DWT_GetDeltaT(&offset_cal_DWT_Count);
}
else
{
dt=DWT_GetDeltaT(&offset_cal_DWT_Count)*1000.0;
}
BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0];
bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
{
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_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1]*dt;
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2]*dt;
}
else
{
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
}
}
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t)((buf[0] << 3) | (buf[1] >> 5));
if (bmi088_raw_temp > 1023)
{
bmi088_raw_temp -= 2048;
}
bmi088->Temperature = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
}
#if defined(BMI088_USE_SPI)
static void BMI088_write_single_reg(uint8_t reg, uint8_t data)
{
BMI088_read_write_byte(reg);
BMI088_read_write_byte(data);
}
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data)
{
BMI088_read_write_byte(reg | 0x80);
*return_data = BMI088_read_write_byte(0x55);
}
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len)
{
BMI088_read_write_byte(reg | 0x80);
while (len != 0)
{
*buf = BMI088_read_write_byte(0x55);
buf++;
len--;
}
}
#elif defined(BMI088_USE_IIC)
#endif

121
modules/imu/BMI088driver.h Normal file
View File

@@ -0,0 +1,121 @@
/**
******************************************************************************
* @file BMI088driver.h
* @author
* @version V1.1.2
* @version V1.2.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef BMI088DRIVER_H
#define BMI088DRIVER_H
#include "stdint.h"
#include "main.h"
#define BMI088_TEMP_FACTOR 0.125f
#define BMI088_TEMP_OFFSET 23.0f
#define BMI088_WRITE_ACCEL_REG_NUM 6
#define BMI088_WRITE_GYRO_REG_NUM 6
#define BMI088_GYRO_DATA_READY_BIT 0
#define BMI088_ACCEL_DATA_READY_BIT 1
#define BMI088_ACCEL_TEMP_DATA_READY_BIT 2
#define BMI088_LONG_DELAY_TIME 80
#define BMI088_COM_WAIT_SENSOR_TIME 150
#define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
#define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)
#define BMI088_ACCEL_3G_SEN 0.0008974358974f
#define BMI088_ACCEL_6G_SEN 0.00179443359375f
#define BMI088_ACCEL_12G_SEN 0.0035888671875f
#define BMI088_ACCEL_24G_SEN 0.007177734375f
#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
#define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
#define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
// <20><><EFBFBD>ֶ<EFBFBD><D6B6>޸<EFBFBD>
#if INFANTRY_ID == 0
#define GxOFFSET 0.00247530174f
#define GyOFFSET 0.000393082853f
#define GzOFFSET 0.000393082853f
#define gNORM 9.69293118f
#elif INFANTRY_ID == 1
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#elif INFANTRY_ID == 2
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#elif INFANTRY_ID == 3
#define GxOFFSET 0.00270364084f
#define GyOFFSET -0.000532632112f
#define GzOFFSET 0.00478090625f
#define gNORM 9.73574924f
#elif INFANTRY_ID == 4
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#endif
typedef struct
{
float Accel[3];
float Gyro[3];
float TempWhenCali;
float Temperature;
float AccelScale;
float GyroOffset[3];
float gNorm;
} IMU_Data_t;
enum
{
BMI088_NO_ERROR = 0x00,
BMI088_ACC_PWR_CTRL_ERROR = 0x01,
BMI088_ACC_PWR_CONF_ERROR = 0x02,
BMI088_ACC_CONF_ERROR = 0x03,
BMI088_ACC_SELF_TEST_ERROR = 0x04,
BMI088_ACC_RANGE_ERROR = 0x05,
BMI088_INT1_IO_CTRL_ERROR = 0x06,
BMI088_INT_MAP_DATA_ERROR = 0x07,
BMI088_GYRO_RANGE_ERROR = 0x08,
BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
BMI088_GYRO_LPM1_ERROR = 0x0A,
BMI088_GYRO_CTRL_ERROR = 0x0B,
BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,
BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
BMI088_SELF_TEST_GYRO_ERROR = 0x40,
BMI088_NO_SENSOR = 0xFF,
};
void BMI088_Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
extern uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
extern uint8_t bmi088_accel_init(void);
extern uint8_t bmi088_gyro_init(void);
extern IMU_Data_t BMI088;
extern void BMI088_Read(IMU_Data_t *bmi088);
#endif

180
modules/imu/BMI088reg.h Normal file
View File

@@ -0,0 +1,180 @@
#ifndef BMI088REG_H
#define BMI088REG_H
#define BMI088_ACC_CHIP_ID 0x00 // the register is " Who am I "
#define BMI088_ACC_CHIP_ID_VALUE 0x1E
#define BMI088_ACC_ERR_REG 0x02
#define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
#define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
#define BMI088_FATAL_ERROR_SHFITS 0x0
#define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)
#define BMI088_ACC_STATUS 0x03
#define BMI088_ACCEL_DRDY_SHFITS 0x7
#define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)
#define BMI088_ACCEL_XOUT_L 0x12
#define BMI088_ACCEL_XOUT_M 0x13
#define BMI088_ACCEL_YOUT_L 0x14
#define BMI088_ACCEL_YOUT_M 0x15
#define BMI088_ACCEL_ZOUT_L 0x16
#define BMI088_ACCEL_ZOUT_M 0x17
#define BMI088_SENSORTIME_DATA_L 0x18
#define BMI088_SENSORTIME_DATA_M 0x19
#define BMI088_SENSORTIME_DATA_H 0x1A
#define BMI088_ACC_INT_STAT_1 0x1D
#define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
#define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)
#define BMI088_TEMP_M 0x22
#define BMI088_TEMP_L 0x23
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_CONF_MUST_Set 0x80
#define BMI088_ACC_BWP_SHFITS 0x4
#define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_ODR_SHFITS 0x0
#define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_RANGE 0x41
#define BMI088_ACC_RANGE_SHFITS 0x0
#define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_INT1_IO_CTRL 0x53
#define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_INT2_IO_CTRL 0x54
#define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_INT_MAP_DATA 0x58
#define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
#define BMI088_ACC_INT2_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
#define BMI088_ACC_INT1_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_SELF_TEST_OFF 0x00
#define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
#define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_SUSPEND_MODE 0x03
#define BMI088_ACC_PWR_ACTIVE_MODE 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_ENABLE_ACC_OFF 0x00
#define BMI088_ACC_ENABLE_ACC_ON 0x04
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_CHIP_ID_VALUE 0x0F
#define BMI088_GYRO_X_L 0x02
#define BMI088_GYRO_X_H 0x03
#define BMI088_GYRO_Y_L 0x04
#define BMI088_GYRO_Y_H 0x05
#define BMI088_GYRO_Z_L 0x06
#define BMI088_GYRO_Z_H 0x07
#define BMI088_GYRO_INT_STAT_1 0x0A
#define BMI088_GYRO_DYDR_SHFITS 0x7
#define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_RANGE_SHFITS 0x0
#define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_BANDWIDTH 0x10
// the first num means Output data rate, the second num means bandwidth
#define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
#define BMI088_GYRO_2000_532_HZ 0x00
#define BMI088_GYRO_2000_230_HZ 0x01
#define BMI088_GYRO_1000_116_HZ 0x02
#define BMI088_GYRO_400_47_HZ 0x03
#define BMI088_GYRO_200_23_HZ 0x04
#define BMI088_GYRO_100_12_HZ 0x05
#define BMI088_GYRO_200_64_HZ 0x06
#define BMI088_GYRO_100_32_HZ 0x07
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_NORMAL_MODE 0x00
#define BMI088_GYRO_SUSPEND_MODE 0x80
#define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CTRL 0x15
#define BMI088_DRDY_OFF 0x00
#define BMI088_DRDY_ON 0x80
#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
#define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
#define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
#define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
#define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
#define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_DRDY_IO_OFF 0x00
#define BMI088_GYRO_DRDY_IO_INT3 0x01
#define BMI088_GYRO_DRDY_IO_INT4 0x80
#define BMI088_GYRO_DRDY_IO_BOTH (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)
#define BMI088_GYRO_SELF_TEST 0x3C
#define BMI088_GYRO_RATE_OK_SHFITS 0x4
#define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
#define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
#define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
#define BMI088_GYRO_BIST_RDY_SHFITS 0x1
#define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
#define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
#define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)
#endif

296
modules/imu/ins_task.c Normal file
View File

@@ -0,0 +1,296 @@
/**
******************************************************************************
* @file ins_task.c
* @author Wang Hongxi
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "ins_task.h"
#include "controller.h"
#include "QuaternionEKF.h"
#include "bsp_temperature.h"
#include "spi.h"
INS_t INS;
IMU_Param_t IMU_Param;
PID_t TempCtrl = {0};
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
uint8_t ins_debug_mode = 0;
float RefTemp = 40;
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
void INS_Init(void)
{
// while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR);
IMU_Param.scale[X] = 1;
IMU_Param.scale[Y] = 1;
IMU_Param.scale[Z] = 1;
IMU_Param.Yaw = 0;
IMU_Param.Pitch = 0;
IMU_Param.Roll = 0;
IMU_Param.flag = 1;
IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
// imu heat init
PID_Init(&TempCtrl, 2000, 300, 0, 1000, 20, 0, 0, 0, 0, 0, 0, 0);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
}
void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
// ins update
if ((count % 1) == 0)
{
BMI088_Read(&BMI088);
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
// 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
// 核心函数,EKF更新四元数
IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
// 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
BodyFrameToEarthFrame(xb, INS.xn, INS.q);
BodyFrameToEarthFrame(yb, INS.yn, INS.q);
BodyFrameToEarthFrame(zb, INS.zn, INS.q);
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
float gravity_b[3];
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
{
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
// 获取最终数据
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
}
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count % 1000) == 0)
{
// 200hz
}
count++;
}
/**
* @brief Transform 3dvector from BodyFrame to EarthFrame
* @param[1] vector in BodyFrame
* @param[2] vector in EarthFrame
* @param[3] quaternion
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
{
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
(q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
(q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
}
/**
* @brief Transform 3dvector from EarthFrame to BodyFrame
* @param[1] vector in EarthFrame
* @param[2] vector in BodyFrame
* @param[3] quaternion
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
{
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
(q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
(q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
}
/**
* @brief reserved.用于修正IMU安装误差与标度因数误差,即陀螺仪轴和云台轴的安装偏移
*
*
* @param param IMU参数
* @param gyro 角速度
* @param accel 加速度
*/
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
{
static float lastYawOffset, lastPitchOffset, lastRollOffset;
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
{
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
sinYaw = arm_sin_f32(param->Yaw / 57.295779513f);
sinPitch = arm_sin_f32(param->Pitch / 57.295779513f);
sinRoll = arm_sin_f32(param->Roll / 57.295779513f);
// 1.yaw(alpha) 2.pitch(beta) 3.roll(gamma)
c_11 = cosYaw * cosRoll + sinYaw * sinPitch * sinRoll;
c_12 = cosPitch * sinYaw;
c_13 = cosYaw * sinRoll - cosRoll * sinYaw * sinPitch;
c_21 = cosYaw * sinPitch * sinRoll - cosRoll * sinYaw;
c_22 = cosYaw * cosPitch;
c_23 = -sinYaw * sinRoll - cosYaw * cosRoll * sinPitch;
c_31 = -cosPitch * sinRoll;
c_32 = sinPitch;
c_33 = cosPitch * cosRoll;
param->flag = 0;
}
float gyro_temp[3];
for (uint8_t i = 0; i < 3; i++)
gyro_temp[i] = gyro[i] * param->scale[i];
gyro[X] = c_11 * gyro_temp[X] +
c_12 * gyro_temp[Y] +
c_13 * gyro_temp[Z];
gyro[Y] = c_21 * gyro_temp[X] +
c_22 * gyro_temp[Y] +
c_23 * gyro_temp[Z];
gyro[Z] = c_31 * gyro_temp[X] +
c_32 * gyro_temp[Y] +
c_33 * gyro_temp[Z];
float accel_temp[3];
for (uint8_t i = 0; i < 3; i++)
accel_temp[i] = accel[i];
accel[X] = c_11 * accel_temp[X] +
c_12 * accel_temp[Y] +
c_13 * accel_temp[Z];
accel[Y] = c_21 * accel_temp[X] +
c_22 * accel_temp[Y] +
c_23 * accel_temp[Z];
accel[Z] = c_31 * accel_temp[X] +
c_32 * accel_temp[Y] +
c_33 * accel_temp[Z];
lastYawOffset = param->Yaw;
lastPitchOffset = param->Pitch;
lastRollOffset = param->Roll;
}
/**
* @brief 温度控制
*
*/
void IMU_Temperature_Ctrl(void)
{
PID_Calculate(&TempCtrl, BMI088.Temperature, RefTemp);
imu_pwm_set(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
//------------------------------------functions below are not used in this demo-------------------------------------------------
//----------------------------------you can read them for learning or programming-----------------------------------------------
//----------------------------------they could also be helpful for further design-----------------------------------------------
/**
* @brief Update quaternion
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
{
float qa, qb, qc;
gx *= 0.5f * dt;
gy *= 0.5f * dt;
gz *= 0.5f * dt;
qa = q[0];
qb = q[1];
qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz);
q[1] += (qa * gx + qc * gz - q[3] * gy);
q[2] += (qa * gy - qb * gz + q[3] * gx);
q[3] += (qa * gz + qb * gy - qc * gx);
}
/**
* @brief Convert quaternion to eular angle
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
{
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
}
/**
* @brief Convert eular angle to quaternion
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
{
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
Yaw /= 57.295779513f;
Pitch /= 57.295779513f;
Roll /= 57.295779513f;
cosPitch = arm_cos_f32(Pitch / 2);
cosYaw = arm_cos_f32(Yaw / 2);
cosRoll = arm_cos_f32(Roll / 2);
sinPitch = arm_sin_f32(Pitch / 2);
sinYaw = arm_sin_f32(Yaw / 2);
sinRoll = arm_sin_f32(Roll / 2);
q[0] = cosPitch * cosRoll * cosYaw + sinPitch * sinRoll * sinYaw;
q[1] = sinPitch * cosRoll * cosYaw - cosPitch * sinRoll * sinYaw;
q[2] = sinPitch * cosRoll * sinYaw + cosPitch * sinRoll * cosYaw;
q[3] = cosPitch * cosRoll * sinYaw - sinPitch * sinRoll * cosYaw;
}

80
modules/imu/ins_task.h Normal file
View File

@@ -0,0 +1,80 @@
/**
******************************************************************************
* @file ins_task.h
* @author Wang Hongxi
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef __INS_TASK_H
#define __INS_TASK_H
#include "stdint.h"
#include "BMI088driver.h"
#include "QuaternionEKF.h"
#define X 0
#define Y 1
#define Z 2
#define INS_TASK_PERIOD 1
typedef struct
{
float q[4]; // 四元数估计值
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
float MotionAccel_b[3]; // 机体坐标加速度
float MotionAccel_n[3]; // 绝对系加速度
float AccelLPF; // 加速度低通滤波系数
// 加速度在绝对系的向量表示
float xn[3];
float yn[3];
float zn[3];
float atanxz;
float atanyz;
// 位姿
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} INS_t;
/**
* @brief 用于修正安装误差的参数,demo中可无视
*
*/
typedef struct
{
uint8_t flag;
float scale[3];
float Yaw;
float Pitch;
float Roll;
} IMU_Param_t;
extern INS_t INS;
void INS_Init(void);
void INS_Task(void);
void IMU_Temperature_Ctrl(void);
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
#endif

80
modules/led_light/led_task.c Normal file
View File

@@ -0,0 +1,80 @@
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file led_trigger_task.c/h
* @brief led RGB show.led RGB灯效。
* @note
* @history
* Version Date Author Modification
* V1.0.0 Nov-11-2019 RM 1. rgb led
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/
#include "led_task.h"
#include "bsp_led.h"
#include "cmsis_os.h"
#include "main.h"
#define RGB_FLOW_COLOR_CHANGE_TIME 1000
#define RGB_FLOW_COLOR_LENGHT 6
//blue-> green(dark)-> red -> blue(dark) -> green(dark) -> red(dark) -> blue
//蓝 -> 绿(灭) -> 红 -> 蓝(灭) -> 绿 -> 红(灭) -> 蓝
uint32_t RGB_flow_color[RGB_FLOW_COLOR_LENGHT + 1] = {0xFF0000FF, 0x0000FF00, 0xFFFF0000, 0x000000FF, 0xFF00FF00, 0x00FF0000, 0xFF0000FF};
/**
* @brief led rgb task
* @param[in] pvParameters: NULL
* @retval none
*/
/**
* @brief led RGB任务
* @param[in] pvParameters: NULL
* @retval none
*/
void led_RGB_flow_task()
{
uint16_t i, j;
fp32 delta_alpha, delta_red, delta_green, delta_blue;
fp32 alpha,red,green,blue;
uint32_t aRGB;
while(1)
{
for(i = 0; i < RGB_FLOW_COLOR_LENGHT; i++)
{
alpha = (RGB_flow_color[i] & 0xFF000000) >> 24;
red = ((RGB_flow_color[i] & 0x00FF0000) >> 16);
green = ((RGB_flow_color[i] & 0x0000FF00) >> 8);
blue = ((RGB_flow_color[i] & 0x000000FF) >> 0);
delta_alpha = (fp32)((RGB_flow_color[i + 1] & 0xFF000000) >> 24) - (fp32)((RGB_flow_color[i] & 0xFF000000) >> 24);
delta_red = (fp32)((RGB_flow_color[i + 1] & 0x00FF0000) >> 16) - (fp32)((RGB_flow_color[i] & 0x00FF0000) >> 16);
delta_green = (fp32)((RGB_flow_color[i + 1] & 0x0000FF00) >> 8) - (fp32)((RGB_flow_color[i] & 0x0000FF00) >> 8);
delta_blue = (fp32)((RGB_flow_color[i + 1] & 0x000000FF) >> 0) - (fp32)((RGB_flow_color[i] & 0x000000FF) >> 0);
delta_alpha /= RGB_FLOW_COLOR_CHANGE_TIME;
delta_red /= RGB_FLOW_COLOR_CHANGE_TIME;
delta_green /= RGB_FLOW_COLOR_CHANGE_TIME;
delta_blue /= RGB_FLOW_COLOR_CHANGE_TIME;
for(j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; j++)
{
alpha += delta_alpha;
red += delta_red;
green += delta_green;
blue += delta_blue;
aRGB = ((uint32_t)(alpha)) << 24 | ((uint32_t)(red)) << 16 | ((uint32_t)(green)) << 8 | ((uint32_t)(blue)) << 0;
aRGB_led_show(aRGB);
osDelay(1);
}
}
}
}

38
modules/led_light/led_task.h Normal file
View File

@@ -0,0 +1,38 @@
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file led_trigger_task.c/h
* @brief led RGB show.led RGB<47><42>Ч<EFBFBD><D0A7>
* @note
* @history
* Version Date Author Modification
* V1.0.0 Nov-11-2019 RM 1. rgb led
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/
#ifndef LED_TRIGGER_TASK_H
#define LED_TRIGGER_TASK_H
#include "struct_typedef.h"
/**
* @brief led rgb task
* @param[in] pvParameters: NULL
* @retval none
*/
/**
* @brief led RGB<47><42><EFBFBD><EFBFBD>
* @param[in] pvParameters: NULL
* @retval none
*/
extern void led_RGB_flow_task();
#endif

1
modules/master_machine/master_process.c Normal file
View File

@@ -0,0 +1 @@
#include "Master_process.h"

8
modules/master_machine/master_process.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef MASTER_PROCESS_H
#define MASTER_PROCESS_H
#include "bsp_usart.h"
#endif // !MASTER_PROCESS_H

55
modules/motor/HT04.c Normal file
View File

@@ -0,0 +1,55 @@
#include "HT04.h"
#include "memory.h"
joint_instance joint_motor_info[HT_MOTOR_CNT];
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
float span = x_max - x_min;
float offset = x_min;
return (uint16_t) ((x-offset)*((float)((1<<bits)-1))/span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
void JointControl(joint_instance* _instance,float current)
{
uint16_t tmp;
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
tmp = float_to_uint(current, T_MIN, T_MAX, 12);
_instance->motor_can_instace.rx_buff[6] = tmp>>8;
_instance->motor_can_instace.rx_buff[7] = tmp&0xff;
CANTransmit(_instance);
}
void SetJointMode(joint_mode cmd,joint_instance* _instance)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7]=(uint8_t)cmd;
memcpy(_instance->motor_can_instace.rx_buff,buf,8*sizeof(uint8_t));
CANTransmit(&_instance->motor_can_instace);
}
void DecodeJoint(can_instance* motor_instance)
{
uint16_t tmp;
for (size_t i = 0; i < HT_MOTOR_CNT; i++)
{
if(&joint_motor_info[i].motor_can_instace==motor_instance)
{
tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
joint_motor_info[i].last_ecd=joint_motor_info[i].ecd;
joint_motor_info[i].ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
joint_motor_info[i].speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
tmp=((motor_instance->rx_buff[4]&0xf)<<8) | motor_instance->rx_buff[5];
joint_motor_info[i].given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
break;
}
}
}

38
modules/motor/HT04.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef HT04_H
#define HT04_H
#include "struct_typedef.h"
#include "bsp_can.h"
#define HT_MOTOR_CNT 4
#define P_MIN -95.5f // Radians
#define P_MAX 95.5f
#define V_MIN -45.0f // Rad/s
#define V_MAX 45.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
typedef struct //HT04
{
float last_ecd;
float ecd;
float speed_rpm;
float given_current;
can_instance motor_can_instace;
} joint_instance;
typedef enum
{
CMD_MOTOR_MODE = 0xfc,
CMD_RESET_MODE = 0xfd,
CMD_ZERO_POSITION = 0xfe
} joint_mode;
void JointControl(joint_instance* _instance,float current);
void SetJointMode(joint_mode cmd,joint_instance* _instance);
void DecodeJoint(can_instance* motor_instance);
#endif // !HT04_H#define HT04_H

48
modules/motor/LK9025.c Normal file
View File

@@ -0,0 +1,48 @@
#include"LK9025.h"
static driven_instance driven_motor_info[2];
void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan)
{
static uint8_t idx;
driven_motor_info[idx].motor_can_instance.can_handle=hcan;
driven_motor_info[idx].motor_can_instance.can_module_callback=DecodeDriven;
driven_motor_info[idx].motor_can_instance.rx_id=rx_id;
driven_motor_info[idx].motor_can_instance.tx_id=motor_id;
CANRegister(&driven_motor_info[idx].motor_can_instance);
}
void DrivenControl(int16_t motor1_current,int16_t motor2_current)
{
// LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX);
// LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX);
driven_motor_info[0].motor_can_instance.tx_buff[0] = motor1_current;
driven_motor_info[0].motor_can_instance.tx_buff[1] = motor1_current>>8;
driven_motor_info[0].motor_can_instance.tx_buff[2] = motor2_current;
driven_motor_info[0].motor_can_instance.tx_buff[3] = motor2_current>>8;
CANTransmit(&driven_motor_info[0].motor_can_instance);
}
void SetDrivenMode(driven_mode cmd,uint16_t motor_id)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
// code goes here ...
// CANTransmit(driven_mode)
}
void DecodeDriven(can_instance* _instance)
{
for (size_t i = 0; i < LK_MOTOR_CNT; i++)
{
if(&driven_motor_info[i].motor_can_instance==_instance)
{
driven_motor_info[i].last_ecd = driven_motor_info[i].ecd;
driven_motor_info[i].ecd = (uint16_t)((_instance->rx_buff[7]<<8) | _instance->rx_buff[6]);
driven_motor_info[i].speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);
driven_motor_info[i].given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);
driven_motor_info[i].temperate = _instance->rx_buff[1];
break;
}
}
}

36
modules/motor/LK9025.h Normal file
View File

@@ -0,0 +1,36 @@
#ifndef LK9025_H
#define LK9025_H
#include "struct_typedef.h"
#include "bsp_can.h"
#define LK_MOTOR_CNT 2
#define I_MIN -2000
#define I_MAX 2000
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
typedef struct //9025
{
uint16_t last_ecd;
uint16_t ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
can_instance motor_can_instance;
} driven_instance;
typedef enum
{
unused = 0,
} driven_mode;
void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan);
void DrivenControl(int16_t motor1_current,int16_t motor2_current);
void SetDrivenMode(driven_mode cmd,uint16_t motor_id);
void DecodeDriven(can_instance* _instance);
#endif // LK9025_H

0
modules/motor/dji_motor.c Normal file
View File

0
modules/motor/dji_motor.h Normal file
View File

0
modules/referee/referee.c Normal file
View File

0
modules/referee/referee.h Normal file
View File

116
modules/remote/remote_control.c Normal file
View File

@@ -0,0 +1,116 @@
#include "remote_control.h"
#include "string.h"
#include "bsp_usart.h"
#define RC_CHANNAL_ERROR_VALUE 700
#define REMOTE_CONTROL_FRAME_SIZE 18u
#define RC_abs(x) (x)>0?(x):(-x)
#define RC_USART_SERVICE_ID 1
//remote control data
static RC_ctrl_t rc_ctrl;
static usart_instance rc_usart_instance;
/**
* @brief remote control protocol resolution
* @param[in] sbus_buf: raw data point
* @param[out] rc_ctrl: remote control data struct point
* @retval none
*/
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
/**
* @brief protocol resolve callback
* this func would be called when usart3 idle interrupt happens
*
*/
static void ReceiveCallback()
{
sbus_to_rc(rc_usart_instance.recv_buff,&rc_ctrl);
}
void RC_init(UART_HandleTypeDef* rc_usart_handle)
{
rc_usart_instance.module_callback=ReceiveCallback;
rc_usart_instance.usart_handle=rc_usart_handle;
rc_usart_instance.recv_buff_size=REMOTE_CONTROL_FRAME_SIZE;
USARTRegister(&rc_usart_instance);
}
const RC_ctrl_t *get_remote_control_point(void)
{
return &rc_ctrl;
}
uint8_t RC_data_is_error(void)
{
if (RC_abs(rc_ctrl.rc.ch[0]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (RC_abs(rc_ctrl.rc.ch[1]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (RC_abs(rc_ctrl.rc.ch[2]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (RC_abs(rc_ctrl.rc.ch[3]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (rc_ctrl.rc.s[0] == 0)
{
goto error;
}
if (rc_ctrl.rc.s[1] == 0)
{
goto error;
}
return 0;
error:
rc_ctrl.rc.ch[0] = 0;
rc_ctrl.rc.ch[1] = 0;
rc_ctrl.rc.ch[2] = 0;
rc_ctrl.rc.ch[3] = 0;
rc_ctrl.rc.ch[4] = 0;
rc_ctrl.rc.s[0] = RC_SW_DOWN;
rc_ctrl.rc.s[1] = RC_SW_DOWN;
rc_ctrl.mouse.x = 0;
rc_ctrl.mouse.y = 0;
rc_ctrl.mouse.z = 0;
rc_ctrl.mouse.press_l = 0;
rc_ctrl.mouse.press_r = 0;
rc_ctrl.key.v = 0;
return 1;
}
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
{
if (sbus_buf == NULL || rc_ctrl == NULL)
{
return;
}
rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0
rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1
rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
(sbus_buf[4] << 10)) &0x07ff;
rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3
rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
rc_ctrl->mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis
rc_ctrl->mouse.y = sbus_buf[8] | (sbus_buf[9] << 8); //!< Mouse Y axis
rc_ctrl->mouse.z = sbus_buf[10] | (sbus_buf[11] << 8); //!< Mouse Z axis
rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value
rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
}

99
modules/remote/remote_control.h Normal file
View File

@@ -0,0 +1,99 @@
/**
****************************(C) COPYRIGHT 2016 DJI****************************
* @file remote_control.c/h
* @brief 遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
* 资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
* 的方式保证热插拔的稳定性。
* @note
* @history
* Version Date Author Modification
* V1.0.0 Dec-26-2018 RM 1. done
* V1.0.0 Nov-11-2019 RM 1. support development board tpye c
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2016 DJI****************************
*/
#ifndef REMOTE_CONTROL_H
#define REMOTE_CONTROL_H
#include "struct_typedef.h"
#include "main.h"
#define RC_CH_VALUE_MIN ((uint16_t)364)
#define RC_CH_VALUE_OFFSET ((uint16_t)1024)
#define RC_CH_VALUE_MAX ((uint16_t)1684)
/* ----------------------- RC Switch Definition----------------------------- */
#define RC_SW_UP ((uint16_t)1)
#define RC_SW_MID ((uint16_t)3)
#define RC_SW_DOWN ((uint16_t)2)
#define switch_is_down(s) (s == RC_SW_DOWN)
#define switch_is_mid(s) (s == RC_SW_MID)
#define switch_is_up(s) (s == RC_SW_UP)
/* ----------------------- PC Key Definition-------------------------------- */
#define KEY_PRESSED_OFFSET_W ((uint16_t)1 << 0)
#define KEY_PRESSED_OFFSET_S ((uint16_t)1 << 1)
#define KEY_PRESSED_OFFSET_A ((uint16_t)1 << 2)
#define KEY_PRESSED_OFFSET_D ((uint16_t)1 << 3)
#define KEY_PRESSED_OFFSET_SHIFT ((uint16_t)1 << 4)
#define KEY_PRESSED_OFFSET_CTRL ((uint16_t)1 << 5)
#define KEY_PRESSED_OFFSET_E ((uint16_t)1 << 7)
#define KEY_PRESSED_OFFSET_Q ((uint16_t)1 << 6)
#define KEY_PRESSED_OFFSET_R ((uint16_t)1 << 8)
#define KEY_PRESSED_OFFSET_F ((uint16_t)1 << 9)
#define KEY_PRESSED_OFFSET_G ((uint16_t)1 << 10)
#define KEY_PRESSED_OFFSET_Z ((uint16_t)1 << 11)
#define KEY_PRESSED_OFFSET_X ((uint16_t)1 << 12)
#define KEY_PRESSED_OFFSET_C ((uint16_t)1 << 13)
#define KEY_PRESSED_OFFSET_V ((uint16_t)1 << 14)
#define KEY_PRESSED_OFFSET_B ((uint16_t)1 << 15)
/* ----------------------- Data Struct ------------------------------------- */
typedef __packed struct
{
__packed struct
{
int16_t ch[5];
char s[2];
} rc;
__packed struct
{
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
} mouse;
__packed struct
{
uint16_t v;
} key;
} RC_ctrl_t;
/* ----------------------- Internal Data ----------------------------------- */
/**
* @brief register remote control into usart_serive
*
* @attention remember to assign the correct handler
*
*/
void RC_init(UART_HandleTypeDef* rc_usart_handle);
/**
* @brief Get the remote control point object,then you can access data
*
* @return const RC_ctrl_t*
*/
extern const RC_ctrl_t *get_remote_control_point(void);
/**
* @brief check whether there is an error in rc data
*
* @return uint8_t
*/
extern uint8_t RC_data_is_error(void);
#endif

0
modules/super_cap/super_cap.c Normal file
View File

0
modules/super_cap/super_cap.h Normal file
View File