diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h new file mode 100644 index 0000000..340bff2 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/sbus.h @@ -0,0 +1,131 @@ + /********************************************************************** + * NAME: sbus.h * + * 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 + +#ifndef DRIVERS_SBUS_H_ +#define DRIVERS_SBUS_H_ + +#define SBUS_BAUDRATE 100000 +#define SBUS_FRAME_SIZE 25 +#define SBUS_HEADER 0x0F +#define SBUS_FOOTER 0x00 +#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE + 1 // sbus package is 176 bits (22 bytes) + +#define SBUS_MAX_CHANNEL 18 // Including two digital +#define STICK_CHANNEL_COUNT 4 +#define MAX_AUX_CHANNEL_COUNT (SBUS_MAX_CHANNEL - STICK_CHANNEL_COUNT) + + + +#define PWM_RANGE_MIN 1000 +#define PWM_RANGE_MAX 2000 +#define PWM_RANGE_MIDDLE (PWM_RANGE_MIN + ((PWM_RANGE_MAX - PWM_RANGE_MIN) / 2)) // Should be 1500 default + +#define PWM_PULSE_MIN 750 // minimum PWM pulse considered valid input +#define PWM_PULSE_MAX 2250 // maximum PWM pulse considered valid input + + +/*********************************************************************** +* BRIEF: Failsafe scenarios of the RX * +* INFORMATION: TODO: Implement use of these * +***********************************************************************/ +typedef enum { + RX_FAILSAFE_MODE_AUTO = 0, + RX_FAILSAFE_MODE_HOLD, + RX_FAILSAFE_MODE_SET, + RX_FAILSAFE_MODE_INVALID, +} rxFailsafeChannelMode_e; + + +/*********************************************************************** +* BRIEF: RX packet structure * +* INFORMATION: Contains a whole SBUS message besides the footer * +***********************************************************************/ +typedef struct sbusFrame_s { + /* Whole package is 25 bytes */ + + /* SBUS Header is 0x0F (Byte 0) */ + uint8_t syncByte; + + /* 176 bits of data for channels (11 bits per channel * 16 channels) = 22 bytes (1-22) */ + + /* Channel 1..4 are "sticks" */ + unsigned int chan1 : 11; // Elevator + unsigned int chan2 : 11; // Aileron + unsigned int chan3 : 11; // Throttle + unsigned int chan4 : 11; // Rudder + + /* Channel 5.. are AUX channels */ + unsigned int chan5 : 11; + unsigned int chan6 : 11; + unsigned int chan7 : 11; + unsigned int chan8 : 11; // ARM switch + + /* Depending on define 4 or more channels are used */ + unsigned int chan9 : 11; + unsigned int chan10 : 11; + unsigned int chan11 : 11; + unsigned int chan12 : 11; + unsigned int chan13 : 11; + unsigned int chan14 : 11; + unsigned int chan15 : 11; + unsigned int chan16 : 11; + + /* Byte 23 contains all flags + * Bit 7: digital channel 17 (0x80) + * Bit 6: digital channel 18 (0x40) + * Bit 5: frame lost (0x20) + * Bit 4: failsafe activated (0x10) + * Bit 0-3: n/a + */ + unsigned int flag_DChannel_17 : 1; + unsigned int flag_DChannel_18 : 1; + unsigned int flag_FrameLost : 1; + unsigned int flag_Failsafe : 1; + unsigned int flag_NA : 4; + + /* Byte 24 - The EndByte is 0x00 for FrSky + * Not included in this struct + */ + +} __attribute__ ((__packed__)) sbusFrame_s; + +/* This instance is read by the whole system and should contain actual RX data */ +extern sbusFrame_s sbusChannelData; + +/*********************************************************************** +* BRIEF: Initializes the UART for sbus * +* INFORMATION: A DMA Buffer starts storing the bytes received from RX * +* Sbus Inverter is activated * +***********************************************************************/ +void sbus_init(); + +/*********************************************************************** +* BRIEF: Checks if new RX packet is available * +* INFORMATION: Is called by the scheduler * +***********************************************************************/ +bool sbus_frame_available(); + +/*********************************************************************** +* BRIEF: Updates "sbusChannelData" * +* INFORMATION: Is called by the scheduler * +***********************************************************************/ +void sbus_read(); + +#endif /* DRIVERS_SBUS_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/usart.h b/UAV-ControlSystem/inc/drivers/usart.h index 0448942..4417cdf 100644 --- a/UAV-ControlSystem/inc/drivers/usart.h +++ b/UAV-ControlSystem/inc/drivers/usart.h @@ -23,6 +23,12 @@ #include "stm32f4xx.h" +typedef struct dma_usart_return { + uint8_t* buff; + bool new_data; +} dma_usart_return; + + // Enumeration for USART stop bits typedef enum stop_bits { @@ -33,7 +39,7 @@ typedef enum stop_bits } stop_bits; // Enuymeration for USART parity -typedef enum partiy +typedef enum parity { PARITY_NONE = 0x0, PARITY_EVEN = 0x2, @@ -82,6 +88,7 @@ bool usart_init_dma(USART_TypeDef* usart_inst, * Initialize the specified USART in order to get polling and regular * transmit of messages to work. If the initialization fails this function * returns false and otherwise it returns true +* * For USART1 Inverter is ON on RX by default ***********************************************************************/ bool usart_init(USART_TypeDef* usart_inst, usart_profile* profile_out, @@ -128,7 +135,7 @@ uint32_t usart_poll(usart_profile *profile, * completed so that the DMA can continue writing to the second buffer * without interfering with the rest of the system ***********************************************************************/ -uint8_t* usart_get_dma_buffer(usart_dma_profile *profile); +dma_usart_return usart_get_dma_buffer(usart_dma_profile *profile); /*********************************************************************** * BRIEF: NOT IMPLEMENTED YET diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index a47dab6..a734014 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -102,12 +102,12 @@ /* Led Warnings */ #define USE_LEDS -#define USE_LED_WARNINGS -#define USE_LED_WARNINGS_MISSED_PERIOD +//#define USE_LED_WARNINGS +//#define USE_LED_WARNINGS_MISSED_PERIOD //#define USE_LED_WARNINGS_SYSTEM_LOAD /* Scheduler */ -#define USE_DEBUG_TASKS //Only to be used when testing scheduler, not when intending to run the whole system +//#define USE_DEBUG_TASKS //Only to be used when testing scheduler, not when intending to run the whole system #define USE_TASK_AGE_CYCLE_STATISTICS diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index 35d18a0..457c142 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -36,4 +36,6 @@ uint32_t accumulate(uint32_t list[], int length); ***********************************************************************/ void Error_Handler(void); +uint8_t reverse(uint8_t byte); + #endif /* UTILITIES_H_ */ diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 351ebc6..0fc106d 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -492,7 +492,7 @@ void cliRun() break; case commandMask(commandSize_1, ACTION_SAVE): //Write all the values that have been changed to the eprom - writeEEPROM(); + //writeEEPROM(); break; case commandMask(commandSize_1, ACTION_UNDO): //Undp all things that have not been saved by reading the current saved values in the eeprom diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c new file mode 100644 index 0000000..b9593be --- /dev/null +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -0,0 +1,169 @@ + /********************************************************************** + * 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 = {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; +} + +/*********************************************************************** +* 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 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++) + { + // Look for the beginning of a sbus frame + if (raw_dma_data_t.buff[i] == (uint8_t)SBUS_HEADER && stop_bit_read) + { + 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) + { + 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); + } + } + // 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++; + } + } +} diff --git a/UAV-ControlSystem/src/drivers/usart.c b/UAV-ControlSystem/src/drivers/usart.c index 37e5eeb..417ad60 100644 --- a/UAV-ControlSystem/src/drivers/usart.c +++ b/UAV-ControlSystem/src/drivers/usart.c @@ -43,6 +43,8 @@ //BRR #define USART_BRR(_PCLK_, _BAUD_) ((_PCLK_ /(_BAUD_ * 16)) * 16) // Calculate BRR from the desired baud rate +/* Stores last DMA buffer address from "usart_get_dma_buffer". Is used to compare if data read is new or old */ +uint8_t * prevBuf = NULL; /*********************************************************************** * BRIEF: Initialize the USART with DMA reception of messages @@ -51,6 +53,7 @@ * messages can be received without utilizing any processor load. This * function returns false if any error occurred during the initialization * and true of everything went well +* For USART1 Inverter is ON on RX by default ***********************************************************************/ bool usart_init_dma(USART_TypeDef* usart_inst, // The USART instance to be used, i.e. USART1, USART3 or USART6 for the REVO card usart_dma_profile* profile_out, // The USART profile that will be used when sending or receiving data @@ -71,6 +74,7 @@ bool usart_init_dma(USART_TypeDef* usart_inst, // The USART instance to be dma_rx_instance = DMA2_Stream2; dma_tx_instance = DMA2_Stream5; channel = DMA_CHANNEL_4; + uart1_rx_inverter_init(true); } else if(usart_inst == USART3) { @@ -378,22 +382,31 @@ uint32_t usart_poll(usart_profile *profile, // The USART profile to receive data return i; } + + /*********************************************************************** * BRIEF: Get the DMA buffer that was most recently completed * INFORMATION: * This function will return the buffer that the DMA most recently * completed so that the DMA can continue writing to the second buffer * without interfering with the rest of the system +* A boolean "new_data" tells if the data is new or not as last data is +* read if no new one is available ***********************************************************************/ -uint8_t* usart_get_dma_buffer(usart_dma_profile *profile) +dma_usart_return usart_get_dma_buffer(usart_dma_profile *profile) { + dma_usart_return data = { .buff = NULL , .new_data = false }; // init here first!!!!! continue later // Check which buffer the DMA is writing to at the moment and return the other buffer if(profile->dma_usart_rx_instance->CR & DMA_SxCR_CT) { - return profile->dma_rx_buffer1; + data.buff = profile->dma_rx_buffer1; } else { - return profile->dma_rx_buffer2; + data.buff = profile->dma_rx_buffer2; } + data.new_data = (data.buff != prevBuf); + prevBuf = data.buff; + + return data; } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 78ba9fb..b7c7dab 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -33,6 +33,7 @@ #include "drivers/motors.h" #include "drivers/pwm.h" #include "drivers/system_clock.h" +#include "drivers/sbus.h" @@ -54,13 +55,17 @@ void systemTaskAttitude(void) void systemTaskRx(void) { //Interpret commands to the vehicle + sbus_read(); + sbusFrame_s frame = sbusChannelData; } bool systemTaskRxCheck(uint32_t currentDeltaTime) { //This task is what is controlling the event activation of the systemTaskRx //check if there is anything that has be received. - return false; + + + return sbus_frame_available(); } void systemTaskSerial(void) diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c index a125951..731b5a9 100644 --- a/UAV-ControlSystem/src/utilities.c +++ b/UAV-ControlSystem/src/utilities.c @@ -1,5 +1,3 @@ -/* - * utilities.c /********************************************************************** * NAME: utilities.c * * AUTHOR: Philip Johansson * @@ -40,3 +38,16 @@ void Error_Handler(void) { } } + +uint8_t reverse(uint8_t byte) +{ +// uint8_t ret = 0; +// ret = (byte & 0x80) >> 7 | (byte & 0x01) << 7; +// ret = (byte & 0x40) >> 6 | (byte & 0x02) << 6; +// ret = (byte & 0x20) >> 5 | (byte & 0x04) << 5; +// ret = (byte & 0x10) >> 4 | (byte & 0x08) << 4; + + byte = ((byte * 0x0802LU & 0x22110LU) | (byte * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16; + + return byte; +}