54 lines
1.7 KiB
C
54 lines
1.7 KiB
C
/*
|
|
height_cal Task
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "task\user_task.h"
|
|
#include "timers.h"
|
|
#include "device/bmi088.h"
|
|
#include "BarometerDriver/spl06.h"
|
|
#include "component/ahrs.h"
|
|
#include "module/HeightEstimation.h"
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
AHRS_Accl_t accl_buffer;
|
|
AHRS_Quaternion_t quaternion_buffer;
|
|
float estimated_velocity;
|
|
float estimated_height;
|
|
/* Private function --------------------------------------------------------- */
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
/**
|
|
* \brief ps2 Task
|
|
*
|
|
* \param argument
|
|
*/
|
|
void Task_height_cal(void *argument) {
|
|
(void)argument;
|
|
|
|
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / HEIGHT_CAL_FREQ;
|
|
|
|
osDelay(HEIGHT_CAL_INIT_DELAY);
|
|
uint32_t tick = osKernelGetTickCount();
|
|
|
|
HeightEstimation_Init(HEIGHT_CAL_FREQ);
|
|
while (1) {
|
|
tick += delay_tick;
|
|
|
|
/*User code begin*/
|
|
osMessageQueueGet(task_runtime.msgq.heightestimation.accl, &accl_buffer, NULL, 0);
|
|
osMessageQueueGet(task_runtime.msgq.heightestimation.quaternion, &quaternion_buffer, NULL, 0);
|
|
HeightEstimation_GetHeight(&estimated_height, &estimated_velocity, accl_buffer, quaternion_buffer);
|
|
|
|
osMessageQueuePut(task_runtime.msgq.quad.alt, &estimated_height, 0, 0);
|
|
osMessageQueuePut(task_runtime.msgq.quad.vel, &estimated_velocity, 0, 0);
|
|
/*User code end*/
|
|
|
|
osDelayUntil(tick);
|
|
}
|
|
}
|