/*
 *     路径选择任务
 *       
 */
#include "user_task.h"
#include "map.h"
 
 #ifdef DEBUG
 Action_POS_t pos_action;
 CMD_t cmd_calc;

#else 

static  Action_POS_t action_pos;
static Chassis_Ctrl_t ctrl;

#endif

void Task_calc(void *argument){
	(void)argument; /* 未使用argument，消除警告 */
	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CALCULATE;
	uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */

 while(1)
  {
	#ifdef DEBUG
    /* 记录任务使用的的栈空闄*/
    task_runtime.stack_water_mark.calc =osThreadGetStackSpace(osThreadGetId());
#endif
		osMessageQueueGet(task_runtime.msgq.cmd.calc,&cmd_calc, NULL, 0); 
		osMessageQueueGet(task_runtime.msgq.action.Calc,&pos_action,NULL,0); 

		block_select(&pos_action,&cmd_calc);	
  	path_select(&cmd_calc);//路径点选择

   tick += delay_tick; /* 计算下一个唤醒时刻 */
   osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */	
	}	
}