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

First test

Browse Source
This commit is contained in:
philsson 2016-10-27 10:55:49 +02:00
parent 33426ea90b
commit 6838df050b
4 changed files with 96 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -8,9 +8,11 @@
#ifndef DRIVERS_ARDUINO_COM_H_ #ifndef DRIVERS_ARDUINO_COM_H_
#define DRIVERS_ARDUINO_COM_H_ #define DRIVERS_ARDUINO_COM_H_
#include "drivers/usart.h"
#define ARDUINO_BAUD 115200 #define ARDUINO_BAUD 115200
#define ARDUINO_DMA_SIZE 100 #define ARDUINO_DMA_SIZE 100
void arduinoCom_init(USART_TypeDef usart); void arduinoCom_init(USART_TypeDef* usart_inst);
#endif /* DRIVERS_ARDUINO_COM_H_ */ #endif /* DRIVERS_ARDUINO_COM_H_ */

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

@ -6,7 +6,7 @@
*/ */
#include "drivers/arduino_com.h" #include "drivers/arduino_com.h"
#include "drivers/usart.h"
#include "utilities.h" #include "utilities.h"
#include "string.h" #include "string.h"
#include "stm32f4xx_revo.h" #include "stm32f4xx_revo.h"
@ -20,7 +20,7 @@ enum packet_ids {
}; };
typedef struct compass_data_t { typedef struct compass_data_t {
const uint8_t header = COMPASS_PACKET_ID; uint8_t header;
int16_t x; int16_t x;
int16_t y; int16_t y;
int16_t z; int16_t z;
@ -30,7 +30,7 @@ typedef struct compass_data_t {
compass_data_t compass_data = {0}; compass_data_t compass_data = {0};
typedef struct gps_data_t { typedef struct gps_data_t {
const uint8_t header = GPS_PACKET_ID; uint8_t header;
float latitude; float latitude;
float longitude; float longitude;
uint8_t crc; 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; 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() void arduino_read()
{ {
static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
@ -82,62 +103,72 @@ 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_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) 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++)
{ {
uint8_t msg = 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 // Look for the beginning of a frame
if ( message_it == 0 ) if ( message_it == 0 )
{ {
switch (msg)
if (msg_header_and_size.size != 0)
{ {
case COMPASS_PACKET_ID:
current_packet_size = data_arr[COMPASS_DATA_ID].size;
arduino_arr[(message_it)] = msg; arduino_arr[(message_it)] = msg;
message_it++; message_it++;
new_header = false; current_header = msg;
break; new_header = false; // Just received one
case GPS_PACKET_ID: crc ^= msg;
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 // Look for the end of sbus frame
else else
{ {
if (msg == (uint8_t)SBUS_HEADER && new_header == false) if (msg_header_and_size.size != 0 && new_header == false)
{ {
new_header = true; 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_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) /* Reading the message */
if ((message_it) < msg_header_and_size.size)
{ {
sbus_arr[(message_it)] = msg; arduino_arr[(message_it)] = msg;
crc ^= msg;
message_it++; message_it++;
} }
if ((message_it) == SBUS_FRAME_SIZE) if ((message_it) == msg_header_and_size.size)
{ {
missedMsg++; missedMsg++;
if (msg == (uint8_t)SBUS_FOOTER)
/* TODO: Replace with check for CRC */
if (crc == msg)
{ {
message_it = 0; message_it = 0;
missedMsg--; missedMsg--;
sbusChannelData = *(sbusFrame_s*)sbus_arr; 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 else
{ {
int temp_secondaryHeader = message_it_secondary_head; 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 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 //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; 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 //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; new_header = true;
message_it_secondary_head = innerCount; message_it_secondary_head = innerCount;
@ -160,13 +191,7 @@ void arduino_read()
} }
} }
} }
} }
}
} }

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

@ -121,16 +121,12 @@ void sbus_read()
// If continue only if we get new data from DMA // If continue only if we get new data from DMA
if (raw_dma_data_t.new_data) if (raw_dma_data_t.new_data)
{ {
for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++) for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++)
{ {
uint8_t msg = raw_dma_data_t.buff[i]; uint8_t msg = raw_dma_data_t.buff[i];
// Look for the beginning of a sbus frame // Look for the beginning of a sbus frame
if ( message_it == 0 ) //&& stop_bit_read) 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)) if (msg == ((uint8_t)SBUS_HEADER))
{ {
sbus_arr[(message_it)] = msg; sbus_arr[(message_it)] = msg;
@ -141,12 +137,7 @@ void sbus_read()
{ {
message_it = 0; 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 else
{ {
if (msg == (uint8_t)SBUS_HEADER && new_header == false) if (msg == (uint8_t)SBUS_HEADER && new_header == false)
@ -168,10 +159,6 @@ void sbus_read()
{ {
message_it = 0; message_it = 0;
missedMsg--; 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; sbusChannelData = *(sbusFrame_s*)sbus_arr;
// Linear fitting // Linear fitting
@ -228,17 +215,10 @@ void sbus_read()
message_it_secondary_head = innerCount; 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++;
} }
} }
} }

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

@ -28,6 +28,7 @@
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "drivers/motors.h" #include "drivers/motors.h"
#include "Flight/pid.h" #include "Flight/pid.h"
#include "drivers/arduino_com.h"
/************************************************************************** /**************************************************************************
@ -55,7 +56,14 @@ void init_system()
cliInit(USART3); cliInit(USART3);
//init sbus, using USART1 //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 //init motors to run with oneshot 125, small delay
HAL_Delay(1000); HAL_Delay(1000);