384 lines
12 KiB
C
384 lines
12 KiB
C
/**
|
||
****************************(C) COPYRIGHT 2019 DJI****************************
|
||
* @file remote_control.c/h
|
||
* @brief 遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
|
||
* 资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
|
||
* 的方式保证热插拔的稳定性。
|
||
* @note 该任务是通过串口中断启动，不是freeRTOS任务
|
||
* @history
|
||
* Version Date Author Modification
|
||
* V1.0.0 Dec-01-2019 RM 1. 完成
|
||
*
|
||
@verbatim
|
||
==============================================================================
|
||
|
||
==============================================================================
|
||
@endverbatim
|
||
****************************(C) COPYRIGHT 2019 DJI****************************
|
||
*/
|
||
|
||
|
||
#include "ledi.h"
|
||
#include "bsp/calc_lib.h"
|
||
#include "bsp/uart.h"
|
||
#include "device.h"
|
||
|
||
extern UART_HandleTypeDef huart3;
|
||
extern DMA_HandleTypeDef hdma_usart3_rx;
|
||
|
||
static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl);
|
||
void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
|
||
|
||
LD_t rc_ctrl;
|
||
uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
|
||
uint8_t remote_ready = 0;//ң¿؆÷±¸ͪ³ɠ
|
||
//´®¿ڤma˫»º³凸³õʼ»¯
|
||
|
||
|
||
void remote_control_init(void)
|
||
{
|
||
RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
|
||
}
|
||
|
||
static uint16_t this_time_rx_len = 0;
|
||
|
||
|
||
void USART3_IRQHandler(void)
|
||
{
|
||
|
||
//have received data
|
||
if(huart3.Instance->SR & UART_FLAG_RXNE)
|
||
{
|
||
//ȧ¹ûʇ½ӊՖжϔý¶Áȡdr¼Ĵ憷ǥÁ㍊ __HAL_UART_CLEAR_FEFLAG(&huart3);
|
||
}
|
||
else if(USART3->SR & UART_FLAG_IDLE)
|
||
{
|
||
//ʹӃǥ³ýpe±ꖾλµĺ¯ʽʇҲΪpe idleµȼ¸¸ö¶϶¼ʇ¿¿ψ¶Áȡsrԙ¶ÁȡdrǥÁ㵄
|
||
|
||
__HAL_UART_CLEAR_PEFLAG(&huart3);
|
||
|
||
if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
|
||
{
|
||
|
||
//current memory buffer used is memory0
|
||
|
||
//disable dma to change dma register
|
||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
|
||
//get received data length, length = set_data_length - remain_length
|
||
this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
|
||
|
||
//reset set_data_length
|
||
hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
|
||
|
||
//change memory0 to memory1
|
||
hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
|
||
|
||
//enable dma
|
||
__HAL_DMA_ENABLE(&hdma_usart3_rx);
|
||
|
||
if(this_time_rx_len ==RC_FRAME_LENGTH)
|
||
{
|
||
|
||
sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
|
||
remote_ready = 1;
|
||
}
|
||
}
|
||
|
||
else
|
||
{
|
||
|
||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
|
||
this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
|
||
|
||
hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
|
||
|
||
//change memory1 to memory0
|
||
DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
|
||
|
||
__HAL_DMA_ENABLE(&hdma_usart3_rx);
|
||
|
||
if(this_time_rx_len ==RC_FRAME_LENGTH)
|
||
{
|
||
|
||
sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
|
||
remote_ready = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
//void UART3IdleCallback(void) {
|
||
// USART3_IRQHandler();
|
||
//}
|
||
void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
||
{
|
||
|
||
//enable the dma transfer for the receiver request
|
||
SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
|
||
|
||
//enable idle interrupt
|
||
__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
|
||
|
||
//disable dma, to change the dma register
|
||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
|
||
while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
|
||
{
|
||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
}
|
||
|
||
hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
|
||
|
||
//memory buffer 1
|
||
hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
|
||
|
||
//momory buffer 2
|
||
hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
|
||
|
||
//data length
|
||
hdma_usart3_rx.Instance->NDTR = dma_buf_num;
|
||
|
||
//enable double memory buffer
|
||
SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
|
||
|
||
|
||
//enable dma
|
||
__HAL_DMA_ENABLE(&hdma_usart3_rx);
|
||
|
||
}
|
||
bool REMOTE_WaitDmaCplt(uint32_t timeout) {
|
||
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
|
||
SIGNAL_DR16_RAW_REDY);
|
||
}
|
||
|
||
static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl)
|
||
{
|
||
if (sbus_buf == NULL || rc_ctrl == NULL)
|
||
{
|
||
return;
|
||
}
|
||
|
||
rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff; //Channel 1
|
||
rc_ctrl->ch[3] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
|
||
rc_ctrl->ch[1] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | //Channel 3
|
||
(sbus_buf[5] << 10)) &0x07ff;
|
||
rc_ctrl->ch[2] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
|
||
|
||
rc_ctrl->sw[6] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF; //Channel 5
|
||
rc_ctrl->sw[0] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF; //Channel 6
|
||
rc_ctrl->sw[5] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;; //Channel 7
|
||
rc_ctrl->sw[4] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF; //Channel 8
|
||
rc_ctrl->sw[3] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF; //Channel 9
|
||
rc_ctrl->sw[2] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF; //Channel 10
|
||
rc_ctrl->sw[1] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF; //Channel 11
|
||
rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF; //Channel 12
|
||
|
||
|
||
|
||
|
||
rc_ctrl->ch[1] = map_fp32(rc_ctrl->ch[1],352,1695,-1,1); //lx
|
||
rc_ctrl->ch[0] = map_fp32(rc_ctrl->ch[0],352,1695,-1,1); //ly
|
||
rc_ctrl->ch[2] = map_fp32(rc_ctrl->ch[2],352,1695,-1,1); //rx
|
||
rc_ctrl->ch[3] = map_fp32(rc_ctrl->ch[3],352,1695,-1,1); //ry
|
||
//ˀǸ(-30,30)
|
||
if(rc_ctrl->ch[0]>-0.01&&rc_ctrl->ch[0]<0.01) rc_ctrl->ch[0]=0;
|
||
if(rc_ctrl->ch[1]>-0.01&&rc_ctrl->ch[1]<=0.01) rc_ctrl->ch[1]=0;
|
||
if(rc_ctrl->ch[2]>-0.01&&rc_ctrl->ch[2]<=0.01) rc_ctrl->ch[2]=0;
|
||
if(rc_ctrl->ch[3]>-0.01&&rc_ctrl->ch[3]<=0.01) rc_ctrl->ch[3]=0;
|
||
//MRobot
|
||
remote_ready = 1;
|
||
}
|
||
|
||
|
||
int8_t LD_HandleOffline( LD_t *ld) {
|
||
if (ld == NULL) return DEVICE_ERR_NULL;
|
||
memset(&ld, 0, sizeof(LD_t));
|
||
return 0;
|
||
|
||
}
|
||
|
||
// #include "ledi.h"
|
||
// #include "bsp/calc_lib.h"
|
||
// #include "bsp/uart.h"
|
||
|
||
// extern UART_HandleTypeDef huart3;
|
||
// extern DMA_HandleTypeDef hdma_usart3_rx;
|
||
|
||
// static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl);
|
||
// void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
|
||
|
||
// LD_t rc_ctrl;
|
||
// uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
|
||
// uint8_t remote_ready = 0;//遥控器准备完成
|
||
// //串口dma双缓冲区初始化
|
||
|
||
|
||
// void remote_control_init(void)
|
||
// {
|
||
// RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
|
||
// }
|
||
|
||
// static uint16_t this_time_rx_len = 0;
|
||
|
||
|
||
// void USART3_IRQHandler(void)
|
||
// {
|
||
|
||
// //have received data
|
||
// if(huart3.Instance->SR & UART_FLAG_RXNE)
|
||
// {
|
||
// //如果是接收中断则通过读取dr寄存器清零
|
||
// __HAL_UART_CLEAR_FEFLAG(&huart3);
|
||
// }
|
||
// else if(USART3->SR & UART_FLAG_IDLE)
|
||
// {
|
||
// //使用清除pe标志位的函数是因为pe idle等几个中断都是靠先读取sr再读取dr清零的
|
||
// __HAL_UART_CLEAR_PEFLAG(&huart3);
|
||
|
||
// if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
|
||
// {
|
||
|
||
// //current memory buffer used is memory0
|
||
|
||
// //disable dma to change dma register
|
||
// __HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
|
||
// //get received data length, length = set_data_length - remain_length
|
||
// this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
|
||
|
||
// //reset set_data_length
|
||
// hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
|
||
|
||
// //change memory0 to memory1
|
||
// hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
|
||
|
||
// //enable dma
|
||
// __HAL_DMA_ENABLE(&hdma_usart3_rx);
|
||
|
||
// if(this_time_rx_len ==RC_FRAME_LENGTH)
|
||
// {
|
||
|
||
// sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
|
||
|
||
// }
|
||
// }
|
||
|
||
// else
|
||
// {
|
||
|
||
// __HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
|
||
// this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
|
||
|
||
// hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
|
||
|
||
// //change memory1 to memory0
|
||
// DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
|
||
|
||
// __HAL_DMA_ENABLE(&hdma_usart3_rx);
|
||
|
||
// if(this_time_rx_len ==RC_FRAME_LENGTH)
|
||
// {
|
||
|
||
// sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
|
||
// remote_ready = 1;
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
// }
|
||
// //void UART3IdleCallback(void) {
|
||
// // USART3_IRQHandler();
|
||
// //}
|
||
// void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
||
// {
|
||
|
||
// //enable the dma transfer for the receiver request
|
||
// SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
|
||
|
||
// //enable idle interrupt
|
||
// __HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
|
||
|
||
// //disable dma, to change the dma register
|
||
// __HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
|
||
// while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
|
||
// {
|
||
// __HAL_DMA_DISABLE(&hdma_usart3_rx);
|
||
// }
|
||
|
||
// hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
|
||
|
||
// //memory buffer 1
|
||
// hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
|
||
|
||
// //momory buffer 2
|
||
// hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
|
||
|
||
// //data length
|
||
// hdma_usart3_rx.Instance->NDTR = dma_buf_num;
|
||
|
||
// //enable double memory buffer
|
||
// SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
|
||
|
||
|
||
// //enable dma
|
||
// __HAL_DMA_ENABLE(&hdma_usart3_rx);
|
||
|
||
// }
|
||
|
||
// static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl)
|
||
// {
|
||
// if (sbus_buf == NULL || rc_ctrl == NULL)
|
||
// {
|
||
// return;
|
||
// }
|
||
|
||
// rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff; //Channel 1
|
||
// rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
|
||
// rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | //Channel 3
|
||
// (sbus_buf[5] << 10)) &0x07ff;
|
||
// rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
|
||
|
||
// rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF; //Channel 5
|
||
// rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF; //Channel 6
|
||
// rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;; //Channel 7
|
||
// rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF; //Channel 8
|
||
// rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF; //Channel 9
|
||
// rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF; //Channel 10
|
||
// rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF; //Channel 11
|
||
// rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF; //Channel 12
|
||
|
||
|
||
// // rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306);
|
||
// // rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);
|
||
// // rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700); //x
|
||
// // rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700); //y
|
||
// // rc_ctrl->ch[2] = map(rc_ctrl->ch[2],200,1800,-700,700); //x
|
||
// // rc_ctrl->ch[3] = map(rc_ctrl->ch[3],-694,693,-700,700); //y
|
||
|
||
// rc_ctrl->ch[1] = -map_fp32(rc_ctrl->ch[1],252,1639,-1,1); //x
|
||
// rc_ctrl->ch[0] = map_fp32(rc_ctrl->ch[0],306,1693,-1,1); //y
|
||
// rc_ctrl->ch[2] = -map_fp32(rc_ctrl->ch[2],74,1422,-1,1);
|
||
// rc_ctrl->ch[3] = map_fp32(rc_ctrl->ch[3],194,1580,-1,1); //x
|
||
// //死区(-30,30)
|
||
// if(rc_ctrl->ch[0]>-0.01&&rc_ctrl->ch[0]<0.01) rc_ctrl->ch[0]=0;
|
||
// if(rc_ctrl->ch[1]>-0.01&&rc_ctrl->ch[1]<=0.01) rc_ctrl->ch[1]=0;
|
||
// if(rc_ctrl->ch[2]>-0.05&&rc_ctrl->ch[2]<=0.05) rc_ctrl->ch[2]=0;
|
||
// if(rc_ctrl->ch[3]>-0.01&&rc_ctrl->ch[3]<=0.01) rc_ctrl->ch[3]=0;
|
||
// //MRobot
|
||
// remote_ready = 1;
|
||
// }
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|