ea48b3560b
# Conflicts: # UAV-ControlSystem/src/config/cli.c # UAV-ControlSystem/src/config/eeprom.c # UAV-ControlSystem/src/drivers/sbus.c
319 lines
11 KiB
C
319 lines
11 KiB
C
/**********************************************************************
|
|
* NAME: sbus.c *
|
|
* AUTHOR: Philip Johansson *
|
|
* PURPOSE: Read SBUS messages from RX *
|
|
* INFORMATION: *
|
|
* The SBUS protocol writes most significant bit first. So 1 in binary *
|
|
* which is 0000001 will be sent as 10000000. *
|
|
* Parity: even *
|
|
* Stops: 2 *
|
|
* Start byte: 0x0F (follows 0x00) *
|
|
* Stop byte: 0x00 *
|
|
* Packet size: 25 bytes *
|
|
* *
|
|
* GLOBAL VARIABLES: *
|
|
* Variable Type Description *
|
|
* -------- ---- ----------- *
|
|
* *
|
|
**********************************************************************/
|
|
#include "drivers/sbus.h"
|
|
#include "drivers/usart.h"
|
|
#include "utilities.h"
|
|
#include "string.h"
|
|
#include "drivers/uart1_inverter.h"
|
|
#include "stm32f4xx_revo.h"
|
|
|
|
// Linear fitting
|
|
#define SBUS_UNIT_CONV(channel) ((channel * 0.626) + 880)
|
|
|
|
/* This instance is read by the whole system and should contain actual RX data */
|
|
sbusFrame_s sbusChannelData = {0};
|
|
dma_usart_return raw_dma_data_t;
|
|
dma_usart_return raw_dma_data_t = {0};
|
|
rc_input_t rc_input = {0};
|
|
float rc_rate = 1.0;
|
|
|
|
/* Create a DMA Handler */
|
|
usart_dma_profile dmaHandler;
|
|
|
|
/* TODO: This can be used to keep count of amount of errors before we trigger a failsafe */
|
|
typedef struct rxFailsafeChannelConfiguration_s {
|
|
uint8_t mode;
|
|
uint8_t step;
|
|
} rxFailsafeChannelConfiguration_t;
|
|
|
|
/* TODO: Move to EEPROM or remove or redo differently
|
|
* The thought is to use these values for things as failsafe, RC stick center etc */
|
|
typedef struct rxConfig_s {
|
|
uint8_t rcmap[SBUS_MAX_CHANNEL];
|
|
uint8_t sbus_inversion;
|
|
uint8_t midrc;
|
|
uint8_t deadband;
|
|
uint8_t mincheck;
|
|
uint8_t maxcheck;
|
|
uint8_t rcInterpolation;
|
|
uint8_t rcInterpolationInterval;
|
|
uint8_t max_aux_channel;
|
|
uint16_t airModeActivateThreshold;
|
|
|
|
rxFailsafeChannelConfiguration_t failsafe_channel_configurations[SBUS_MAX_CHANNEL];
|
|
//rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT];
|
|
} rxConfig_t;
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Initializes the UART for sbus *
|
|
* INFORMATION: A DMA Buffer starts storing the bytes received from RX *
|
|
* Sbus Inverter is activated *
|
|
***********************************************************************/
|
|
void sbus_init()
|
|
{
|
|
usart_init_dma(USART1, &dmaHandler, SBUS_BAUDRATE, STOP_BITS_2, PARITY_EVEN, USART1_SBUS_DMA_SIZE, 0);
|
|
uart1_rx_inverter_set(true);
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Checks if new RX packet is available *
|
|
* INFORMATION: Is called by the scheduler *
|
|
***********************************************************************/
|
|
bool sbus_frame_available()
|
|
{
|
|
/* We read data from DMA */
|
|
raw_dma_data_t = usart_get_dma_buffer(&dmaHandler);
|
|
|
|
return raw_dma_data_t.new_data;
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Truncates RX data between [1000;2000] *
|
|
* INFORMATION: Range set by defines *
|
|
***********************************************************************/
|
|
uint16_t rx_truncate(uint16_t rx_channel)
|
|
{
|
|
if (rx_channel < PWM_RANGE_MIN)
|
|
return PWM_RANGE_MIN;
|
|
else if (rx_channel > PWM_RANGE_MAX)
|
|
return PWM_RANGE_MAX;
|
|
else
|
|
return rx_channel;
|
|
}
|
|
|
|
int16_t rc_input_mapping(float channel)
|
|
{
|
|
return (int16_t)((channel-1500)*rc_rate);
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Updates "sbusChannelData" *
|
|
* INFORMATION: Is called by the scheduler *
|
|
***********************************************************************/
|
|
void sbus_read()
|
|
{
|
|
// Holds what we've read so far
|
|
static uint8_t sbus_arr[SBUS_FRAME_SIZE];
|
|
static uint8_t message_it = 0;
|
|
static uint32_t missedMsg = 0;
|
|
static uint8_t message_it_secondary_head = 0;
|
|
static bool new_header = false;
|
|
|
|
static int sbus_arr_iterator = SBUS_FRAME_SIZE;
|
|
static bool stop_bit_read = false;
|
|
|
|
|
|
// If continue only if we get new data from DMA
|
|
if (raw_dma_data_t.new_data)
|
|
{
|
|
|
|
|
|
|
|
for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++)
|
|
{
|
|
uint8_t msg = raw_dma_data_t.buff[i];
|
|
// Look for the beginning of a sbus frame
|
|
if ( message_it == 0 ) //&& stop_bit_read)
|
|
{
|
|
//message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
|
|
if (msg == ((uint8_t)SBUS_HEADER))
|
|
{
|
|
sbus_arr[(message_it)] = msg;
|
|
message_it++;
|
|
new_header = false;
|
|
}
|
|
else
|
|
{
|
|
message_it = 0;
|
|
}
|
|
|
|
// sbus_arr_iterator = 0;
|
|
// stop_bit_read = false;
|
|
}
|
|
// Look for the end of sbus frame
|
|
//else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER)
|
|
else
|
|
{
|
|
if (msg == (uint8_t)SBUS_HEADER && new_header == false)
|
|
{
|
|
new_header = true;
|
|
message_it_secondary_head = message_it; //save the value of the position in The buffer array, not the dma array index
|
|
}
|
|
|
|
if ((message_it) < SBUS_FRAME_SIZE)
|
|
{
|
|
sbus_arr[(message_it)] = msg;
|
|
message_it++;
|
|
}
|
|
|
|
if ((message_it) == SBUS_FRAME_SIZE)
|
|
{
|
|
missedMsg++;
|
|
if (msg == (uint8_t)SBUS_FOOTER)
|
|
{
|
|
message_it = 0;
|
|
missedMsg--;
|
|
//stop_bit_read = true;
|
|
// If the expected byte is stop byte, then we overwrite to the return value.
|
|
//if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
|
|
//{
|
|
sbusChannelData = *(sbusFrame_s*)sbus_arr;
|
|
|
|
// Linear fitting
|
|
sbusChannelData.chan1 = SBUS_UNIT_CONV(sbusChannelData.chan1);
|
|
sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2);
|
|
sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3);
|
|
sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4);
|
|
sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5);
|
|
sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6);
|
|
sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7);
|
|
sbusChannelData.chan8 = SBUS_UNIT_CONV(sbusChannelData.chan8);
|
|
|
|
// TODO: Depending on defines don't process more than necessary
|
|
sbusChannelData.chan9 = SBUS_UNIT_CONV(sbusChannelData.chan9);
|
|
sbusChannelData.chan10 = SBUS_UNIT_CONV(sbusChannelData.chan10);
|
|
sbusChannelData.chan11 = SBUS_UNIT_CONV(sbusChannelData.chan11);
|
|
sbusChannelData.chan12 = SBUS_UNIT_CONV(sbusChannelData.chan12);
|
|
sbusChannelData.chan13 = SBUS_UNIT_CONV(sbusChannelData.chan13);
|
|
sbusChannelData.chan14 = SBUS_UNIT_CONV(sbusChannelData.chan14);
|
|
sbusChannelData.chan15 = SBUS_UNIT_CONV(sbusChannelData.chan15);
|
|
sbusChannelData.chan16 = SBUS_UNIT_CONV(sbusChannelData.chan16);
|
|
|
|
// TODO: Failsafe using defines checking if channels are in range BEFORE we truncate
|
|
sbusChannelData.chan1 = rx_truncate(sbusChannelData.chan1);
|
|
sbusChannelData.chan2 = rx_truncate(sbusChannelData.chan2);
|
|
sbusChannelData.chan3 = rx_truncate(sbusChannelData.chan3);
|
|
sbusChannelData.chan4 = rx_truncate(sbusChannelData.chan4);
|
|
sbusChannelData.chan5 = rx_truncate(sbusChannelData.chan5);
|
|
sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6);
|
|
sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7);
|
|
sbusChannelData.chan8 = rx_truncate(sbusChannelData.chan8);
|
|
|
|
rc_input.Roll = rc_input_mapping((float)sbusChannelData.chan1);
|
|
rc_input.Pitch = rc_input_mapping((float)sbusChannelData.chan2);
|
|
rc_input.Yaw = rc_input_mapping((float)sbusChannelData.chan4);
|
|
rc_input.Throttle = (int16_t)sbusChannelData.chan3;
|
|
}
|
|
else
|
|
{
|
|
int temp_secondaryHeader = message_it_secondary_head;
|
|
message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array
|
|
new_header = false; //set new header to false, this is true if there is another header within the buffer
|
|
|
|
//Move all the remaning messages in the buffer to the start of the buffer
|
|
for (int i = temp_secondaryHeader; i < SBUS_FRAME_SIZE; i++)
|
|
{
|
|
int innerCount = i-temp_secondaryHeader;
|
|
sbus_arr[innerCount] = sbus_arr[i];
|
|
|
|
//check if we find another possible header inside the rest of the buffer and save that
|
|
if (sbus_arr[innerCount] == (uint8_t)SBUS_HEADER && innerCount > 0 && new_header == false )
|
|
{
|
|
new_header = true;
|
|
message_it_secondary_head = innerCount;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
// // Copy next byte into the sbus_arr
|
|
// if (sbus_arr_iterator < SBUS_FRAME_SIZE)
|
|
// sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i];
|
|
// sbus_arr_iterator++;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Give the value of a channel in "sbusChannelData" *
|
|
* INFORMATION: Given a channel id the value of that channel will be *
|
|
* returned. *
|
|
***********************************************************************/
|
|
int getChannelValue(sbusFrame_s frame, int id)
|
|
{
|
|
int toReturn = 0;
|
|
//find the correct channel from the provided value id
|
|
switch (id)
|
|
{
|
|
case 1:
|
|
toReturn = frame.chan1;
|
|
break;
|
|
case 2:
|
|
toReturn = frame.chan2;
|
|
break;
|
|
case 3:
|
|
toReturn = frame.chan3;
|
|
break;
|
|
case 4:
|
|
toReturn = frame.chan4;
|
|
break;
|
|
case 5:
|
|
toReturn = frame.chan5;
|
|
break;
|
|
case 6:
|
|
toReturn = frame.chan6;
|
|
break;
|
|
case 7:
|
|
toReturn = frame.chan7;
|
|
break;
|
|
case 8:
|
|
toReturn = frame.chan8;
|
|
break;
|
|
#if ((STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT) > 8)
|
|
case 9:
|
|
toReturn = frame.chan9;
|
|
break;
|
|
case 10:
|
|
toReturn = frame.chan10;
|
|
break;
|
|
case 11:
|
|
toReturn = frame.chan11;
|
|
break;
|
|
case 12:
|
|
toReturn = frame.chan12;
|
|
break;
|
|
case 13:
|
|
toReturn = frame.chan13;
|
|
break;
|
|
case 14:
|
|
toReturn = frame.chan14;
|
|
break;
|
|
case 15:
|
|
toReturn = frame.chan15;
|
|
break;
|
|
case 16:
|
|
toReturn = frame.chan16;
|
|
break;
|
|
case 17:
|
|
toReturn = frame.flag_DChannel_17;
|
|
break;
|
|
case 18:
|
|
toReturn = frame.flag_DChannel_18;
|
|
break;
|
|
#endif
|
|
default:
|
|
toReturn = 0;
|
|
break;
|
|
}
|
|
return toReturn;
|
|
}
|