/*
 *   导航任务
 *    
 */

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