130 lines
3.9 KiB
C
130 lines
3.9 KiB
C
|
|
#include "remote_control.h"
|
|
#include "main.h"
|
|
#include "calc_lib.h"
|
|
|
|
|
|
extern UART_HandleTypeDef huart3;
|
|
extern DMA_HandleTypeDef hdma_usart3_rx;
|
|
|
|
/**
|
|
* @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);
|
|
|
|
//remote control data
|
|
|
|
RC_ctrl_t rc_ctrl;
|
|
|
|
//receive data, 18 bytes one frame, but set 36 bytes
|
|
|
|
static uint8_t sbus_rx_buf[25];
|
|
|
|
/**
|
|
* @brief remote control init
|
|
* @param[in] none
|
|
* @retval none
|
|
*/
|
|
|
|
void remote_control_init(void)
|
|
{
|
|
RC_Init(sbus_rx_buf, SBUS_RX_BUF_NUM);
|
|
}
|
|
/**
|
|
* @brief get remote control data point
|
|
* @param[in] none
|
|
* @retval remote control data point
|
|
*/
|
|
|
|
const RC_ctrl_t *get_remote_control_point(void)
|
|
{
|
|
return &rc_ctrl;
|
|
}
|
|
|
|
|
|
void USART3_IRQHandler1(void)
|
|
{
|
|
if(USART3->SR &(1<<5))
|
|
{
|
|
USART3->SR |= (0<<5);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
* @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)
|
|
{
|
|
if (sbus_buf == NULL || rc_ctrl == NULL)
|
|
{
|
|
return;
|
|
}
|
|
|
|
rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff; //!< Channel 0
|
|
rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //!< Channel 1
|
|
rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | //!< Channel 2
|
|
(sbus_buf[5] << 10)) &0x07ff;
|
|
rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //!< Channel 3
|
|
|
|
rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF; //!< Switch left
|
|
rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF; //!< Switch right
|
|
rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;; //!< Mouse X axis
|
|
rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF; //!< Mouse Y axis
|
|
rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF; //!< Mouse Z axis
|
|
rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF; //!< Mouse Left Is Press ?
|
|
rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF; //!< Mouse Right Is Press ?
|
|
rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF; //!< KeyBoard value
|
|
|
|
rc_ctrl->ch[0] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->ch[1] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->ch[2] -= RC_CH_VALUE_OFFSET;
|
|
rc_ctrl->ch[3] -= RC_CH_VALUE_OFFSET;
|
|
|
|
rc_ctrl->ch[0] += 24; //y(-694,693)
|
|
rc_ctrl->ch[1] = -rc_ctrl->ch[1]-24; //x(-693,694)
|
|
rc_ctrl->ch[2] = -rc_ctrl->ch[2]+764; //m(95,1482)
|
|
rc_ctrl->ch[3] += 24; //w(-694,693)
|
|
|
|
rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700); //x(-700,700)
|
|
rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700); //y(-700,700)
|
|
rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]); //w
|
|
|
|
//(-30,30)
|
|
if(rc_ctrl->ch[0]>-30&&rc_ctrl->ch[0]<30) rc_ctrl->ch[0]=0;
|
|
if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<30) rc_ctrl->ch[1]=0;
|
|
if(rc_ctrl->ch[2]>-30&&rc_ctrl->ch[2]<30) rc_ctrl->ch[2]=0;
|
|
if(rc_ctrl->ch[3]>-30&&rc_ctrl->ch[3]<30) rc_ctrl->ch[3]=0;
|
|
}
|
|
|
|
|
|
void DMA1_Stream1_IRQHandler1(void)
|
|
{
|
|
/* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
|
|
if(DMA1->LISR &(0x800))
|
|
{
|
|
if(sbus_rx_buf[0]!=0x0F)
|
|
{
|
|
RC_restart(SBUS_RX_BUF_NUM);
|
|
}
|
|
else
|
|
{
|
|
sbus_to_rc(sbus_rx_buf,&rc_ctrl);
|
|
}
|
|
|
|
DMA1->LIFCR |= 1<<11;
|
|
}
|
|
}
|
|
|