/*
    Task2 Task
    遥控器任务，主要是大疆dr16和乐迪遥控器
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "device/ledi.h"
#include "module/chassis.h"

/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
extern uint8_t remote_ready;
extern LD_t rc_ctrl;
#ifdef DEBUG
DR16_t dr16;
Chassis_CMD_t cmd_chassis;
LD_t ld;

#else
static DR16_t dr16;
static CMD_RC_t cmd_rc;
static LD_t LD;
#endif

/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_Task2(void *argument)
{
  (void)argument; /* 未使用argument，消除警告 */

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

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

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
                                          /* USER CODE INIT BEGIN */
  // DR16_Init(&dr16);
  remote_control_init();

  /* USER CODE INIT END */

  while (1)
  {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */



if(remote_ready)
	  {
		  remote_ready =0;
	  }
	  memcpy(&ld, &rc_ctrl, sizeof(LD_t));
	//  osMessageQueueReset(task_runtime.msgq.dr16_data);
	if( osMessageQueuePut(task_runtime.msgq.dr16_data,&ld, 0, 0) ==osOK ){

	}
switch (ld.sw[0])
{
case 352:
 cmd_chassis.mode = STOP ;
 cmd_chassis.cmd_power_on_safe = 1;
  break;

  case 1024:
  cmd_chassis.mode = RC ;
  cmd_chassis.cmd_power_on_safe =2;
  break;
  case 1695:
  cmd_chassis.mode = LOCK;
  cmd_chassis.cmd_power_on_safe =3;
  break;
default:
cmd_chassis.cmd_power_on_safe=4;
cmd_chassis.mode = STOP ;
  break;
}
switch (ld.sw[1])
{

  case 352:
  cmd_chassis.throttle =1.0f;
  break;
  case 1024:
  cmd_chassis.throttle = 1.5f;
  break;
  case 1695:
  cmd_chassis.throttle = 5.0f;
  break;
}

    cmd_chassis.Vx = ld.ch[3] * 1.0f;
    cmd_chassis.Vy = ld.ch[2] * 1.0f;
    cmd_chassis.Vw = ld.ch[0] * 1.0f;
// osMessageQueueReset(task_runtime.msgq.cmd_chassis);
if (osMessageQueuePut(task_runtime.msgq.cmd_chassis, &cmd_chassis, 0, 0) == osOK){

}


    // /*开启dma接收遥控器数据*/
    // DR16_StartDmaRecv(&dr16);
    // /*上传DR16数据到消息队列*/
    // if (DR16_WaitDmaCplt(20))
    // {
//      DR16_ParseData(&dr16);
//          osMessageQueueReset(task_runtime.msgq.dr16_data);
//    osMessageQueuePut(task_runtime.msgq.dr16_data, &dr16, 0, 0);
    //       switch (dr16.data.sw_l)
    // {
    // case CMD_SW_UP:
    //   cmd_chassis.mode = RC;
    //   cmd_chassis.cmd_power_on_safe=1;
    //   break;
    // case CMD_SW_MID:

    // default:
    //   cmd_chassis.mode = STOP;
    //   cmd_chassis.cmd_power_on_safe=0;
    //   break;
    //  }
    // cmd_chassis.Vx = dr16.data.ch_r_x * 1.0f;
    // cmd_chassis.Vy = dr16.data.ch_r_y * 1.0f;
    // cmd_chassis.Vw = dr16.data.ch_l_x * 1.0f;
    // // cmd_chassis.throttle = dr16.data.ch_l_y * 1.0f;  
    // //dji遥控器不太适合用作油门，先不给，后续如果改用ld遥控器可以使用油门

    // osMessageQueuePut(task_runtime.msgq.cmd_chassis, &cmd_chassis, 0, 0);
    // }
//    else
//    {
//      DR16_Offline(&dr16);
         // 超时处理，发送安全命令
        // cmd_chassis.mode = STOP;
        // cmd_chassis.cmd_power_on_safe=2;
        // cmd_chassis.Vx = 0.0f;
        // cmd_chassis.Vy = 0.0f;
        // cmd_chassis.Vw = 0.0f;
        // cmd_chassis.throttle = 0.0f;
        
        // osMessageQueuePut(task_runtime.msgq.cmd_chassis, &cmd_chassis, 0, 0);
//    }



    


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