/* Includes ----------------------------------------------------------------- */
#include "component/LIGHTBRIDGE2_cmd.h"

#include <string.h>
#include <math.h>
/* Private define ----------------------------------------------------------- */
#define DEVICE_LIGHTBRIDGE2_CH_VALUE_MIN  364u
#define DEVICE_LIGHTBRIDGE2_CH_VALUE_MID  1024u
#define DEVICE_LIGHTBRIDGE2_CH_VALUE_MAX  1684u
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

int8_t COMP_LIGHTBRIDGE2_CMD_ParseRc(const DEVICE_LIGHTBRIDGE2_t *rc, COMP_LIGHTBRIDGE2_CMD_t *lightbridge2_cmd) {

  if (rc == NULL) return -1;
  if (lightbridge2_cmd == NULL) return -1;

	lightbridge2_cmd->ch_l_x = rc->data.ch_l_x; //遥控器左摇杆X轴
	lightbridge2_cmd->ch_l_y = rc->data.ch_l_y; //遥控器左摇杆Y轴
	lightbridge2_cmd->ch_r_x = rc->data.ch_r_x; //遥控器右摇杆X轴
	lightbridge2_cmd->ch_r_y = rc->data.ch_r_y; //遥控器右摇杆Y轴
	
	//lightbridge2_cmd->chassis_ctrl_vec.x = 0.5f*lightbridge2_cmd->ch_r_x;   //底盘控制量X轴
	//lightbridge2_cmd->chassis_ctrl_vec.y = 0.5f*lightbridge2_cmd->ch_r_y;   //底盘控制量Y轴


	
	/*主要就是一个遥控器检测是否连接*/
	if(rc->online == false||rc->data.key_A==LIGHTBRIDGE2_CMD_SW_ERR) //遥控器离线或A键异常
	lightbridge2_cmd->mode = MODE_CMD_BREAKING_CONTROL;  //失控保护模式
	else if(rc->data.key_E == LIGHTBRIDGE2_CMD_SW_DOWN)lightbridge2_cmd->mode = MODE_CMD_ONLT_GIMBAL; //遥控器E键下档
	else if(rc->data.key_E == LIGHTBRIDGE2_CMD_SW_MID)lightbridge2_cmd->mode = MODE_CMD_FOLLOW;//遥控器E键中档
	else if(rc->data.key_E == LIGHTBRIDGE2_CMD_SW_UP)lightbridge2_cmd->mode = MODE_CMD_SPIN;//遥控器E键上档
	else lightbridge2_cmd->mode = MODE_CMD_BREAKING_CONTROL; //默认模式-失控保护

	lightbridge2_cmd->online = rc->online;
  return 0;
}