diff --git a/bsp/dwt.c b/bsp/dwt.c new file mode 100644 index 0000000..15e3e51 --- /dev/null +++ b/bsp/dwt.c @@ -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) + { + } +} diff --git a/bsp/dwt.h b/bsp/dwt.h new file mode 100644 index 0000000..817e922 --- /dev/null +++ b/bsp/dwt.h @@ -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_ */ diff --git a/component/QuaternionEKF.c b/component/QuaternionEKF.c new file mode 100644 index 0000000..6c69918 --- /dev/null +++ b/component/QuaternionEKF.c @@ -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; +} diff --git a/component/QuaternionEKF.h b/component/QuaternionEKF.h new file mode 100644 index 0000000..f7b4d7e --- /dev/null +++ b/component/QuaternionEKF.h @@ -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 diff --git a/component/ist8310.c b/component/ist8310.c new file mode 100644 index 0000000..1e2d5aa --- /dev/null +++ b/component/ist8310.c @@ -0,0 +1,167 @@ +/* + IST8310 地磁传感器。 + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "ist8310.h" + +#include +#include +#include + +#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; +} diff --git a/component/ist8310.h b/component/ist8310.h new file mode 100644 index 0000000..b11128e --- /dev/null +++ b/component/ist8310.h @@ -0,0 +1,48 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#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 diff --git a/component/ui.c b/component/ui.c new file mode 100644 index 0000000..4544784 --- /dev/null +++ b/component/ui.c @@ -0,0 +1,301 @@ +/* + UI相关命令 +*/ +#include "component\ui.h" + +#include + +/** + * @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; +} \ No newline at end of file diff --git a/component/ui.h b/component/ui.h new file mode 100644 index 0000000..5580d3e --- /dev/null +++ b/component/ui.h @@ -0,0 +1,276 @@ +/* + UI相关命令 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#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 \ No newline at end of file