97 lines
3.3 KiB
C
97 lines
3.3 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 -------------------------------------------------------- */
|
||
int8_t HeightEstimation_Updata(float accl_z)
|
||
{
|
||
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);
|
||
|
||
accl_z_filtered=LowPassFilter2p_Apply(&accl_z_filter, accl_z);
|
||
accl_z_WithoutGravity = (accl_z_filtered - 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, 30.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;
|
||
|
||
float accl_z_buffer=0;
|
||
float gravity_sum=0;
|
||
while(initializeCounter<INITIALIZE_SAMPLE_COUNT)
|
||
{
|
||
osMessageQueueGet(task_runtime.msgq.heightestimation.accl_z, &accl_z_buffer, NULL, 0);
|
||
gravity_sum+=accl_z_buffer;
|
||
initializeCounter++;
|
||
osDelay(20);
|
||
}
|
||
gravity=gravity_sum/INITIALIZE_SAMPLE_COUNT;
|
||
return 0;
|
||
}
|
||
|
||
int8_t HeightEstimation_GetHeight(float *heightout, float *velocityout, float accl_z)
|
||
{
|
||
HeightEstimation_Updata(accl_z);
|
||
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;
|
||
}
|
||
|