59 lines
2.5 KiB
C
59 lines
2.5 KiB
C
/* Includes ----------------------------------------------------------------- */
|
||
#include "at9s_pro_cmd.h"
|
||
|
||
#include <string.h>
|
||
#include <math.h>
|
||
|
||
/* Private define ----------------------------------------------------------- */
|
||
#define DEVICE_AT9S_CH_VALUE_MIN 364u
|
||
#define DEVICE_AT9S_CH_VALUE_MID 1024u
|
||
#define DEVICE_AT9S_CH_VALUE_MAX 1684u
|
||
#define DEAD_AREA 0.05f
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function -------------------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
*
|
||
* @param rc ң<><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
* @param dr16_cmd <20><><EFBFBD><EFBFBD>
|
||
* @param dt_sec <20><><EFBFBD>ν<EFBFBD><CEBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC>
|
||
* @return int8_t 0<><30>Ӧû<D3A6>д<EFBFBD><D0B4><EFBFBD>
|
||
*/
|
||
int8_t COMP_AT9S_CMD_ParseRc(const DEVICE_AT9S_t *rc, COMP_AT9S_CMD_t *at9s_cmd) {
|
||
/* ָ<><D6B8><EFBFBD><EFBFBD> */
|
||
if (rc == NULL) return -1;
|
||
if (at9s_cmd == NULL) return -1;
|
||
|
||
at9s_cmd->ch_l_x = 2.0f*(rc->data.ch_l_x-209.0f)/(1596.0f-209.0f)-1.0f;
|
||
at9s_cmd->ch_l_y = (rc->data.ch_l_y-178.0f)/(1575.0f-198.0f);
|
||
at9s_cmd->ch_r_x = 2.0f*(rc->data.ch_r_x-210.0f)/(1596.0f-210.0f)-1.0f;
|
||
at9s_cmd->ch_r_y = 2.0f*(rc->data.ch_r_y-221.0f)/(1604.0f-217.0f)-1.0f;
|
||
|
||
if(fabs(at9s_cmd->ch_l_x)<=DEAD_AREA)at9s_cmd->ch_l_x = 0;
|
||
if(fabs(at9s_cmd->ch_l_y)<=DEAD_AREA)at9s_cmd->ch_l_y = 0;
|
||
if(fabs(at9s_cmd->ch_r_x)<=DEAD_AREA)at9s_cmd->ch_r_x = 0;
|
||
if(fabs(at9s_cmd->ch_r_y)<=DEAD_AREA)at9s_cmd->ch_r_y= 0;
|
||
|
||
if(fabs(at9s_cmd->ch_l_x)>=1.0f)at9s_cmd->ch_l_x= at9s_cmd->ch_l_x>0?1.0f:-1.0f;
|
||
if(fabs(at9s_cmd->ch_l_y)>=1.0f)at9s_cmd->ch_l_y= at9s_cmd->ch_l_y>0?1.0f:-1.0f;
|
||
if(fabs(at9s_cmd->ch_r_x)>=1.0f)at9s_cmd->ch_r_x= at9s_cmd->ch_r_x>0?1.0f:-1.0f;
|
||
if(fabs(at9s_cmd->ch_r_y)>=1.0f)at9s_cmd->ch_r_y= at9s_cmd->ch_r_y>0?1.0f:-1.0f;
|
||
|
||
if(rc->online == true && rc->data.key_C == AT9S_CMD_SW_UP){at9s_cmd->stop = false;at9s_cmd->lockoutput = false;}
|
||
else if(rc->online == false || rc->data.key_C == AT9S_CMD_SW_MID)at9s_cmd->lockoutput = true;
|
||
else if(rc->online == false || rc->data.key_C == AT9S_CMD_SW_DOWN)at9s_cmd->stop = true;
|
||
else at9s_cmd->stop = true;
|
||
|
||
if(rc->data.ch_l_x==0&&rc->data.ch_l_y==0&&rc->data.ch_r_x==0&&rc->data.ch_r_y==0)at9s_cmd->start=true;
|
||
else at9s_cmd->start=false;
|
||
|
||
at9s_cmd->online = rc->online;
|
||
return 0;
|
||
}
|
||
|
||
|