/*
    main_control Task
*/

/* Includes ----------------------------------------------------------------- */
#include "task\user_task.h"
#include "main.h"
#include "module/easy_control.h"
#include "bsp/buzzer_pwm.h"
#include "bsp/dwt.h"
#include "bsp/Delay.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};
PS2_TypeDef 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() / TASK_FREQ_MAIN_CONTROL;

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


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

	FlightController_Init(TASK_FREQ_MAIN_CONTROL);
	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); /* 运行结束，等待下一次唤醒 */
  }
}