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

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

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

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

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"
 
 
 /**************************************************************************