69 lines
1.3 KiB
C
69 lines
1.3 KiB
C
#include "user_task.h"
|
|
#include "navi.h"
|
|
#include "map.h"
|
|
|
|
#ifdef DEBUG
|
|
ops_t o;
|
|
Action_POS_t pos_get;
|
|
CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
|
|
|
|
#else
|
|
|
|
static ops_t o;
|
|
static Action_POS_t pos_get;
|
|
static CMD_ACTION_t ops_out;
|
|
|
|
|
|
#endif
|
|
|
|
|
|
void Task_Navi(void *argument){
|
|
|
|
(void)argument;
|
|
|
|
const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;
|
|
|
|
Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
|
|
|
|
|
|
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
|
|
|
while (1)
|
|
{
|
|
#ifdef DEBUG
|
|
/* 记录任务使用的的栈空间 */
|
|
task_runtime.stack_water_mark.action =
|
|
osThreadGetStackSpace(osThreadGetId());
|
|
#endif
|
|
osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
|
|
|
|
osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);
|
|
|
|
osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0);
|
|
|
|
go_path(&o,&ops_out);
|
|
|
|
//将解算后的导航期望运动值放入消息队列供其他任务使用
|
|
osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out);
|
|
osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0);
|
|
|
|
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
|
osDelayUntil(tick);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
|
|
|
|
|