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

imu test failed

Browse Source
This commit is contained in:
TuxMonkey
2025-11-15 22:18:00 +08:00
parent 14a3cbb955
commit de8a0d5117
24 changed files with 2458 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
Core/Inc/main.h
View File

@@ -61,10 +61,10 @@ void Error_Handler(void);
#define ACC_CS_GPIO_Port GPIOC
#define GYRO_CS_Pin GPIO_PIN_3
#define GYRO_CS_GPIO_Port GPIOC
#define pump1_Pin GPIO_PIN_0
#define pump1_GPIO_Port GPIOA
#define pump2_Pin GPIO_PIN_2
#define pump2_GPIO_Port GPIOA
#define power2_Pin GPIO_PIN_0
#define power2_GPIO_Port GPIOA
#define power1_Pin GPIO_PIN_2
#define power1_GPIO_Port GPIOA
#define pump3_Pin GPIO_PIN_9
#define pump3_GPIO_Port GPIOE
#define ACC_INT_Pin GPIO_PIN_10

6
Core/Src/gpio.c
View File

@@ -62,7 +62,7 @@ void MX_GPIO_Init(void)
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15|GYRO_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOA, pump1_Pin|pump2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA, power2_Pin|power1_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOE, pump3_Pin|pump4_Pin, GPIO_PIN_RESET);
@@ -90,8 +90,8 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : pump1_Pin pump2_Pin */
GPIO_InitStruct.Pin = pump1_Pin|pump2_Pin;
/*Configure GPIO pins : power2_Pin power1_Pin */
GPIO_InitStruct.Pin = power2_Pin|power1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;

19
Core/Src/main.c
View File

@@ -60,11 +60,8 @@
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MPU_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
@@ -177,15 +174,13 @@ void SystemClock_Config(void)
*/
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY))
{
}
while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSI
| RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48|RCC_OSCILLATORTYPE_HSI
|RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSIState = RCC_HSI_DIV1;
RCC_OscInitStruct.HSICalibrationValue = 64;
@@ -207,9 +202,9 @@ void SystemClock_Config(void)
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
| RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2
| RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1;
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
|RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
@@ -229,7 +224,7 @@ void SystemClock_Config(void)
/* USER CODE END 4 */
/* MPU Configuration */
/* MPU Configuration */
void MPU_Config(void)
{

4
TronOneH7_Scaffold.ioc
View File

@@ -552,7 +552,7 @@ NVIC.USART2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.USART3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
PA0.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
PA0.GPIO_Label=pump1
PA0.GPIO_Label=power2
PA0.GPIO_PuPd=GPIO_PULLDOWN
PA0.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA0.Locked=true
@@ -567,7 +567,7 @@ PA12.Locked=true
PA12.Mode=Device_Only_FS
PA12.Signal=USB_OTG_HS_DP
PA2.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
PA2.GPIO_Label=pump2
PA2.GPIO_Label=power1
PA2.GPIO_PuPd=GPIO_PULLDOWN
PA2.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA2.Locked=true

508
User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.c Normal file
View File

@@ -0,0 +1,508 @@
/**
******************************************************************************
* @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 *init_quaternion, 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);
// 姿态初始化
for (int i = 0; i < 4; i++)
{
QEKF_INS.IMU_QuaternionEKF.xhat_data[i] = init_quaternion[i];
}
// 自定义函数初始化,用于扩展或增加kf的基础功能
QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
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;
/* 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]);
// accelInvNorm = invSqrt(1);
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 = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
QEKF_INS.Roll = 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;
// 翻了的话改变一下2.0前面的符号，长短边朝前也记得调整一下=号右边的内容就ok
// 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;
}

78
User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.h Normal file
View File

@@ -0,0 +1,78 @@
/**
******************************************************************************
* @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 *init_quaternion, float process_noise1, float process_noise2, float measure_noise,
float lambda, float lpf);
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
#endif

5
User_Code/module/algorithm/controller/controller.c Normal file
View File

@@ -0,0 +1,5 @@
//
// Created by ASUS on 2025/11/15.
//
#include "controller.h"

8
User_Code/module/algorithm/controller/controller.h Normal file
View File

@@ -0,0 +1,8 @@
//
// Created by ASUS on 2025/11/15.
//
#ifndef TRONONEH7_SCAFFOLD_CONTROLLER_H
#define TRONONEH7_SCAFFOLD_CONTROLLER_H
#endif //TRONONEH7_SCAFFOLD_CONTROLLER_H

5
User_Code/module/algorithm/crc/crc16.c Normal file
View File

@@ -0,0 +1,5 @@
//
// Created by ASUS on 2025/11/15.
//
#include "crc16.h"

8
User_Code/module/algorithm/crc/crc16.h Normal file
View File

@@ -0,0 +1,8 @@
//
// Created by ASUS on 2025/11/15.
//
#ifndef TRONONEH7_SCAFFOLD_CRC16_H
#define TRONONEH7_SCAFFOLD_CRC16_H
#endif //TRONONEH7_SCAFFOLD_CRC16_H

5
User_Code/module/algorithm/crc/crc8.c Normal file
View File

@@ -0,0 +1,5 @@
//
// Created by ASUS on 2025/11/15.
//
#include "crc8.h"

8
User_Code/module/algorithm/crc/crc8.h Normal file
View File

@@ -0,0 +1,8 @@
//
// Created by ASUS on 2025/11/15.
//
#ifndef TRONONEH7_SCAFFOLD_CRC8_H
#define TRONONEH7_SCAFFOLD_CRC8_H
#endif //TRONONEH7_SCAFFOLD_CRC8_H

5
User_Code/module/algorithm/kalman_filter/kalman_filter.c Normal file
View File

@@ -0,0 +1,5 @@
//
// Created by ASUS on 2025/11/15.
//
#include "kalman_filter.h"

8
User_Code/module/algorithm/kalman_filter/kalman_filter.h Normal file
View File

@@ -0,0 +1,8 @@
//
// Created by ASUS on 2025/11/15.
//
#ifndef TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
#define TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
#endif //TRONONEH7_SCAFFOLD_KALMAN_FILTER_H

215
User_Code/module/algorithm/user_lib/user_lib.c Normal file
View File

@@ -0,0 +1,215 @@
/**
******************************************************************************
* @file user_lib.c
* @author Wang Hongxi
* @author modified by neozng
* @version 0.2 beta
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "stdlib.h"
#include "memory.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
void *zmalloc(size_t size)
{
void *ptr = malloc(size);
memset(ptr, 0, size);
return ptr;
}
// 快速开方
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 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;
}
// 限幅函数
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;
}
// 三维向量归一化
float *Norm3d(float *v)
{
float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
v[0] /= len;
v[1] /= len;
v[2] /= len;
return v;
}
// 计算模长
float NormOf3d(float *v)
{
return Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
// 三维向量叉乘v1 x v2
void Cross3d(float *v1, float *v2, float *res)
{
res[0] = v1[1] * v2[2] - v1[2] * v2[1];
res[1] = v1[2] * v2[0] - v1[0] * v2[2];
res[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
// 三维向量点乘
float Dot3d(float *v1, float *v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
// 均值滤波,删除buffer中的最后一个元素,填入新的元素并求平均值
float AverageFilter(float new_data, float *buf, uint8_t len)
{
float sum = 0;
for (uint8_t i = 0; i < len - 1; i++)
{
buf[i] = buf[i + 1];
sum += buf[i];
}
buf[len - 1] = new_data;
sum += new_data;
return sum / len;
}
void MatInit(mat *m, uint8_t row, uint8_t col)
{
m->numCols = col;
m->numRows = row;
m->pData = (float *) zmalloc(row * col * sizeof(float));
}

165
User_Code/module/algorithm/user_lib/user_lib.h Normal file
View File

@@ -0,0 +1,165 @@
/**
******************************************************************************
* @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; //输入数据
float out; //输出数据
float min_value; //限幅最小值
float max_value; //限幅最大值
float frame_period; //时间间隔
} 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;
//快速开方
float Sqrt(float x);
//斜波函数初始化
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
//斜波函数计算
float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
//绝对限制
float abs_limit(float num, float Limit);
//判断符号位
float sign(float value);
//浮点死区
float float_deadband(float Value, float minValue, float maxValue);
// int26死区
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
//限幅函数
float float_constrain(float Value, float minValue, float maxValue);
//限幅函数
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
//循环限幅函数
float loop_float_constrain(float Input, float minValue, float maxValue);
//角度 °限幅 180 ~ -180
float theta_format(float Ang);
int float_rounding(float raw);
//弧度格式化为-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

31
User_Code/module/periph/imu/BMI088Middleware.c Normal file
View File

@@ -0,0 +1,31 @@
#include "BMI088Middleware.h"
#include "main.h"
SPI_HandleTypeDef *BMI088_SPI;
void BMI088_ACCEL_NS_L(void)
{
HAL_GPIO_WritePin(ACC_CS_GPIO_Port, ACC_CS_Pin, GPIO_PIN_RESET);
}
void BMI088_ACCEL_NS_H(void)
{
HAL_GPIO_WritePin(ACC_CS_GPIO_Port, ACC_CS_Pin, GPIO_PIN_SET);
}
void BMI088_GYRO_NS_L(void)
{
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_RESET);
}
void BMI088_GYRO_NS_H(void)
{
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_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, 100); //1000or100?
return rx_data;
}

34
User_Code/module/periph/imu/BMI088Middleware.h Normal file
View File

@@ -0,0 +1,34 @@
#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

416
User_Code/module/periph/imu/BMI088driver.c Normal file
View File

@@ -0,0 +1,416 @@
#include "BMI088driver.h"
#include "BMI088reg.h"
#include "BMI088Middleware.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
#include <math.h>
#pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
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;
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 BMI088_Accel_Init_Table[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_1600_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 BMI088_Gyro_Init_Table[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 BMI088Init(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 = 0.0f, gNormMax = 0.0f, gNormMin = 0.0f;
startTime = DWT_GetTimeline_s();
do
{
if (DWT_GetTimeline_s() - startTime > 12)
{
// <20><>????
bmi088->GyroOffset[0] = GxOFFSET;
bmi088->GyroOffset[1] = GyOFFSET;
bmi088->GyroOffset[2] = GzOFFSET;
bmi088->gNorm = gNORM;
bmi088->TempWhenCali = 40;
LOGERROR("[BMI088] Calibrate Failed! Use offline params");
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.5f || //0.15
gyroDiff[1] > 0.5f || //0.15
gyroDiff[2] > 0.5f) //0.15
{
LOGWARNING("[bmi088] calibration was interrupted\n");
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);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// accel software reset
BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
// HAL_Delay(BMI088_LONG_DELAY_TIME);
HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
// check commiunication is normal after reset
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check the "who am I"
if (res != BMI088_ACC_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 acc chip id");
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(BMI088_Accel_Init_Table[write_reg_num][0],
BMI088_Accel_Init_Table[write_reg_num][1]);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_accel_read_single_reg(BMI088_Accel_Init_Table[write_reg_num][0], res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
if (res != BMI088_Accel_Init_Table[write_reg_num][1])
{
// write_reg_num--;
// return BMI088_Accel_Init_Table[write_reg_num][2];
error |= BMI088_Accel_Init_Table[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(BMI088_LONG_DELAY_TIME);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// reset the gyro sensor
BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
// HAL_Delay(BMI088_LONG_DELAY_TIME);
HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
// check commiunication is normal after reset
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check the "who am I"
if (res != BMI088_GYRO_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
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(BMI088_Gyro_Init_Table[write_reg_num][0],
BMI088_Gyro_Init_Table[write_reg_num][1]);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_gyro_read_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0], res);
HAL_Delay(BMI088_LONG_DELAY_TIME); //但是不知道为什么还是yaw pitch还是nan
if (res != BMI088_Gyro_Init_Table[write_reg_num][1])
{
write_reg_num--;
// return BMI088_Gyro_Init_Table[write_reg_num][2];
error |= BMI088_Accel_Init_Table[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
void BMI088_Read(IMU_Data_t *bmi088)
{
static uint8_t buf[8] = {0};
static int16_t bmi088_raw_temp;
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];
bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1];
bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2];
}
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

133
User_Code/module/periph/imu/BMI088driver.h Normal file
View File

@@ -0,0 +1,133 @@
#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
/* IMU数据结构体 */
typedef struct
{
float Accel[3];
float Gyro[3];
float TempWhenCali;
float Temperature;
float AccelScale;
float GyroOffset[3];
float gNorm;
} IMU_Data_t;
/* BMI088错误码枚举 */
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,
};
extern IMU_Data_t BMI088;
/**
* @brief 初始化BMI088,传入连接的SPI总线handle,以及是否进行在线标定
*
* @param bmi088_SPI handle
* @param calibrate 1为进行在线标定,0使用离线数据
* @return uint8_t 成功则返回BMI088_NO_ERROR
*/
extern uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
/**
* @brief 加速计初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_accel_init(void);
/**
* @brief 陀螺仪初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_gyro_init(void);
/**
* @brief 读取一次BMI088的数据,包括gyro和accel
*
* @param bmi088 传入BMI088实例(结构体)
*/
extern void BMI088_Read(IMU_Data_t *bmi088);
#endif

180
User_Code/module/periph/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

351
User_Code/module/periph/imu/ins_task.c Normal file
View File

@@ -0,0 +1,351 @@
/**
******************************************************************************
* @file ins_task.c
* @author Wang Hongxi
* @author annotation and modificaiton by neozng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "ins_task.h"
#include "controller.h"
#include "QuaternionEKF.h"
#include "spi.h"
#include "tim.h"
#include "user_lib.h"
#include "general_def.h"
// #include "master_process.h"
static INS_t INS;
static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0};
//旋转矩阵
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
static float RefTemp = 40; // 恒温设定温度
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
static void IMUPWMSet(uint16_t pwm)
{
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, pwm);
}
/**
* @brief 温度控制
*
*/
static void IMU_Temperature_Ctrl(void)
{
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
static void InitQuaternion(float *init_q4)
{
float acc_init[3] = {0};
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
float axis_rot[3] = {0}; // 旋转轴
// 读取100次加速度计数据,取平均值作为初始值
for (uint8_t i = 0; i < 100; ++i)
{
BMI088_Read(&BMI088);
acc_init[X] += BMI088.Accel[X];
acc_init[Y] += BMI088.Accel[Y];
acc_init[Z] += BMI088.Accel[Z];
DWT_Delay(0.001);
}
for (uint8_t i = 0; i < 3; ++i)
acc_init[i] /= 100;
Norm3d(acc_init);
// 计算原始加速度矢量和导航系重力加速度矢量的夹角
float angle = acosf(Dot3d(acc_init, gravity_norm));
Cross3d(acc_init, gravity_norm, axis_rot);
Norm3d(axis_rot);
init_q4[0] = cosf(angle / 2.0f);
for (uint8_t i = 0; i < 2; ++i)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
}
attitude_t *INS_Init(void)
{
if (!INS.init)
INS.init = 1;
else
return (attitude_t *) &INS.Gyro;
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
while (BMI088Init(&hspi2, 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;
float init_quaternion[4] = {0};
InitQuaternion(init_quaternion);
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
// imu heat init
PID_Init_Config_s config = {.MaxOut = 2000,
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PIDInit(&TempCtrl, &config);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
DWT_GetDeltaT(&INS_DWT_Count);
return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}
/* 注意以1kHz的频率运行此任务 */
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;
// VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
}
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count++ % 1000) == 0)
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
}
/**
* @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;
}
//------------------------------------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;
}

143
User_Code/module/periph/imu/ins_task.h Normal file
View File

@@ -0,0 +1,143 @@
/**
******************************************************************************
* @file ins_task.h
* @author Wang Hongxi
* @author annotation and modification by NeoZeng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
* 应用层调用初始化函数.
*
******************************************************************************
*/
#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 Gyro[3]; // 角速度
float Accel[3]; // 加速度
// 还需要增加角速度数据
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct
{
float q[4]; // 四元数估计值
float MotionAccel_b[3]; // 机体坐标加速度
float MotionAccel_n[3]; // 绝对系加速度
float AccelLPF; // 加速度低通滤波系数
// bodyframe在绝对系的向量表示
float xn[3];
float yn[3];
float zn[3];
// 加速度在机体系和XY两轴的夹角
// float atanxz;
// float atanyz;
// IMU量测值
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
// 位姿
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
uint8_t init;
} INS_t;
/* 用于修正安装误差的参数 */
typedef struct
{
uint8_t flag;
float scale[3];
float Yaw;
float Pitch;
float Roll;
} IMU_Param_t;
/**
* @brief 初始化惯导解算系统
*
*/
attitude_t *INS_Init(void);
/**
* @brief 此函数放入实时系统中,以1kHz频率运行
* p.s. osDelay(1);
*
*/
void INS_Task(void);
/**
* @brief 四元数更新函数,即实现dq/dt=0.5Ωq
*
* @param q 四元数
* @param gx
* @param gy
* @param gz
* @param dt 距离上次调用的时间间隔
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
/**
* @brief 四元数转换成欧拉角 ZYX
*
* @param q
* @param Yaw
* @param Pitch
* @param Roll
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
/**
* @brief ZYX欧拉角转换为四元数
*
* @param Yaw
* @param Pitch
* @param Roll
* @param q
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
/**
* @brief 机体系到惯性系的变换函数
*
* @param vecBF body frame
* @param vecEF earth frame
* @param q
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
/**
* @brief 惯性系转换到机体系
*
* @param vecEF
* @param vecBF
* @param q
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
#endif

22
ozonedeb/windebnewestux.jdebug.user
View File

@@ -1,5 +1,5 @@
OpenDocument="tasks.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=2297
OpenDocument="tasks.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=2293
OpenDocument="stm32h7xx_hal.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c", Line=329
OpenDocument="list.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Middlewares/Third_Party/FreeRTOS/Source/list.c", Line=144
OpenDocument="cmsis_gcc.h", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Drivers/CMSIS/Include/cmsis_gcc.h", Line=264
@@ -7,25 +7,25 @@ OpenDocument="cmsis_os2.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Midd
OpenDocument="stm32h7xx_it.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Core/Src/stm32h7xx_it.c", Line=99
OpenDocument="delayticks.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/User_Code/bsp/delayticks/delayticks.c", Line=0
OpenDocument="buzzer.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/User_Code/module/periph/buzzer/buzzer.c", Line=191
OpenDocument="main.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Core/Src/main.c", Line=69
OpenDocument="main.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Core/Src/main.c", Line=70
OpenToolbar="Debug", Floating=0, x=0, y=0
OpenToolbar="Breakpoints", Floating=0, x=1, y=0
OpenWindow="Registers 1", DockArea=BOTTOM, x=1, y=0, w=362, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=551, h=273, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Disassembly", DockArea=BOTTOM, x=2, y=0, w=628, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Break & Tracepoints", DockArea=LEFT, x=0, y=1, w=551, h=290, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VectorCatchIndexMask=254
OpenWindow="Break & Tracepoints", DockArea=LEFT, x=0, y=1, w=551, h=288, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VectorCatchIndexMask=254
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=466, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x20005E30
OpenWindow="Global Data", DockArea=RIGHT, x=0, y=2, w=681, h=441, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=2, w=551, h=295, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Functions", DockArea=LEFT, x=0, y=3, w=551, h=214, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Call Graph", DockArea=LEFT, x=0, y=3, w=551, h=214, TabPos=2, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Global Data", DockArea=RIGHT, x=0, y=2, w=681, h=437, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=2, w=551, h=294, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Functions", DockArea=LEFT, x=0, y=3, w=551, h=217, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Call Graph", DockArea=LEFT, x=0, y=3, w=551, h=217, TabPos=2, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=1101, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=681, h=423, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="491;-46", PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="459;-65", CodeGraphLegendShown=1, CodeGraphLegendPosition="475;0"
OpenWindow="Console", DockArea=LEFT, x=0, y=3, w=551, h=214, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="RTOS", DockArea=RIGHT, x=0, y=0, w=681, h=229, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing=""
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=681, h=420, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="491;-46", PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="459;-65", CodeGraphLegendShown=1, CodeGraphLegendPosition="475;0"
OpenWindow="Console", DockArea=LEFT, x=0, y=3, w=551, h=217, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="RTOS", DockArea=RIGHT, x=0, y=0, w=681, h=236, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing=""
TableHeader="Call Graph", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Stack Total";"Stack Local";"Code Total";"Code Local";"Depth";"Called From"], ColWidths=[384;100;100;100;100;100;118]
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1164;100;100;100;100]
TableHeader="Global Data", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Location";"Size";"Type";"Scope"], ColWidths=[222;130;100;54;95;100]
TableHeader="Global Data", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Location";"Size";"Type";"Scope"], ColWidths=[222;130;100;54;95;414]
TableHeader="Vector Catches", SortCol="None", SortOrder="ASCENDING", VisibleCols=["";"Vector Catch";"Description"], ColWidths=[50;300;500]
TableHeader="Break & Tracepoints", SortCol="None", SortOrder="ASCENDING", VisibleCols=["";"Type";"Location";"Extras"], ColWidths=[100;100;142;209]
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[229;100;100;100;902]