From 6838df050b4484589e7d0d55a789cfc8c8cc7285 Mon Sep 17 00:00:00 2001 From: philsson Date: Thu, 27 Oct 2016 10:55:49 +0200 Subject: [PATCH] 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);