/*
   底盘控制任务
*/
/* Includes ----------------------------------------------------------------- */

#include "Chassis.h"
#include "user_task.h"
#include "can_use.h"
#include <cmsis_os2.h>
#include "up.h"
#include "vofa.h"

static CAN_t can;



#ifdef DEBUG

CAN_Output_t out;

Chassis_t chassis  ;

CMD_t ctrl;

UP_t upp;
#else 

static CAN_Output_t out;

static Chassis_t chassis;

static Chassis_Ctrl_t ctrl;

static UP_t upp;


#endif



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

  uint32_t tick = osKernelGetTickCount();
//	
//	
////	HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
//	
	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.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
//	/*can上设备数据获取*/		
//	osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
//			
//	osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0);
			
	/*底盘控制信息获取,目前dji遥控器*/		
    osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
			
	/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/		
//	osKernelLock();
	
  /*更新电机反馈	*/	
   Chassis_UpdateFeedback(&chassis, &can);   
      
	 /*底盘控制*/
	Chassis_Control(&chassis,&ctrl,&out); 
	
	 /*解锁*/
//      osKernelUnlock();
	
	tick += delay_tick;
        osDelayUntil(tick);
	  }
	
}