/********************************************************************** * 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; }