/*
  底盘控制任务〄1�7

*/
/* Includes ----------------------------------------------------------------- */

#include "Chassis.h"
#include "user_task.h"
#include "vofa.h"

static CAN_t can;

#ifdef DEBUG

CAN_Output_t out;

Chassis_t chassis;

CMD_t ctrl;

 Action_POS_t action_pos;
#else 

static CAN_Output_t out;

static Chassis_t chassis;

static Chassis_Ctrl_t ctrl;

static  Action_POS_t action_pos;
#endif

/**
 * \brief 底盘任务
 *
 * \param argument 未使甄1�7
 */
void Task_Chassis(void *argument)
{
	
	 (void)argument; /* 未使用argument，消除警告*/
	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CHASSIS;

	Chassis_init(&chassis,&(task_runtime.config.chassis_config->chassis),TASK_FREQ_CHASSIS);

  uint32_t tick = osKernelGetTickCount(); 
	
	while(1)
		{
#ifdef DEBUG
    task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
#endif
			//暂时没发现不同任务同时获取imu数据会出现问题
						
	osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro,NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed6020, &can, NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed5065, &can, NULL, 0);
			
//	osMessageQueueGet(task_runtime.msgq.can.feedback.pitchfed6020, &can, NULL, 0);
	
//	osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed3508, &can, NULL, 0);
			
  osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
	
  osMessageQueueGet(task_runtime.msgq.action.Chassis,&action_pos,NULL,0);
	
	  osKernelLock();
			
     Chassis_UpdateFeedback(&chassis, &can);
      
	  Chassis_Control(&chassis,&ctrl,&out, &action_pos);

    osKernelUnlock();

    osMessageQueueReset(task_runtime.msgq.can.output.chassis6020);
    osMessageQueuePut(task_runtime.msgq.can.output.chassis6020, &out.chassis6020, 0, 0);

    osMessageQueueReset(task_runtime.msgq.can.output.chassis5065);
    osMessageQueuePut(task_runtime.msgq.can.output.chassis5065, &out.chassis5065, 0, 0);
		
//		osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);
//		osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor3508, 0, 0);
		
//		osMessageQueueReset(task_runtime.msgq.can.output.pitch6020);
//		osMessageQueuePut(task_runtime.msgq.can.output.pitch6020, &out.pitch6020, 0, 0);
		
//	    vofa_send[0] = chassis.vofa_send[0];
//    	vofa_send[1] = chassis.vofa_send[1];
//		  vofa_send[2] = chassis.vofa_send[2];
//		  vofa_send[3] = chassis.vofa_send[3];
//	  	vofa_send[4] = chassis.vofa_send[4];
//     	vofa_send[5] = chassis.vofa_send[5];
//		  vofa_send[6] = chassis.vofa_send[6];
//	  	vofa_send[7] = chassis.vofa_send[7];
//		 
//		  vofa_tx_main(vofa_send);	
		
		tick += delay_tick;
    osDelayUntil(tick); 
		 
	  }
	
}