104 lines
3.0 KiB
C
104 lines
3.0 KiB
C
/*
|
|
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);
|
|
}
|
|
}
|