117 lines
3.6 KiB
C
117 lines
3.6 KiB
C
#include "remote_control.h"
|
|
#include "string.h"
|
|
#include "bsp_usart.h"
|
|
|
|
#define RC_CHANNAL_ERROR_VALUE 700
|
|
#define REMOTE_CONTROL_FRAME_SIZE 18u
|
|
#define RC_abs(x) (x)>0?(x):(-x)
|
|
#define RC_USART_SERVICE_ID 1
|
|
|
|
//remote control data
|
|
static RC_ctrl_t rc_ctrl;
|
|
static usart_instance rc_usart_instance;
|
|
|
|
/**
|
|
* @brief remote control protocol resolution
|
|
* @param[in] sbus_buf: raw data point
|
|
* @param[out] rc_ctrl: remote control data struct point
|
|
* @retval none
|
|
*/
|
|
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
|
|
|
|
/**
|
|
* @brief protocol resolve callback
|
|
* this func would be called when usart3 idle interrupt happens
|
|
*
|
|
*/
|
|
static void ReceiveCallback()
|
|
{
|
|
sbus_to_rc(rc_usart_instance.recv_buff,&rc_ctrl);
|
|
}
|
|
|
|
void RC_init(UART_HandleTypeDef* rc_usart_handle)
|
|
{
|
|
rc_usart_instance.module_callback=ReceiveCallback;
|
|
rc_usart_instance.usart_handle=rc_usart_handle;
|
|
rc_usart_instance.recv_buff_size=REMOTE_CONTROL_FRAME_SIZE;
|
|
USARTRegister(&rc_usart_instance);
|
|
}
|
|
|
|
const RC_ctrl_t *get_remote_control_point(void)
|
|
{
|
|
return &rc_ctrl;
|
|
}
|
|
|
|
uint8_t RC_data_is_error(void)
|
|
{
|
|
if (RC_abs(rc_ctrl.rc.ch[0]) > RC_CHANNAL_ERROR_VALUE)
|
|
{
|
|
goto error;
|
|
}
|
|
if (RC_abs(rc_ctrl.rc.ch[1]) > RC_CHANNAL_ERROR_VALUE)
|
|
{
|
|
goto error;
|
|
}
|
|
if (RC_abs(rc_ctrl.rc.ch[2]) > RC_CHANNAL_ERROR_VALUE)
|
|
{
|
|
goto error;
|
|
}
|
|
if (RC_abs(rc_ctrl.rc.ch[3]) > RC_CHANNAL_ERROR_VALUE)
|
|
{
|
|
goto error;
|
|
}
|
|
if (rc_ctrl.rc.s[0] == 0)
|
|
{
|
|
goto error;
|
|
}
|
|
if (rc_ctrl.rc.s[1] == 0)
|
|
{
|
|
goto error;
|
|
}
|
|
return 0;
|
|
error:
|
|
rc_ctrl.rc.ch[0] = 0;
|
|
rc_ctrl.rc.ch[1] = 0;
|
|
rc_ctrl.rc.ch[2] = 0;
|
|
rc_ctrl.rc.ch[3] = 0;
|
|
rc_ctrl.rc.ch[4] = 0;
|
|
rc_ctrl.rc.s[0] = RC_SW_DOWN;
|
|
rc_ctrl.rc.s[1] = RC_SW_DOWN;
|
|
rc_ctrl.mouse.x = 0;
|
|
rc_ctrl.mouse.y = 0;
|
|
rc_ctrl.mouse.z = 0;
|
|
rc_ctrl.mouse.press_l = 0;
|
|
rc_ctrl.mouse.press_r = 0;
|
|
rc_ctrl.key.v = 0;
|
|
return 1;
|
|
}
|
|
|
|
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
|
|
{
|
|
if (sbus_buf == NULL || rc_ctrl == NULL)
|
|
{
|
|
return;
|
|
}
|
|
rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0
|
|
rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1
|
|
rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
|
|
(sbus_buf[4] << 10)) &0x07ff;
|
|
rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3
|
|
rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
|
|
rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
|
|
rc_ctrl->mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis
|
|
rc_ctrl->mouse.y = sbus_buf[8] | (sbus_buf[9] << 8); //!< Mouse Y axis
|
|
rc_ctrl->mouse.z = sbus_buf[10] | (sbus_buf[11] << 8); //!< Mouse Z axis
|
|
rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
|
|
rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
|
|
rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value
|
|
rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL
|
|
|
|
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
|
|
}
|
|
|