robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-08-01 21:29:01 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

随便添加一下

Browse Source
This commit is contained in:
Robofish 2025-08-01 03:28:47 +08:00
parent a91f175e9e
commit b36738ecac
8 changed files with 1518 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
bsp/dwt.c Normal file
View File

@ -0,0 +1,122 @@
/**
******************************************************************************
* @file dwt.c
* @author Wang Hongxi
* @version V1.1.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "bsp/dwt.h"
DWT_Time_t SysTime;
static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
static uint32_t CYCCNT_RountCount;
static uint32_t CYCCNT_LAST;
uint64_t CYCCNT64;
static void DWT_CNT_Update(void);
void DWT_Init(uint32_t CPU_Freq_mHz)
{
/* 使能DWT外设 */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
/* DWT CYCCNT寄存器计数清0 */
DWT->CYCCNT = (uint32_t)0u;
/* 使能Cortex-M DWT CYCCNT寄存器 */
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
CPU_FREQ_Hz = CPU_Freq_mHz * 1000000;
CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000;
CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000;
CYCCNT_RountCount = 0;
}
float DWT_GetDeltaT(uint32_t *cnt_last)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
float dt = ((uint32_t)(cnt_now - *cnt_last)) / ((float)(CPU_FREQ_Hz));
*cnt_last = cnt_now;
DWT_CNT_Update();
return dt;
}
double DWT_GetDeltaT64(uint32_t *cnt_last)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
double dt = ((uint32_t)(cnt_now - *cnt_last)) / ((double)(CPU_FREQ_Hz));
*cnt_last = cnt_now;
DWT_CNT_Update();
return dt;
}
void DWT_SysTimeUpdate(void)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3;
DWT_CNT_Update();
CYCCNT64 = (uint64_t)CYCCNT_RountCount * (uint64_t)UINT32_MAX + (uint64_t)cnt_now;
CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz;
CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz;
SysTime.s = CNT_TEMP1;
SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms;
CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms;
SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us;
}
float DWT_GetTimeline_s(void)
{
DWT_SysTimeUpdate();
float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f;
return DWT_Timelinef32;
}
float DWT_GetTimeline_ms(void)
{
DWT_SysTimeUpdate();
float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f;
return DWT_Timelinef32;
}
uint64_t DWT_GetTimeline_us(void)
{
DWT_SysTimeUpdate();
uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us;
return DWT_Timelinef32;
}
static void DWT_CNT_Update(void)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
if (cnt_now < CYCCNT_LAST)
CYCCNT_RountCount++;
CYCCNT_LAST = cnt_now;
}
void DWT_Delay(float Delay)
{
uint32_t tickstart = DWT->CYCCNT;
float wait = Delay;
while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz)
{
}
}

37
bsp/dwt.h Normal file
View File

@ -0,0 +1,37 @@
/**
******************************************************************************
* @file dwt.h
* @author Wang Hongxi
* @version V1.1.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _DWT_H
#define _DWT_H
#include "main.h"
#include "stdint.h"
typedef struct
{
uint32_t s;
uint16_t ms;
uint16_t us;
} DWT_Time_t;
void DWT_Init(uint32_t CPU_Freq_mHz);
float DWT_GetDeltaT(uint32_t *cnt_last);
double DWT_GetDeltaT64(uint32_t *cnt_last);
float DWT_GetTimeline_s(void);
float DWT_GetTimeline_ms(void);
uint64_t DWT_GetTimeline_us(void);
void DWT_Delay(float Delay);
void DWT_SysTimeUpdate(void);
extern DWT_Time_t SysTime;
#endif /* DWT_H_ */

492
component/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
component/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

167
component/ist8310.c Normal file
View File

@ -0,0 +1,167 @@
/*
IST8310 地磁传感器。
*/
/* Includes ----------------------------------------------------------------- */
#include "ist8310.h"
#include <gpio.h>
#include <stdbool.h>
#include <string.h>
#include "bsp\delay.h"
#include "bsp\gpio.h"
#include "bsp\i2c.h"
/* Private define ----------------------------------------------------------- */
#define IST8310_WAI (0x00)
#define IST8310_STAT1 (0x02)
#define IST8310_DATAXL (0x03)
#define IST8310_STAT2 (0x09)
#define IST8310_CNTL1 (0x0A)
#define IST8310_CNTL2 (0x0B)
#define IST8310_STR (0x0C)
#define IST8310_TEMPL (0x1C)
#define IST8310_TEMPH (0x1D)
#define IST8310_AVGCNTL (0x41)
#define IST8310_PDCNTL (0x42)
#define IST8310_CHIP_ID (0x10)
#define IST8310_IIC_ADDRESS (0x0E << 1)
#define IST8310_LEN_RX_BUFF (6)
/* Private macro ------------------------------------------------------------ */
#define IST8310_SET() \
HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_SET)
#define IST8310_RESET() \
HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_RESET)
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
uint8_t ist8310_rxbuf[IST8310_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void IST8310_WriteSingle(uint8_t reg, uint8_t data) {
HAL_I2C_Mem_Write(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg,
I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
}
static uint8_t IST8310_ReadSingle(uint8_t reg) {
uint8_t buf = 0;
HAL_I2C_Mem_Read(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg,
I2C_MEMADD_SIZE_8BIT, &buf, 1, 100);
return buf;
}
static void IST8310_Read(uint8_t reg, uint8_t *data, uint8_t len) {
if (data == NULL) return;
HAL_I2C_Mem_Read_DMA(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS,
reg, I2C_MEMADD_SIZE_8BIT, data, len);
}
static void IST8310_MemRxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_RAW_REDY);
}
static void IST8310_IntCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_NEW_DATA);
}
/* Exported functions ------------------------------------------------------- */
int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) {
if (ist8310 == NULL) return DEVICE_ERR_NULL;
if (cali == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
ist8310->cali = cali;
IST8310_RESET();
BSP_Delay(50);
IST8310_SET();
BSP_Delay(50);
if (IST8310_ReadSingle(IST8310_WAI) != IST8310_CHIP_ID)
return DEVICE_ERR_NO_DEV;
BSP_GPIO_DisableIRQ(CMPS_INT_Pin);
BSP_I2C_RegisterCallback(BSP_I2C_COMP, HAL_I2C_MEM_RX_CPLT_CB,
IST8310_MemRxCpltCallback);
BSP_GPIO_RegisterCallback(CMPS_INT_Pin, IST8310_IntCallback);
/* Init. */
/* 0x00: Stand-By mode. 0x01: Single measurement mode. */
/* 0x08: Data ready function enable. DRDY signal active low*/
IST8310_WriteSingle(IST8310_CNTL2, 0x08);
IST8310_WriteSingle(IST8310_AVGCNTL, 0x09);
IST8310_WriteSingle(IST8310_PDCNTL, 0xC0);
IST8310_WriteSingle(IST8310_CNTL1, 0x0B);
BSP_Delay(10);
inited = true;
BSP_GPIO_EnableIRQ(CMPS_INT_Pin);
return DEVICE_OK;
}
bool IST8310_WaitNew(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_IST8310_MAGN_NEW_DATA, osFlagsWaitAll,
timeout) == SIGNAL_IST8310_MAGN_NEW_DATA);
}
int8_t IST8310_StartDmaRecv() {
IST8310_Read(IST8310_DATAXL, ist8310_rxbuf, IST8310_LEN_RX_BUFF);
return DEVICE_OK;
}
uint32_t IST8310_WaitDmaCplt() {
return osThreadFlagsWait(SIGNAL_IST8310_MAGN_RAW_REDY, osFlagsWaitAll,
osWaitForever);
}
int8_t IST8310_Parse(IST8310_t *ist8310) {
if (ist8310 == NULL) return DEVICE_ERR_NULL;
#if 1
/* Magn -> T */
int16_t raw_x, raw_y, raw_z;
memcpy(&raw_x, ist8310_rxbuf + 0, sizeof(raw_x));
memcpy(&raw_y, ist8310_rxbuf + 2, sizeof(raw_y));
memcpy(&raw_z, ist8310_rxbuf + 4, sizeof(raw_z));
ist8310->magn.x = (float)raw_x;
ist8310->magn.y = (float)raw_y;
ist8310->magn.z = (float)-raw_z;
#else
const int16_t *raw_x = (int16_t *)(ist8310_rxbuf + 0);
const int16_t *raw_y = (int16_t *)(ist8310_rxbuf + 2);
const int16_t *raw_z = (int16_t *)(ist8310_rxbuf + 4);
ist8310->magn.x = (float)*raw_x;
ist8310->magn.y = (float)*raw_y;
ist8310->magn.z = -(float)*raw_z;
#endif
ist8310->magn.x *= 3.0f / 20.0f;
ist8310->magn.y *= 3.0f / 20.0f;
ist8310->magn.z *= 3.0f / 20.0f;
ist8310->magn.x = (ist8310->magn.x - ist8310->cali->magn_offset.x) *
ist8310->cali->magn_scale.x;
ist8310->magn.y = (ist8310->magn.y - ist8310->cali->magn_offset.y) *
ist8310->cali->magn_scale.y;
ist8310->magn.z = (ist8310->magn.z - ist8310->cali->magn_offset.y) *
ist8310->cali->magn_scale.z;
return DEVICE_OK;
}

48
component/ist8310.h Normal file
View File

@ -0,0 +1,48 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component\ahrs.h"
#include "device\device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
struct {
float x;
float y;
float z;
} magn_offset; /* 磁力计偏置 */
struct {
float x;
float y;
float z;
} magn_scale; /* 磁力计缩放 */
} IST8310_Cali_t; /* IST8310校准数据 */
typedef struct {
AHRS_Magn_t magn;
const IST8310_Cali_t *cali;
} IST8310_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali);
int8_t IST8310_Restart(void);
bool IST8310_WaitNew(uint32_t timeout);
int8_t IST8310_StartDmaRecv();
uint32_t IST8310_WaitDmaCplt();
int8_t IST8310_Parse(IST8310_t *ist8310);
#ifdef __cplusplus
}
#endif

301
component/ui.c Normal file
View File

@ -0,0 +1,301 @@
/*
UI相关命令
*/
#include "component\ui.h"
#include <stdio.h>
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end) {
if (grapic_line == NULL) return -1;
snprintf((char *)grapic_line->name, 2, "%s", name);
grapic_line->layer = layer;
grapic_line->type_op = type_op;
grapic_line->type_ele = 0;
grapic_line->color = color;
grapic_line->width = width;
grapic_line->x_start = x_start;
grapic_line->y_start = y_start;
grapic_line->x_end = x_end;
grapic_line->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end) {
if (grapic_rectangle == NULL) return -1;
snprintf((char *)grapic_rectangle->name, 2, "%s", name);
grapic_rectangle->type_op = type_op;
grapic_rectangle->type_ele = 1;
grapic_rectangle->layer = layer;
grapic_rectangle->color = color;
grapic_rectangle->width = width;
grapic_rectangle->x_start = x_start;
grapic_rectangle->y_start = y_start;
grapic_rectangle->x_end = x_end;
grapic_rectangle->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius) {
if (grapic_cycle == NULL) return -1;
snprintf((char *)grapic_cycle->name, 2, "%s", name);
grapic_cycle->type_op = type_op;
grapic_cycle->layer = layer;
grapic_cycle->type_ele = 2;
grapic_cycle->color = color;
grapic_cycle->width = width;
grapic_cycle->x_start = x_center;
grapic_cycle->y_start = y_center;
grapic_cycle->radius = radius;
return 0;
}
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis) {
if (grapic_oval == NULL) return -1;
snprintf((char *)grapic_oval->name, 2, "%s", name);
grapic_oval->type_op = type_op;
grapic_oval->type_ele = 3;
grapic_oval->layer = layer;
grapic_oval->color = color;
grapic_oval->width = width;
grapic_oval->x_start = x_center;
grapic_oval->y_start = y_center;
grapic_oval->x_end = x_semiaxis;
grapic_oval->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) {
if (grapic_arc == NULL) return -1;
snprintf((char *)grapic_arc->name, 2, "%s", name);
grapic_arc->type_op = type_op;
grapic_arc->type_ele = 4;
grapic_arc->layer = layer;
grapic_arc->color = color;
grapic_arc->angle_start = angle_start;
grapic_arc->angle_end = angle_end;
grapic_arc->width = width;
grapic_arc->x_start = x_center;
grapic_arc->y_start = y_center;
grapic_arc->x_end = x_semiaxis;
grapic_arc->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low) {
if (grapic_floating == NULL) return -1;
snprintf((char *)grapic_floating->name, 2, "%s", name);
grapic_floating->type_op = type_op;
grapic_floating->type_ele = 5;
grapic_floating->layer = layer;
grapic_floating->color = color;
grapic_floating->angle_start = font_size;
grapic_floating->angle_end = digits;
grapic_floating->width = width;
grapic_floating->x_start = x_start;
grapic_floating->y_start = y_start;
grapic_floating->radius = float_high;
grapic_floating->x_end = float_middle;
grapic_floating->y_end = float_low;
return 0;
}
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low) {
if (grapic_integer == NULL) return -1;
snprintf((char *)grapic_integer->name, 2, "%s", name);
grapic_integer->type_op = type_op;
grapic_integer->type_ele = 6;
grapic_integer->layer = layer;
grapic_integer->color = color;
grapic_integer->angle_start = font_size;
grapic_integer->width = width;
grapic_integer->x_start = x_start;
grapic_integer->y_start = y_start;
grapic_integer->radius = int32_t_high;
grapic_integer->x_end = int32_t_middle;
grapic_integer->y_end = int32_t_low;
return 0;
}
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character) {
if (grapic_character == NULL) return -1;
snprintf((char *)grapic_character->grapic.name, 2, "%s", name);
grapic_character->grapic.type_op = type_op;
grapic_character->grapic.type_ele = 7;
grapic_character->grapic.layer = layer;
grapic_character->grapic.color = color;
grapic_character->grapic.angle_start = font_size;
grapic_character->grapic.angle_end = length;
grapic_character->grapic.width = width;
grapic_character->grapic.x_start = x_start;
grapic_character->grapic.y_start = y_start;
snprintf((char *)grapic_character->character, 29, "%s", character);
return 0;
}
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) {
if (del == NULL) return -1;
del->del_operation = opt;
del->layer = layer;
return 0;
}

276
component/ui.h Normal file
View File

@ -0,0 +1,276 @@
/*
UI相关命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include "component\user_math.h"
#define UI_DEL_OPERATION_NOTHING (0)
#define UI_DEL_OPERATION_DEL (1)
#define UI_DEL_OPERATION_DEL_ALL (2)
#define UI_GRAPIC_OPERATION_NOTHING (0)
#define UI_GRAPIC_OPERATION_ADD (1)
#define UI_GRAPIC_OPERATION_REWRITE (2)
#define UI_GRAPIC_OPERATION_DEL (3)
#define UI_GRAPIC_LAYER_CONST (0)
#define UI_GRAPIC_LAYER_AUTOAIM (1)
#define UI_GRAPIC_LAYER_CHASSIS (2)
#define UI_GRAPIC_LAYER_CAP (3)
#define UI_GRAPIC_LAYER_GIMBAL (4)
#define UI_GRAPIC_LAYER_SHOOT (5)
#define UI_GRAPIC_LAYER_CMD (6)
#define UI_DEFAULT_WIDTH (0x01)
#define UI_CHAR_DEFAULT_WIDTH (0x02)
typedef enum {
RED_BLUE,
YELLOW,
GREEN,
ORANGE,
PURPLISH_RED,
PINK,
CYAN,
BLACK,
WHITE
} UI_Color_t;
typedef struct __packed {
uint8_t op;
uint8_t num_layer;
} UI_InterStudent_UIDel_t;
typedef struct __packed {
uint8_t name[3];
uint8_t type_op : 3;
uint8_t type_ele : 3;
uint8_t layer : 4;
uint8_t color : 4;
uint16_t angle_start : 9;
uint16_t angle_end : 9;
uint16_t width : 10;
uint16_t x_start : 11;
uint16_t y_start : 11;
uint16_t radius : 10;
uint16_t x_end : 11;
uint16_t y_end : 11;
} UI_Ele_t;
typedef struct __packed {
UI_Ele_t grapic;
} UI_Drawgrapic_1_t;
typedef struct __packed {
UI_Ele_t grapic[2];
} UI_Drawgrapic_2_t;
typedef struct __packed {
UI_Ele_t grapic[5];
} UI_Drawgrapic_5_t;
typedef struct __packed {
UI_Ele_t grapic[7];
} UI_Drawgrapic_7_t;
typedef struct __packed {
UI_Ele_t grapic;
uint8_t character[30];
} UI_Drawcharacter_t;
typedef struct __packed {
uint8_t del_operation;
uint8_t layer;
} UI_Del_t;
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end);
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end);
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius);
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis);
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis);
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low);
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low);
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character);
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer);
#ifdef __cplusplus
}
#endif