/*
    main_control Task
*/

/* Includes ----------------------------------------------------------------- */
#include "task\user_task.h"
#include "main.h"
#include "module/xm_quadctrl.h"
#include "device/buzzer.h"
#include "bsp/time.h"
#include "bsp/pwm.h"
#include "component/ahrs.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
//uint32_t main_control_cnt_last;
//float main_control_dt;

//SensorData sensors = {0};
// main_control_rc;

//uint8_t main_control_running_states;
/* Private function --------------------------------------------------------- */

/* Exported functions ------------------------------------------------------- */
/**
 * \brief main_control Task
 *
 * \param argument 未使用
 */

//	Control_t ctrl;
void Task_main_control(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / MAIN_CTRL_INIT_DELAY;

  osDelay(MAIN_CTRL_FREQ); /* 延时一段时间再开启任务	*/


  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */

//	FlightController_Init(MAIN_CTRL_INIT_DELAY);
//	BSP_PWM_Start(BSP_PWM_MOTOR_OUT1);
//	BSP_PWM_Start(BSP_PWM_MOTOR_OUT2);
//	BSP_PWM_Start(BSP_PWM_MOTOR_OUT3);
//	BSP_PWM_Start(BSP_PWM_MOTOR_OUT4);
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    
    /*User code begin*/
//    AHRS_Accl_t accl_data;
//    AHRS_Gyro_t gyro_data;
//    float fixed_height_buffer;
//	float speed_z_buffer;  
//    osMessageQueueGet(task_runtime.msgq.accl, &accl_data, NULL, 0);
//    osMessageQueueGet(task_runtime.msgq.gyro, &gyro_data, NULL, 0);
//    osMessageQueueGet(task_runtime.msgq.fixed_height, &fixed_height_buffer, NULL, 0);
//	osMessageQueueGet(task_runtime.msgq.speed_z, &speed_z_buffer, NULL, 0);
//    osMessageQueueGet(task_runtime.msgq.rc, &main_control_rc, NULL, 0);
//            sensors.accel[0] = accl_data.x;
//            sensors.accel[1] = accl_data.y;
//            sensors.accel[2] = accl_data.z;
//            sensors.gyro[0] = gyro_data.x;
//            sensors.gyro[1] = gyro_data.y;
//            sensors.gyro[2] = gyro_data.z;
//            sensors.baro_alt = fixed_height_buffer;
//			sensors.speed_z = speed_z_buffer;
//	  // 获取遥控器输入

//    main_control_dt = DWT_GetDeltaT(&main_control_cnt_last);

//     // 执行控制计算
////        Control_t ctrl;
//        FlightController_Update(sensors, &ctrl, main_control_dt, main_control_rc);
//        float output_check=0;
//      switch(main_control_running_states){
//		  case 0: 
//		output_check = 0.275+0.21*0.8*((main_control_rc.Rocker_LY-0.5)*2);
//		BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, output_check);
//		BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, output_check); 
//		BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, output_check);
//		BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, output_check);
//			if(main_control_rc.Key_R_Up == 1){
//		    main_control_running_states=1;
//			}else if(main_control_rc.Key_R_Right == 1){
//			main_control_running_states=4;
//			}
//			break;
//		  case 1:	
//		  BSP_BUZZER_Set(BSP_BUZZER_ON, BSP_BUZZER_C5, 1);
//		  osDelay(500);
//		  BSP_BUZZER_Stop();
//		  osDelay(2000);
//		  main_control_running_states=2;
//		  if(main_control_rc.Key_R_Right == 1){
//		    main_control_running_states=4;
//			}
//		    break;
//		  case 2:
//			  MotorMixer_Init();
//			  Apply_Motor_Outputs(ctrl);
//		  if(main_control_rc.Key_R_Down == 1){
//		  main_control_running_states=3;
//		  }else if(main_control_rc.Key_R_Right == 1){
//		  main_control_running_states=4;
//		  } 
//			break;
//		  case 3:
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, 0.275);
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, 0.275); 
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, 0.275);
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, 0.275);
//		  main_control_running_states=0;
//		  if(main_control_rc.Key_R_Right == 1){
//		    main_control_running_states=4;
//			}
//			break;
//		  case 4:
//			MotorMixer_OutStop();
//		  break;
//		  default:
//			break;
//	  }
//	  BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, 0.275);
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, 0.275); 
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, 0.275);
//		  BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, 0.275);
    /*User code end*/

    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
}