/*
    height_cal Task
*/

/* Includes ----------------------------------------------------------------- */
#include "task\user_task.h"
#include "timers.h"
#include "device/bmi088.h"
#include "component/ahrs.h"
#include "component/fixed_height.h"
#include "bsp/dwt.h"
#include "bsp/buzzer_pwm.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
uint32_t height_cal_cnt_last;
float height_cal_dt;

 AHRS_Accl_t height_cal_accl_buffer;
 
 float initial_gravity;
 float height_cal_accl_without_gravity;

 float initial_baro_height;
 uint32_t baro_height_buffer;
 float baro_height_m;

 AltitudeKalman alt_kf;

 float height_to_send;

 uint8_t height_cal_running_states=0; /* 0: 计算重力加速度, 1: 计算初始气压计高度, 2: 开始融合高度 */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

/**
 * \brief ps2 Task
 *
 * \param argument 
 */
void Task_height_cal(void *argument) {
  (void)argument; 

  
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_HEIGHT_CAL;

 // osDelay(TASK_INIT_DELAY_HEIGHT_CAL); 

	static uint8_t accl_rx_flag = 0;
	static uint8_t baro_rx_flag = 0;
	AltKalman_Init(&alt_kf);
  uint32_t tick = osKernelGetTickCount(); 
  while (1) {
    tick += delay_tick; 
    
    /*User code begin*/
	height_cal_dt = DWT_GetDeltaT(&height_cal_cnt_last);
	  
	switch(height_cal_running_states){
		case 0:
			if(osMessageQueueGet(task_runtime.msgq.accl, &height_cal_accl_buffer, NULL, 0) == osOK){accl_rx_flag++;}
			if(accl_rx_flag<100){
				initial_gravity += height_cal_accl_buffer.z; 
			}else{
				initial_gravity /= 100.0f;
				height_cal_running_states=1;
			}
			break;
		case 1:
			if(osMessageQueueGet(task_runtime.msgq.baro_height, &baro_height_buffer, NULL, 0) == osOK){baro_rx_flag++;}
			if(baro_rx_flag<50){
				initial_baro_height = (float)baro_height_buffer; 
			}else{
				height_cal_running_states=2;
			}
			break;
		case 2:
			BSP_BUZZER_Set(BSP_BUZZER_OFF, BSP_BUZZER_E5, 0.5);
			osMessageQueueGet(task_runtime.msgq.accl, &height_cal_accl_buffer, NULL, 0);
			osMessageQueueGet(task_runtime.msgq.baro_height, &baro_height_buffer, NULL, 0);
			
			if(baro_height_buffer>0&&baro_height_buffer<200000){
			baro_height_m = ((float)baro_height_buffer-initial_baro_height)/1000;			
			height_cal_accl_without_gravity = (height_cal_accl_buffer.z - initial_gravity)*9.80665f; 
			
			AltKalman_Update(&alt_kf, height_cal_accl_without_gravity, baro_height_m, height_cal_dt);
			
			osMessageQueuePut(task_runtime.msgq.fixed_height, &alt_kf.z, 0, 0);
			osMessageQueuePut(task_runtime.msgq.speed_z, &alt_kf.v, 0, 0);
			}else{
//			BSP_BUZZER_Set(BSP_BUZZER_ON, BSP_BUZZER_E5, 0.5);
			}
				
			break;
		default:
			break;
	}
    /*User code end*/

    osDelayUntil(tick); 
  }
}