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

Merge remote-tracking branch 'refs/remotes/origin/ArduinoCom'

Browse Source
This commit is contained in:
Lennart Eriksson 2016-11-01 10:11:55 +01:00
commit 9c9a721cfa
4 changed files with 279 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
UAV-ControlSystem/inc/drivers/arduino_com.h Normal file
View File

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

200
UAV-ControlSystem/src/drivers/arduino_com.c Normal file
View File

@ -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]);
}
}
}

21
UAV-ControlSystem/src/drivers/sbus.c
View File

@ -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++;
}
}
}

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

@ -28,6 +28,7 @@
#include "drivers/motormix.h"
#include "drivers/motors.h"
#include "Flight/pid.h"
#include "drivers/arduino_com.h"
/**************************************************************************