From 33426ea90b4d25e1b0eb4b2fa1d54aa75dd5b7b9 Mon Sep 17 00:00:00 2001 From: philsson Date: Wed, 26 Oct 2016 16:27:21 +0200 Subject: [PATCH 1/7] Commit only to switch branch. Not ready! --- UAV-ControlSystem/inc/drivers/arduino_com.h | 16 ++ UAV-ControlSystem/src/drivers/arduino_com.c | 172 ++++++++++++++++++++ UAV-ControlSystem/src/drivers/sbus.c | 1 - 3 files changed, 188 insertions(+), 1 deletion(-) create mode 100644 UAV-ControlSystem/inc/drivers/arduino_com.h create mode 100644 UAV-ControlSystem/src/drivers/arduino_com.c diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h new file mode 100644 index 0000000..4c1d0fc --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -0,0 +1,16 @@ +/* + * arduino_com.h + * + * Created on: 26 okt. 2016 + * Author: Philip + */ + +#ifndef DRIVERS_ARDUINO_COM_H_ +#define DRIVERS_ARDUINO_COM_H_ + +#define ARDUINO_BAUD 115200 +#define ARDUINO_DMA_SIZE 100 + +void arduinoCom_init(USART_TypeDef usart); + +#endif /* DRIVERS_ARDUINO_COM_H_ */ diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c new file mode 100644 index 0000000..6d4df13 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -0,0 +1,172 @@ +/* + * arduino_com.c + * + * Created on: 26 okt. 2016 + * Author: Philip + */ + +#include "drivers/arduino_com.h" +#include "drivers/usart.h" +#include "utilities.h" +#include "string.h" +#include "stm32f4xx_revo.h" + +usart_dma_profile dmaHandler; +dma_usart_return raw_dma_data_t; + +enum packet_ids { + COMPASS_PACKET_ID = 0xA1, + GPS_PACKET_ID = 0xB1, +}; + +typedef struct compass_data_t { + const uint8_t header = COMPASS_PACKET_ID; + int16_t x; + int16_t y; + int16_t z; + uint8_t crc; +} compass_data_t; + +compass_data_t compass_data = {0}; + +typedef struct gps_data_t { + const uint8_t header = GPS_PACKET_ID; + float latitude; + float longitude; + uint8_t crc; +} gps_data_t; + +gps_data_t gps_data = {0}; + +typedef struct arduino_data_t { + uint8_t size; //Size of the data + void * dataPtr; //pointer to the data +} arduino_data_t ; + +enum arduino_data_e { + COMPASS_DATA_ID, + GPS_DATA_ID, + ARDUINO_DATA_COUNT, +}; + +arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { + [COMPASS_DATA_ID] = { + .size = sizeof(compass_data), + .dataPtr = &compass_data, + }, + [GPS_DATA_ID] = { + .size = sizeof(gps_data), + .dataPtr = &gps_data, + }, +}; + + +void arduinoCom_init(USART_TypeDef usart) +{ + usart_init_dma(usart, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0); +} + + +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; +} + +void arduino_read() +{ + static uint8_t arduino_arr[ARDUINO_DMA_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 uint8_t current_packet_size = 0; + + if (raw_dma_data_t.new_data) + { + + for (int i = 0; i < ARDUINO_DMA_SIZE; i++) + { + uint8_t msg = raw_dma_data_t.buff[i]; + // Look for the beginning of a frame + if ( message_it == 0 ) + { + switch (msg) + { + case COMPASS_PACKET_ID: + current_packet_size = data_arr[COMPASS_DATA_ID].size; + arduino_arr[(message_it)] = msg; + message_it++; + new_header = false; + break; + case GPS_PACKET_ID: + current_packet_size = data_arr[GPS_DATA_ID].size; + arduino_arr[(message_it)] = msg; + message_it++; + new_header = false; + break; + default: + message_it = 0; + + } + + } + // Look for the end of sbus frame + 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--; + sbusChannelData = *(sbusFrame_s*)sbus_arr; + } + } + 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; + } + } + + } + } + + } + + + } + + + + +} diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index f61c0b0..c58955d 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; From 6838df050b4484589e7d0d55a789cfc8c8cc7285 Mon Sep 17 00:00:00 2001 From: philsson Date: Thu, 27 Oct 2016 10:55:49 +0200 Subject: [PATCH 2/7] First test --- UAV-ControlSystem/inc/drivers/arduino_com.h | 4 +- UAV-ControlSystem/src/drivers/arduino_com.c | 143 ++++++++++++-------- UAV-ControlSystem/src/drivers/sbus.c | 20 --- UAV-ControlSystem/src/main.c | 10 +- 4 files changed, 96 insertions(+), 81 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 4c1d0fc..ddcc06b 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -8,9 +8,11 @@ #ifndef DRIVERS_ARDUINO_COM_H_ #define DRIVERS_ARDUINO_COM_H_ +#include "drivers/usart.h" + #define ARDUINO_BAUD 115200 #define ARDUINO_DMA_SIZE 100 -void arduinoCom_init(USART_TypeDef usart); +void arduinoCom_init(USART_TypeDef* usart_inst); #endif /* DRIVERS_ARDUINO_COM_H_ */ diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 6d4df13..514199d 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -6,7 +6,7 @@ */ #include "drivers/arduino_com.h" -#include "drivers/usart.h" + #include "utilities.h" #include "string.h" #include "stm32f4xx_revo.h" @@ -20,7 +20,7 @@ enum packet_ids { }; typedef struct compass_data_t { - const uint8_t header = COMPASS_PACKET_ID; + uint8_t header; int16_t x; int16_t y; int16_t z; @@ -30,7 +30,7 @@ typedef struct compass_data_t { compass_data_t compass_data = {0}; typedef struct gps_data_t { - const uint8_t header = GPS_PACKET_ID; + uint8_t header; float latitude; float longitude; uint8_t crc; @@ -61,9 +61,9 @@ arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { }; -void arduinoCom_init(USART_TypeDef usart) +void arduinoCom_init(USART_TypeDef* usart_inst) { - usart_init_dma(usart, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0); + usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0); } @@ -75,6 +75,27 @@ bool arduino_frame_available() return raw_dma_data_t.new_data; } +arduino_data_t find_packet_from_header(uint8_t header) +{ + arduino_data_t arduino_data = { + .dataPtr = NULL, + .size = 0, + }; + + 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; +} + void arduino_read() { static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; @@ -82,62 +103,72 @@ void arduino_read() static uint32_t missedMsg = 0; static uint8_t message_it_secondary_head = 0; static bool new_header = false; - static uint8_t current_packet_size = 0; + //static uint8_t current_packet_size = 0; + static uint8_t current_header = 0; + static uint8_t crc = 0; + static arduino_data_t msg_header_and_size = {0}; if (raw_dma_data_t.new_data) { for (int i = 0; i < ARDUINO_DMA_SIZE; i++) + { + + uint8_t msg = raw_dma_data_t.buff[i]; + msg_header_and_size = find_packet_from_header(msg); + + // Look for the beginning of a frame + if ( message_it == 0 ) { - uint8_t msg = raw_dma_data_t.buff[i]; - // Look for the beginning of a frame - if ( message_it == 0 ) + + + + if (msg_header_and_size.size != 0) { - switch (msg) - { - case COMPASS_PACKET_ID: - current_packet_size = data_arr[COMPASS_DATA_ID].size; - arduino_arr[(message_it)] = msg; - message_it++; - new_header = false; - break; - case GPS_PACKET_ID: - current_packet_size = data_arr[GPS_DATA_ID].size; - arduino_arr[(message_it)] = msg; - message_it++; - new_header = false; - break; - default: - message_it = 0; - - } - + arduino_arr[(message_it)] = msg; + message_it++; + current_header = msg; + new_header = false; // Just received one + crc ^= msg; } - // Look for the end of sbus frame - else + } + // Look for the end of sbus frame + else + { + if (msg_header_and_size.size != 0 && new_header == false) { - 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 + } + + /* Reading the message */ + if ((message_it) < msg_header_and_size.size) + { + arduino_arr[(message_it)] = msg; + crc ^= msg; + message_it++; + } + + if ((message_it) == msg_header_and_size.size) + { + missedMsg++; + + /* TODO: Replace with check for CRC */ + if (crc == msg) { - new_header = true; - message_it_secondary_head = message_it; //save the value of the position in The buffer array, not the dma array index + message_it = 0; + missedMsg--; + arduino_data_t current_header_and_size = find_packet_from_header(current_header); + + uint8_t sizeof_data = current_header_and_size.size; + uint8_t* tmp_ptr_to; + tmp_ptr_to = current_header_and_size.dataPtr; + + memcpy(tmp_ptr_to,arduino_arr,sizeof_data); + } - 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--; - sbusChannelData = *(sbusFrame_s*)sbus_arr; - } - } + /* If CRC does not match */ else { int temp_secondaryHeader = message_it_secondary_head; @@ -145,13 +176,13 @@ void arduino_read() 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++) + for (int i = temp_secondaryHeader; i < ARDUINO_DMA_SIZE; i++) { int innerCount = i-temp_secondaryHeader; - sbus_arr[innerCount] = sbus_arr[i]; + arduino_arr[innerCount] = arduino_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 ) + if (((arduino_data_t)(find_packet_from_header(innerCount))).size != 0 && innerCount > 0 && new_header == false ) { new_header = true; message_it_secondary_head = innerCount; @@ -160,13 +191,7 @@ void arduino_read() } } - } - - + } } - - - - } diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index c58955d..ccd9474 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -121,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; @@ -141,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) @@ -168,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 @@ -228,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..45675e3 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" /************************************************************************** @@ -55,7 +56,14 @@ void init_system() cliInit(USART3); //init sbus, using USART1 - sbus_init(); + //sbus_init(); + + // TODO!! TEMP code + arduinoCom_init(USART1); + while (1) { + arduino_read(); + } + //init motors to run with oneshot 125, small delay HAL_Delay(1000); From fd743c755e7ae92b46cd2ff4df2b0b3f2a16a7b2 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Thu, 27 Oct 2016 13:24:56 +0200 Subject: [PATCH 3/7] Fix for starting sampling of data --- UAV-ControlSystem/inc/drivers/arduino_com.h | 2 +- UAV-ControlSystem/src/drivers/arduino_com.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index ddcc06b..5b4321d 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -11,7 +11,7 @@ #include "drivers/usart.h" #define ARDUINO_BAUD 115200 -#define ARDUINO_DMA_SIZE 100 +#define ARDUINO_DMA_SIZE 15 void arduinoCom_init(USART_TypeDef* usart_inst); diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 514199d..a8a0834 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -63,7 +63,7 @@ arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { void arduinoCom_init(USART_TypeDef* usart_inst) { - usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0); + usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0); } @@ -107,6 +107,7 @@ void arduino_read() static uint8_t current_header = 0; static uint8_t crc = 0; static arduino_data_t msg_header_and_size = {0}; + arduino_frame_available(); if (raw_dma_data_t.new_data) { From 1c0c7fb00f71e3ab57ec88a5aa35e2ae9dc0a1ce Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 09:23:40 +0100 Subject: [PATCH 4/7] First drafto of new parser --- UAV-ControlSystem/inc/drivers/arduino_com.h | 49 +++- UAV-ControlSystem/src/drivers/arduino_com.c | 284 +++++++++++++------- UAV-ControlSystem/src/main.c | 4 +- 3 files changed, 231 insertions(+), 106 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 5b4321d..03641cf 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -11,8 +11,55 @@ #include "drivers/usart.h" #define ARDUINO_BAUD 115200 -#define ARDUINO_DMA_SIZE 15 +#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_ */ diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index a8a0834..68dd612 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -19,25 +19,6 @@ enum packet_ids { GPS_PACKET_ID = 0xB1, }; -typedef struct compass_data_t { - uint8_t header; - int16_t x; - int16_t y; - int16_t z; - uint8_t crc; -} compass_data_t; - -compass_data_t compass_data = {0}; - -typedef struct gps_data_t { - uint8_t header; - float latitude; - float longitude; - uint8_t crc; -} gps_data_t; - -gps_data_t gps_data = {0}; - typedef struct arduino_data_t { uint8_t size; //Size of the data void * dataPtr; //pointer to the data @@ -51,22 +32,32 @@ enum arduino_data_e { arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { [COMPASS_DATA_ID] = { - .size = sizeof(compass_data), + .size = 8, .dataPtr = &compass_data, }, [GPS_DATA_ID] = { - .size = sizeof(gps_data), + .size = 10, .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 */ @@ -75,6 +66,11 @@ bool arduino_frame_available() 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 = { @@ -82,6 +78,7 @@ arduino_data_t find_packet_from_header(uint8_t header) .size = 0, }; + /* Check what header it is and return the correct datapointer if correct header*/ switch (header) { case COMPASS_PACKET_ID: @@ -96,6 +93,83 @@ arduino_data_t find_packet_from_header(uint8_t header) return arduino_data; } +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; + 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() { static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; @@ -103,96 +177,98 @@ void arduino_read() static uint32_t missedMsg = 0; static uint8_t message_it_secondary_head = 0; static bool new_header = false; - //static uint8_t current_packet_size = 0; - static uint8_t current_header = 0; static uint8_t crc = 0; - static arduino_data_t msg_header_and_size = {0}; - arduino_frame_available(); + static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; + static bool find_new_header = true; if (raw_dma_data_t.new_data) { - for (int i = 0; i < ARDUINO_DMA_SIZE; i++) { + arduino_parse_message(raw_dma_data_t.buff[i]); - uint8_t msg = raw_dma_data_t.buff[i]; - msg_header_and_size = find_packet_from_header(msg); - - // Look for the beginning of a frame - if ( message_it == 0 ) - { - - - - if (msg_header_and_size.size != 0) - { - arduino_arr[(message_it)] = msg; - message_it++; - current_header = msg; - new_header = false; // Just received one - crc ^= msg; - } - } - // Look for the end of sbus frame - else - { - if (msg_header_and_size.size != 0 && 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 - } - - /* Reading the message */ - if ((message_it) < msg_header_and_size.size) - { - arduino_arr[(message_it)] = msg; - crc ^= msg; - message_it++; - } - - if ((message_it) == msg_header_and_size.size) - { - missedMsg++; - - /* TODO: Replace with check for CRC */ - if (crc == msg) - { - message_it = 0; - missedMsg--; - arduino_data_t current_header_and_size = find_packet_from_header(current_header); - - uint8_t sizeof_data = current_header_and_size.size; - uint8_t* tmp_ptr_to; - tmp_ptr_to = current_header_and_size.dataPtr; - - memcpy(tmp_ptr_to,arduino_arr,sizeof_data); - - } - - /* If CRC does not match */ - 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 < ARDUINO_DMA_SIZE; i++) - { - int innerCount = i-temp_secondaryHeader; - arduino_arr[innerCount] = arduino_arr[i]; - - //check if we find another possible header inside the rest of the buffer and save that - if (((arduino_data_t)(find_packet_from_header(innerCount))).size != 0 && innerCount > 0 && new_header == false ) - { - new_header = true; - message_it_secondary_head = innerCount; - } - } - - } - } - } +// uint8_t msg = raw_dma_data_t.buff[i]; //raw_dma_data_t.buff[i]; +// if(find_new_header) +// { +// msg_header_and_size = find_packet_from_header(msg); +// if(msg_header_and_size.size != 0) +// { +// find_new_header = false; +// arduino_arr[(message_it)] = msg; +// message_it++; +// new_header = false; // Just received one +// crc ^= msg; +// } +// } +// // Look for the end of sbus frame +// 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(msg)).size != 0 && 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 +// } +// +// /* Reading the message except the crc byte */ +// if ((message_it) < (msg_header_and_size.size - 1)) +// { +// arduino_arr[(message_it)] = msg; +// crc ^= msg; +// 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)] = msg; +// missedMsg++; +// +// /* TODO: Replace with check for CRC */ +// if (crc == msg) +// { +// /* Clear necessary variables in order to fill the buffer with new ones */ +// message_it = 0; +// find_new_header = true; +// missedMsg--; +// crc = 0; +// +// memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size); +// } +// +// /* If CRC does not match */ +// else +// { +// +// int temp_secondaryHeader = message_it_secondary_head; +// int length = msg_header_and_size.size; +// int new_struct_size = 0; +// message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array +// +// if(new_header) +// msg_header_and_size = find_packet_from_header(arduino_arr[temp_secondaryHeader]); +// +// /* Clear necessary variables so that a new message can be read into the buffer correctly */ +// new_header = false; //set new header to false, this is true if there is another header within the buffer +// crc = 0; +// +// //Move all the remaning messages in the buffer to the start of the buffer +// for (int j = temp_secondaryHeader; j < length; j++) +// { +// int innerCount = j - temp_secondaryHeader; +// arduino_arr[innerCount] = arduino_arr[j]; +// +// crc ^= arduino_arr[j]; +// +// //check if we find another possible header inside the rest of the buffer and save that +// if (((arduino_data_t)(find_packet_from_header(innerCount))).size != 0 && innerCount > 0 && new_header == false ) +// { +// new_header = true; +// message_it_secondary_head = innerCount; +// } +// } +// } +// } +// } } } } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 45675e3..438106f 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -61,7 +61,9 @@ void init_system() // TODO!! TEMP code arduinoCom_init(USART1); while (1) { - arduino_read(); + if(arduino_frame_available()) + arduino_read(); + HAL_Delay(15); } From 50bc7bac304c408d96b7aef0442177a5232736e3 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 09:34:57 +0100 Subject: [PATCH 5/7] Arduino com working with parser and everything (hopefully) --- UAV-ControlSystem/inc/drivers/arduino_com.h | 13 +++ UAV-ControlSystem/src/drivers/arduino_com.c | 101 ++------------------ UAV-ControlSystem/src/main.c | 12 +-- 3 files changed, 28 insertions(+), 98 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 03641cf..64ce846 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -63,3 +63,16 @@ bool arduino_frame_available(); 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 index 68dd612..d09c013 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -93,6 +93,13 @@ arduino_data_t find_packet_from_header(uint8_t header) 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]; @@ -172,103 +179,13 @@ void arduino_parse_message(uint8_t data) ***********************************************************************/ void arduino_read() { - static uint8_t arduino_arr[ARDUINO_DMA_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 uint8_t crc = 0; - static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; - static bool find_new_header = true; - + //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]); - -// uint8_t msg = raw_dma_data_t.buff[i]; //raw_dma_data_t.buff[i]; -// if(find_new_header) -// { -// msg_header_and_size = find_packet_from_header(msg); -// if(msg_header_and_size.size != 0) -// { -// find_new_header = false; -// arduino_arr[(message_it)] = msg; -// message_it++; -// new_header = false; // Just received one -// crc ^= msg; -// } -// } -// // Look for the end of sbus frame -// 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(msg)).size != 0 && 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 -// } -// -// /* Reading the message except the crc byte */ -// if ((message_it) < (msg_header_and_size.size - 1)) -// { -// arduino_arr[(message_it)] = msg; -// crc ^= msg; -// 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)] = msg; -// missedMsg++; -// -// /* TODO: Replace with check for CRC */ -// if (crc == msg) -// { -// /* Clear necessary variables in order to fill the buffer with new ones */ -// message_it = 0; -// find_new_header = true; -// missedMsg--; -// crc = 0; -// -// memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size); -// } -// -// /* If CRC does not match */ -// else -// { -// -// int temp_secondaryHeader = message_it_secondary_head; -// int length = msg_header_and_size.size; -// int new_struct_size = 0; -// message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array -// -// if(new_header) -// msg_header_and_size = find_packet_from_header(arduino_arr[temp_secondaryHeader]); -// -// /* Clear necessary variables so that a new message can be read into the buffer correctly */ -// new_header = false; //set new header to false, this is true if there is another header within the buffer -// crc = 0; -// -// //Move all the remaning messages in the buffer to the start of the buffer -// for (int j = temp_secondaryHeader; j < length; j++) -// { -// int innerCount = j - temp_secondaryHeader; -// arduino_arr[innerCount] = arduino_arr[j]; -// -// crc ^= arduino_arr[j]; -// -// //check if we find another possible header inside the rest of the buffer and save that -// if (((arduino_data_t)(find_packet_from_header(innerCount))).size != 0 && innerCount > 0 && new_header == false ) -// { -// new_header = true; -// message_it_secondary_head = innerCount; -// } -// } -// } -// } -// } } } } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 438106f..b683c4c 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -59,12 +59,12 @@ void init_system() //sbus_init(); // TODO!! TEMP code - arduinoCom_init(USART1); - while (1) { - if(arduino_frame_available()) - arduino_read(); - HAL_Delay(15); - } +// arduinoCom_init(USART1); +// while (1) { +// if(arduino_frame_available()) +// arduino_read(); +// HAL_Delay(15); +// } //init motors to run with oneshot 125, small delay From 1b601f153d274f508d97f857b3965e1906b9af6b Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 09:44:41 +0100 Subject: [PATCH 6/7] Fixes for the master --- UAV-ControlSystem/src/drivers/arduino_com.c | 1 + UAV-ControlSystem/src/main.c | 11 +---------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index d09c013..8e1394d 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -147,6 +147,7 @@ void arduino_parse_message(uint8_t 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); diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index b683c4c..7b5e199 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -56,16 +56,7 @@ void init_system() cliInit(USART3); //init sbus, using USART1 - //sbus_init(); - - // TODO!! TEMP code -// arduinoCom_init(USART1); -// while (1) { -// if(arduino_frame_available()) -// arduino_read(); -// HAL_Delay(15); -// } - + sbus_init(); //init motors to run with oneshot 125, small delay HAL_Delay(1000); From be8b35a336f7feb1e33e65ec093c70ae02d212e2 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 10:06:56 +0100 Subject: [PATCH 7/7] fixed commenting --- UAV-ControlSystem/src/drivers/arduino_com.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 8e1394d..594ff57 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -11,32 +11,40 @@ #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 = 8, + .size = COMPASS_PACKET_SIZE, .dataPtr = &compass_data, }, [GPS_DATA_ID] = { - .size = 10, + .size = GPS_PACKET_SIZE, .dataPtr = &gps_data, }, };