72 lines
1.7 KiB
C
72 lines
1.7 KiB
C
/*
|
|
* 导航任务
|
|
*
|
|
*/
|
|
|
|
#include "user_task.h"
|
|
#include "navi.h"
|
|
#include "can_use.h"
|
|
|
|
static CAN_t can;
|
|
|
|
#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;
|
|
|
|
Navi_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
|
|
|
|
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
|
|
|
while (1)
|
|
{
|
|
#ifdef DEBUG
|
|
/* 记录任务使用的的栈空间 */
|
|
task_runtime.stack_water_mark.navi =
|
|
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.action.Navi ,&pos_get,NULL,0);
|
|
|
|
osMessageQueueGet(task_runtime.msgq.can.feedback.sickfed,&can,NULL,0);
|
|
|
|
//sick_mini板
|
|
for (uint8_t i = 0; i < 4; i++) {
|
|
o.sick_distance[i] = can.sicked_mini.sick_distance[i]/10;
|
|
}
|
|
//sick板
|
|
// for (uint8_t i = 0; i < 4; i++) {
|
|
// o.sick_distance[i] = can.sickfed.as_array[i].sick_distance/10;
|
|
// }
|
|
|
|
//计算sick微分函数放在外面实时计算微分
|
|
sick_calculate_d(&o);
|
|
|
|
go_path(&o,&ops_out);
|
|
|
|
//将解算后的导航期望运动值放入消息队列供其他任务使用
|
|
osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9);
|
|
osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9,(&ops_out),0,0);
|
|
|
|
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
|
osDelayUntil(tick);
|
|
}
|
|
}
|
|
|