diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h new file mode 100644 index 0000000..64ce846 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -0,0 +1,78 @@ +/* + * arduino_com.h + * + * Created on: 26 okt. 2016 + * Author: Philip + */ + +#ifndef DRIVERS_ARDUINO_COM_H_ +#define DRIVERS_ARDUINO_COM_H_ + +#include "drivers/usart.h" + +#define ARDUINO_BAUD 115200 +#define ARDUINO_DMA_SIZE 20 + + +/*********************************************************************** +* BRIEF: RX packet structure from arduino com * +* INFORMATION: Contains the whole compass message * +***********************************************************************/ +typedef struct compass_data_t { + uint8_t header; + int16_t x; + int16_t y; + int16_t z; + uint8_t crc; +} compass_data_t; + +/*********************************************************************** +* BRIEF: RX packet structure from arduino com * +* INFORMATION: Contains the whole gps data message * +***********************************************************************/ +typedef struct gps_data_t { + uint8_t header; + float latitude; + float longitude; + uint8_t crc; +} gps_data_t; + +/* An instance of the GPS data read from Arduino Com */ +gps_data_t gps_data; + +/* An instance of the compass data read from Arduino Com */ +compass_data_t compass_data; + + +/*********************************************************************** +* BRIEF: Initializes the UART for Arduino com * +* INFORMATION: A DMA Buffer starts storing the bytes received from RX * +***********************************************************************/ +void arduinoCom_init(USART_TypeDef* usart_inst); + +/*********************************************************************** +* BRIEF: Checks if new RX packet is available * +* INFORMATION: Is called by the scheduler * +***********************************************************************/ +bool arduino_frame_available(); + +/*********************************************************************** +* BRIEF: Updates "gps_data" and "compass_data" * +* INFORMATION: Is called by the scheduler * +***********************************************************************/ +void arduino_read(); + +#endif /* DRIVERS_ARDUINO_COM_H_ */ + + + +//----------------------- Example code for the parser -------------------------- + +//arduinoCom_init(USART1); +//while (1) { +// if(arduino_frame_available()) +// arduino_read(); +// HAL_Delay(15); +// float lng = gps_data.longitude; +// float lat = gps_data.latitude; +//} diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c new file mode 100644 index 0000000..594ff57 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -0,0 +1,200 @@ +/* + * arduino_com.c + * + * Created on: 26 okt. 2016 + * Author: Philip + */ + +#include "drivers/arduino_com.h" + +#include "utilities.h" +#include "string.h" +#include "stm32f4xx_revo.h" + +#define COMPASS_PACKET_SIZE 8 +#define GPS_PACKET_SIZE 10 + + +usart_dma_profile dmaHandler; +dma_usart_return raw_dma_data_t; + +// enumeration to hold the id:s of the different packages +enum packet_ids { + COMPASS_PACKET_ID = 0xA1, + GPS_PACKET_ID = 0xB1, +}; + +// Structure used to hold the data for "data_arr" +typedef struct arduino_data_t { + uint8_t size; //Size of the data + void * dataPtr; //pointer to the data +} arduino_data_t ; + +// An enumeration of the array positions of the data in the "data_arr" +enum arduino_data_e { + COMPASS_DATA_ID, + GPS_DATA_ID, + ARDUINO_DATA_COUNT, +}; + +// An array to hold the pointers to the different data structures that are used in the rest of the system; +arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { + [COMPASS_DATA_ID] = { + .size = COMPASS_PACKET_SIZE, + .dataPtr = &compass_data, + }, + [GPS_DATA_ID] = { + .size = GPS_PACKET_SIZE, + .dataPtr = &gps_data, + }, +}; + + +/*********************************************************************** +* BRIEF: Initializes the UART for Arduino com * +* INFORMATION: A DMA Buffer starts storing the bytes received from RX * +***********************************************************************/ +void arduinoCom_init(USART_TypeDef* usart_inst) +{ + /* initialize the USART with a dma buffer */ + usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0); +// usart_transmit(&dmaHandler.usart_pro, "data", 4, 100000); +} + + +/*********************************************************************** +* BRIEF: Checks if new RX packet is available * +* INFORMATION: Is called by the scheduler * +***********************************************************************/ +bool arduino_frame_available() +{ + /* We read data from DMA */ + raw_dma_data_t = usart_get_dma_buffer(&dmaHandler); + + return raw_dma_data_t.new_data; +} + +/*********************************************************************** +* BRIEF: Private function that looks for the correct data struct from * +* the header data * +* INFORMATION: * +***********************************************************************/ +arduino_data_t find_packet_from_header(uint8_t header) +{ + arduino_data_t arduino_data = { + .dataPtr = NULL, + .size = 0, + }; + + /* Check what header it is and return the correct datapointer if correct header*/ + switch (header) + { + case COMPASS_PACKET_ID: + arduino_data = data_arr[COMPASS_DATA_ID]; + break; + case GPS_PACKET_ID: + arduino_data = data_arr[GPS_DATA_ID]; + break; + default: + break; + } + return arduino_data; +} + + +/*********************************************************************** +* BRIEF: A function that parses the message byte per byte and then +* if it was wrong it will do recursive call and parse the rest +* of the message +* INFORMATION: * +***********************************************************************/ +void arduino_parse_message(uint8_t data) +{ + static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; + static bool find_header = true; + static uint8_t message_it = 0; + static uint8_t secondary_message_it = 0; + static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; + static uint8_t crc = 0; + + if(find_header) + { + msg_header_and_size = find_packet_from_header(data); + + if(msg_header_and_size.size != 0) + { + find_header = false; + arduino_arr[(message_it)] = data; + message_it++; + crc ^= data; + } + } + else + { + /* If we find any new possible header then we should be able to return to that point in time */ + if (((arduino_data_t)find_packet_from_header(data)).size != 0 && secondary_message_it == 0) + { + secondary_message_it = message_it; //save the value of the position in The buffer array, not the dma array index + } + + /* Reading the message except the crc byte */ + if ((message_it) < (msg_header_and_size.size - 1)) + { + arduino_arr[(message_it)] = data; + crc ^= data; + message_it++; + } + else if ((message_it) == (msg_header_and_size.size - 1)) + { + /* put the crc code into the data buffer as well */ + arduino_arr[(message_it)] = data; + + /* TODO: Replace with check for CRC */ + if (crc == data) + { + /* Clear necessary variables in order to fill the buffer with new ones */ + message_it = 0; + find_header = true; + secondary_message_it = 0; + crc = 0; + + memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size); + } + else + { + int size = msg_header_and_size.size; + int new_iter = secondary_message_it; + + crc = 0; + find_header = true; + message_it = 0; + secondary_message_it = 0; + + if(new_iter > 0) + { + for(int i = new_iter; i < size; i++) + { + arduino_parse_message(arduino_arr[i]); + } + } + } + } + } +} + +/*********************************************************************** +* BRIEF: Updates "gps_data" and "compass_data" * +* INFORMATION: Is called by the scheduler * +***********************************************************************/ +void arduino_read() +{ + //If the DMA has come to a new buffer + if (raw_dma_data_t.new_data) + { + // parse the entire message to the gps_data and compass_data + for (int i = 0; i < ARDUINO_DMA_SIZE; i++) + { + arduino_parse_message(raw_dma_data_t.buff[i]); + } + } +} diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index f61c0b0..ccd9474 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -28,7 +28,6 @@ /* 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; @@ -122,16 +121,12 @@ void sbus_read() // 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; @@ -142,12 +137,7 @@ void sbus_read() { 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) @@ -169,10 +159,6 @@ void sbus_read() { 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 @@ -229,17 +215,10 @@ void sbus_read() 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++; } - } } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 9722f46..7b5e199 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -28,6 +28,7 @@ #include "drivers/motormix.h" #include "drivers/motors.h" #include "Flight/pid.h" +#include "drivers/arduino_com.h" /**************************************************************************