#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);
   }

}
	
	
	
	
//	
//	
//	
//	
//	
//	
//