/* 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;
}