philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

First drafto of new parser

Browse Source
This commit is contained in:
Lennart Eriksson 2016-11-01 09:23:40 +01:00
parent fd743c755e
commit 1c0c7fb00f
3 changed files with 231 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
UAV-ControlSystem/inc/drivers/arduino_com.h
View File

@ -11,8 +11,55 @@
#include "drivers/usart.h" #include "drivers/usart.h"
#define ARDUINO_BAUD 115200 #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); 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_ */ #endif /* DRIVERS_ARDUINO_COM_H_ */

284
UAV-ControlSystem/src/drivers/arduino_com.c
View File

@ -19,25 +19,6 @@ enum packet_ids {
GPS_PACKET_ID = 0xB1, 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 { typedef struct arduino_data_t {
uint8_t size; //Size of the data uint8_t size; //Size of the data
void * dataPtr; //pointer to the data void * dataPtr; //pointer to the data
@ -51,22 +32,32 @@ enum arduino_data_e {
arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
[COMPASS_DATA_ID] = { [COMPASS_DATA_ID] = {
.size = sizeof(compass_data), .size = 8,
.dataPtr = &compass_data, .dataPtr = &compass_data,
}, },
[GPS_DATA_ID] = { [GPS_DATA_ID] = {
.size = sizeof(gps_data), .size = 10,
.dataPtr = &gps_data, .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) 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_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() bool arduino_frame_available()
{ {
/* We read data from DMA */ /* We read data from DMA */
@ -75,6 +66,11 @@ bool arduino_frame_available()
return raw_dma_data_t.new_data; 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 find_packet_from_header(uint8_t header)
{ {
arduino_data_t arduino_data = { arduino_data_t arduino_data = {
@ -82,6 +78,7 @@ arduino_data_t find_packet_from_header(uint8_t header)
.size = 0, .size = 0,
}; };
/* Check what header it is and return the correct datapointer if correct header*/
switch (header) switch (header)
{ {
case COMPASS_PACKET_ID: case COMPASS_PACKET_ID:
@ -96,6 +93,83 @@ arduino_data_t find_packet_from_header(uint8_t header)
return arduino_data; 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() void arduino_read()
{ {
static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
@ -103,96 +177,98 @@ void arduino_read()
static uint32_t missedMsg = 0; static uint32_t missedMsg = 0;
static uint8_t message_it_secondary_head = 0; static uint8_t message_it_secondary_head = 0;
static bool new_header = false; static bool new_header = false;
//static uint8_t current_packet_size = 0;
static uint8_t current_header = 0;
static uint8_t crc = 0; static uint8_t crc = 0;
static arduino_data_t msg_header_and_size = {0}; static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL };
arduino_frame_available(); static bool find_new_header = true;
if (raw_dma_data_t.new_data) if (raw_dma_data_t.new_data)
{ {
for (int i = 0; i < ARDUINO_DMA_SIZE; i++) 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]; // uint8_t msg = raw_dma_data_t.buff[i]; //raw_dma_data_t.buff[i];
msg_header_and_size = find_packet_from_header(msg); // if(find_new_header)
// {
// Look for the beginning of a frame // msg_header_and_size = find_packet_from_header(msg);
if ( message_it == 0 ) // if(msg_header_and_size.size != 0)
{ // {
// find_new_header = false;
// arduino_arr[(message_it)] = msg;
// message_it++;
if (msg_header_and_size.size != 0) // new_header = false; // Just received one
{ // crc ^= msg;
arduino_arr[(message_it)] = msg; // }
message_it++; // }
current_header = msg; // // Look for the end of sbus frame
new_header = false; // Just received one // else
crc ^= msg; // {
} // /* 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)
// Look for the end of sbus frame // {
else // 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 (msg_header_and_size.size != 0 && new_header == false) // }
{ //
new_header = true; // /* Reading the message except the crc byte */
message_it_secondary_head = message_it; //save the value of the position in The buffer array, not the dma array index // if ((message_it) < (msg_header_and_size.size - 1))
} // {
// arduino_arr[(message_it)] = msg;
/* Reading the message */ // crc ^= msg;
if ((message_it) < msg_header_and_size.size) // message_it++;
{ // }
arduino_arr[(message_it)] = msg; // else if ((message_it) == (msg_header_and_size.size - 1))
crc ^= msg; // {
message_it++; // /* put the crc code into the data buffer as well */
} // arduino_arr[(message_it)] = msg;
// missedMsg++;
if ((message_it) == msg_header_and_size.size) //
{ // /* TODO: Replace with check for CRC */
missedMsg++; // if (crc == msg)
// {
/* TODO: Replace with check for CRC */ // /* Clear necessary variables in order to fill the buffer with new ones */
if (crc == msg) // message_it = 0;
{ // find_new_header = true;
message_it = 0; // missedMsg--;
missedMsg--; // crc = 0;
arduino_data_t current_header_and_size = find_packet_from_header(current_header); //
// memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size);
uint8_t sizeof_data = current_header_and_size.size; // }
uint8_t* tmp_ptr_to; //
tmp_ptr_to = current_header_and_size.dataPtr; // /* If CRC does not match */
// else
memcpy(tmp_ptr_to,arduino_arr,sizeof_data); // {
//
} // int temp_secondaryHeader = message_it_secondary_head;
// int length = msg_header_and_size.size;
/* If CRC does not match */ // int new_struct_size = 0;
else // message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array
{ //
int temp_secondaryHeader = message_it_secondary_head; // if(new_header)
message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array // msg_header_and_size = find_packet_from_header(arduino_arr[temp_secondaryHeader]);
new_header = false; //set new header to false, this is true if there is another header within the buffer //
// /* Clear necessary variables so that a new message can be read into the buffer correctly */
//Move all the remaning messages in the buffer to the start of the buffer // new_header = false; //set new header to false, this is true if there is another header within the buffer
for (int i = temp_secondaryHeader; i < ARDUINO_DMA_SIZE; i++) // crc = 0;
{ //
int innerCount = i-temp_secondaryHeader; // //Move all the remaning messages in the buffer to the start of the buffer
arduino_arr[innerCount] = arduino_arr[i]; // for (int j = temp_secondaryHeader; j < length; j++)
// {
//check if we find another possible header inside the rest of the buffer and save that // int innerCount = j - temp_secondaryHeader;
if (((arduino_data_t)(find_packet_from_header(innerCount))).size != 0 && innerCount > 0 && new_header == false ) // arduino_arr[innerCount] = arduino_arr[j];
{ //
new_header = true; // crc ^= arduino_arr[j];
message_it_secondary_head = innerCount; //
} // //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;
} // }
// }
// }
// }
// }
} }
} }
} }

2
UAV-ControlSystem/src/main.c
View File

@ -61,7 +61,9 @@ void init_system()
// TODO!! TEMP code // TODO!! TEMP code
arduinoCom_init(USART1); arduinoCom_init(USART1);
while (1) { while (1) {
if(arduino_frame_available())
arduino_read(); arduino_read();
HAL_Delay(15);
} }