116 lines
4.0 KiB
C
116 lines
4.0 KiB
C
/* Includes ----------------------------------------------------------------- */
|
||
#include "bsp/time.h"
|
||
#include "BarometerDriver/spl06.h"
|
||
#include "BarometerDriver/user_iic.h"
|
||
|
||
#include "component/AltitudeKalman.h"
|
||
#include "component/filter.h"
|
||
#include "task/user_task.h"
|
||
/* Private defin#define INITIALIZE_SAMPLE_COUNT 100e ----------------------------------------------------------- */
|
||
#define INITIALIZE_SAMPLE_COUNT 100
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private variables -------------------------------------------------------- */
|
||
|
||
uint16_t initializeCounter=0; //初始化采样计数
|
||
|
||
/* 运行时间 */
|
||
uint64_t HeightEstimation_now;
|
||
float HeightEstimation_dt;
|
||
uint64_t HeightEstimation_last;
|
||
/* 气压数据 */
|
||
int32_t pressure=0;
|
||
int32_t temperature=0;
|
||
int32_t height=0; //绝对高度，单位mm
|
||
|
||
int32_t initial_height=0; //初始高度，单位mm
|
||
float relative_height=0; //相对高度，单位m
|
||
LowPassFilter2p_t relative_height_filter;
|
||
float relative_height_filtered;
|
||
/* 加速度数据 */
|
||
LowPassFilter2p_t accl_z_filter;//加速度z轴低通滤波器
|
||
float accl_z_filtered; //低通滤波后的z轴加速度
|
||
|
||
float gravity; //重力加速度
|
||
float accl_z_WithoutGravity; //z轴去g加速度
|
||
/* 卡尔曼滤波器估计高度 */
|
||
AltitudeKalman alt_kf;
|
||
/* Private function -------------------------------------------------------- */
|
||
float remove_gravity_q(float ax, float ay, float az,
|
||
float q0, float q1, float q2, float q3)
|
||
{
|
||
/* Body -> NED 方向余弦矩阵第三行 */
|
||
float C31 = 2.0f*(q1*q3 - q0*q2);
|
||
float C32 = 2.0f*(q2*q3 + q0*q1);
|
||
float C33 = 1.0f - 2.0f*(q1*q1 + q2*q2);
|
||
|
||
/* NED 下 DOWN 方向的原始加速度 */
|
||
float an_z = C31*ax + C32*ay + C33*az;
|
||
|
||
/* 去重力 */
|
||
return an_z;
|
||
}
|
||
|
||
float accel_z_earth;
|
||
int8_t HeightEstimation_Updata(AHRS_Accl_t accl, AHRS_Quaternion_t quaternion)
|
||
{
|
||
SPL06_Update(&temperature, &pressure);
|
||
height = SPL06_CaculateHeight(pressure);
|
||
|
||
relative_height=(float)((height - initial_height)/1000.0f);
|
||
relative_height_filtered=LowPassFilter2p_Apply(&relative_height_filter, relative_height);
|
||
|
||
accel_z_earth = remove_gravity_q(accl.x, accl.y, accl.z,
|
||
quaternion.q0, quaternion.q1, quaternion.q2, quaternion.q3);
|
||
|
||
accel_z_earth=LowPassFilter2p_Apply(&accl_z_filter, accel_z_earth);
|
||
accl_z_WithoutGravity = (accel_z_earth - gravity) * 9.80665f;
|
||
return 0;
|
||
}
|
||
/* Exported functions ------------------------------------------------------- */
|
||
int8_t HeightEstimation_Init(float freq)
|
||
{
|
||
SPL06_Init();
|
||
BSP_TIME_Delay(100);
|
||
LowPassFilter2p_Init(&accl_z_filter, freq, 10.0f);
|
||
LowPassFilter2p_Init(&relative_height_filter, freq, 5.0f);
|
||
AltKalman_Init(&alt_kf);
|
||
|
||
while(initializeCounter<INITIALIZE_SAMPLE_COUNT)
|
||
{
|
||
SPL06_Update(&temperature, &pressure);
|
||
initial_height=SPL06_CaculateHeight(pressure);
|
||
initializeCounter++;
|
||
}
|
||
initializeCounter=0;
|
||
|
||
AHRS_Accl_t accl_z_buffer={0,0,0};
|
||
float gravity_sum=0;
|
||
while(initializeCounter<INITIALIZE_SAMPLE_COUNT)
|
||
{
|
||
osMessageQueueGet(task_runtime.msgq.heightestimation.accl, &accl_z_buffer, NULL, 0);
|
||
gravity_sum+=accl_z_buffer.z;
|
||
initializeCounter++;
|
||
osDelay(20);
|
||
}
|
||
gravity=gravity_sum/INITIALIZE_SAMPLE_COUNT;
|
||
return 0;
|
||
}
|
||
|
||
int8_t HeightEstimation_GetHeight(float *heightout, float *velocityout, AHRS_Accl_t accl, AHRS_Quaternion_t eulr)
|
||
{
|
||
HeightEstimation_Updata(accl,eulr);
|
||
if(pressure==0){
|
||
//错误处理
|
||
}
|
||
HeightEstimation_now =BSP_TIME_Get_us() / 1000000.0f;
|
||
HeightEstimation_dt =(BSP_TIME_Get_us() - HeightEstimation_last) / 1000000.0f;
|
||
HeightEstimation_last=BSP_TIME_Get_us();
|
||
AltKalman_Update(&alt_kf, accl_z_WithoutGravity, relative_height, HeightEstimation_dt);
|
||
|
||
*heightout=alt_kf.z;
|
||
*velocityout=alt_kf.v;
|
||
return 0;
|
||
}
|
||
|