/*
上层控制任务
*/
/* Includes ----------------------------------------------------------------- */

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

#include "vofa.h"

static  CAN_t can;


#ifdef DEBUG

CAN_Output_t UP_CAN_out;

UP_t UP  ;

CMD_t up_cmd;


#else 

static CAN_Output_t up_can_out;

static UP_t UP;

static CMD_t up_cmd;

#endif
float  aaa=0;
float bbb=0;
float CCC=0;


/**
 * \brief 
 *
 * \param argument 
 */
void Task_up(void *argument)
{
	
	 (void)argument; /* 未使用argument,消除警告*/
	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
	
  uint32_t tick = osKernelGetTickCount();
	up_init(&UP,&(task_runtime.config.chassis_config->up ),TASK_FREQ_UP);
	
	while(1)
		{
#ifdef DEBUG
    task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
#endif
		UP_UpdateFeedback(&UP, &can,&up_cmd) ;
//    GM6020_control(&UP, 100) ;
//			UP_M3508_speed(&UP, 500); 
			
//				UP_angle_control(&UP,0,M2006);
//			
//			
//			VESC_M5065_Control(&UP,  20000);
			
			
//			
//			GO_SendData(&UP, 1,CCC);
//			GO_SendData(&UP, 0,aaa);

      UP_control(&UP,&UP_CAN_out,&up_cmd);
//			UP.motor_target .go_shoot =aaa;
//			UP.motor_target .M2006_angle =bbb ;

//      UP_control(&UP,&UP_CAN_out,&up_cmd);
//			UP.motor_target .M3508_angle =CCC;
//			UP.motor_target .go_shoot =aaa;
//			UP.motor_target .M2006_angle =bbb;
//			UP.motor_target .go_spin  =ddd;
			
			ALL_Motor_Control(&UP,&UP_CAN_out);

		osDelay(1);
			

	/*imu数据获取*/					
	osMessageQueueGet(task_runtime.msgq.imu.eulr, &UP.pos088.imu_eulr, NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.imu.gyro, &UP.pos088.bmi088.gyro,NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
	/*can上设备数据获取*/	
	osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
					
		osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0);
		
	/*锁定RTOS(实时操作系统)内核,防止任务切换,直到 osKernelUnlock() 被调用*/		
	osKernelLock();
	
	 /*解锁*/
      osKernelUnlock();



		osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
		osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &UP_CAN_out.motor3508, 0, 0);  //发送数据
		osMessageQueuePut(task_runtime.msgq.can.output.chassis6020 ,&UP_CAN_out.chassis6020 , 0, 0); 
		osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0); 
	

   vofa_send [0]=UP.vofa_send [0];
   vofa_send [1]=UP.vofa_send [1];
	 vofa_tx_main (vofa_send);
	 
	tick += delay_tick;
        osDelayUntil(tick);
		 
	  }
	
}