From 33426ea90b4d25e1b0eb4b2fa1d54aa75dd5b7b9 Mon Sep 17 00:00:00 2001 From: philsson Date: Wed, 26 Oct 2016 16:27:21 +0200 Subject: [PATCH 01/24] Commit only to switch branch. Not ready! --- UAV-ControlSystem/inc/drivers/arduino_com.h | 16 ++ UAV-ControlSystem/src/drivers/arduino_com.c | 172 ++++++++++++++++++++ UAV-ControlSystem/src/drivers/sbus.c | 1 - 3 files changed, 188 insertions(+), 1 deletion(-) create mode 100644 UAV-ControlSystem/inc/drivers/arduino_com.h create mode 100644 UAV-ControlSystem/src/drivers/arduino_com.c diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h new file mode 100644 index 0000000..4c1d0fc --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -0,0 +1,16 @@ +/* + * arduino_com.h + * + * Created on: 26 okt. 2016 + * Author: Philip + */ + +#ifndef DRIVERS_ARDUINO_COM_H_ +#define DRIVERS_ARDUINO_COM_H_ + +#define ARDUINO_BAUD 115200 +#define ARDUINO_DMA_SIZE 100 + +void arduinoCom_init(USART_TypeDef usart); + +#endif /* DRIVERS_ARDUINO_COM_H_ */ diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c new file mode 100644 index 0000000..6d4df13 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -0,0 +1,172 @@ +/* + * arduino_com.c + * + * Created on: 26 okt. 2016 + * Author: Philip + */ + +#include "drivers/arduino_com.h" +#include "drivers/usart.h" +#include "utilities.h" +#include "string.h" +#include "stm32f4xx_revo.h" + +usart_dma_profile dmaHandler; +dma_usart_return raw_dma_data_t; + +enum packet_ids { + COMPASS_PACKET_ID = 0xA1, + GPS_PACKET_ID = 0xB1, +}; + +typedef struct compass_data_t { + const uint8_t header = COMPASS_PACKET_ID; + 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 { + const uint8_t header = GPS_PACKET_ID; + float latitude; + float longitude; + uint8_t crc; +} gps_data_t; + +gps_data_t gps_data = {0}; + +typedef struct arduino_data_t { + uint8_t size; //Size of the data + void * dataPtr; //pointer to the data +} arduino_data_t ; + +enum arduino_data_e { + COMPASS_DATA_ID, + GPS_DATA_ID, + ARDUINO_DATA_COUNT, +}; + +arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { + [COMPASS_DATA_ID] = { + .size = sizeof(compass_data), + .dataPtr = &compass_data, + }, + [GPS_DATA_ID] = { + .size = sizeof(gps_data), + .dataPtr = &gps_data, + }, +}; + + +void arduinoCom_init(USART_TypeDef usart) +{ + usart_init_dma(usart, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0); +} + + +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; +} + +void arduino_read() +{ + static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; + static uint8_t message_it = 0; + 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; + + 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]; + // Look for the beginning of a frame + if ( message_it == 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; + + } + + } + // Look for the end of sbus frame + else + { + 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 + } + + 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; + } + } + else + { + int temp_secondaryHeader = message_it_secondary_head; + message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array + 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++) + { + int innerCount = i-temp_secondaryHeader; + sbus_arr[innerCount] = sbus_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 ) + { + new_header = true; + message_it_secondary_head = innerCount; + } + } + + } + } + + } + + + } + + + + +} diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index f61c0b0..c58955d 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/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; From 6838df050b4484589e7d0d55a789cfc8c8cc7285 Mon Sep 17 00:00:00 2001 From: philsson Date: Thu, 27 Oct 2016 10:55:49 +0200 Subject: [PATCH 02/24] 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); From fd743c755e7ae92b46cd2ff4df2b0b3f2a16a7b2 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Thu, 27 Oct 2016 13:24:56 +0200 Subject: [PATCH 03/24] Fix for starting sampling of data --- UAV-ControlSystem/inc/drivers/arduino_com.h | 2 +- UAV-ControlSystem/src/drivers/arduino_com.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index ddcc06b..5b4321d 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -11,7 +11,7 @@ #include "drivers/usart.h" #define ARDUINO_BAUD 115200 -#define ARDUINO_DMA_SIZE 100 +#define ARDUINO_DMA_SIZE 15 void arduinoCom_init(USART_TypeDef* usart_inst); diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 514199d..a8a0834 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -63,7 +63,7 @@ arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { void arduinoCom_init(USART_TypeDef* usart_inst) { - usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0); + usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0); } @@ -107,6 +107,7 @@ void arduino_read() static uint8_t current_header = 0; static uint8_t crc = 0; static arduino_data_t msg_header_and_size = {0}; + arduino_frame_available(); if (raw_dma_data_t.new_data) { From 3039f3a3948ea69e0ff558ba0f617b928161316b Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 18:17:13 +0100 Subject: [PATCH 04/24] pid SHIT FIX --- UAV-ControlSystem/src/Flight/pid.c | 28 ++++++++++++++++++++++++---- UAV-ControlSystem/src/main.c | 2 +- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index e538621..018181f 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -151,6 +151,17 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ +// +// if (calcGravity(accelProfile) > 1.15) +// { +// +// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT; +// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT; +// +// } +// else +// { + float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ @@ -188,6 +199,8 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; + + // sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); // sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); @@ -456,23 +469,23 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID) case PID_ID_GYRO: PidProfileBuff[ID].DOF = 3; - PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms break; case PID_ID_ACCELEROMETER: PidProfileBuff[ID].DOF = 2; - PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000; + PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; break; case PID_ID_COMPASS: - PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; + PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; break; case PID_ID_BAROMETER: - PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; + PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; break; default: @@ -618,4 +631,11 @@ void pidEproom(void) PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; + PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; + PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; + PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; + + + } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 2c9dfaf..f678bab 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -60,7 +60,7 @@ void init_system() pidEproom(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART3); + cliInit(USART6); //init sbus, using USART1 sbus_init(); From 4b7fd40967e862ddee0e571234b849d1ce2320e2 Mon Sep 17 00:00:00 2001 From: mdhsweden Date: Mon, 31 Oct 2016 18:30:09 +0100 Subject: [PATCH 05/24] Cast Ints to float in constrainf. What happens if we return an int on a function which returns a float? Just to be sure --- UAV-ControlSystem/src/Flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 018181f..b7fee19 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -1,6 +1,6 @@ /************************************************************************** * NAME: pid.c * -* AUTHOR: Johan Gärtner * +* AUTHOR: Johan G�rtner * * * PURPOSE: This file contains pid functions * * INFORMATION: pidUAV is the final pid controller which only takes * @@ -114,9 +114,9 @@ float convertData(int inputRange, int outputRange, int offset, float value) float constrainf(float amt, int low, int high) { if (amt < low) - return low; + return (float)low; else if (amt > high) - return high; + return (float)high; else return amt; } @@ -469,7 +469,7 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID) case PID_ID_GYRO: PidProfileBuff[ID].DOF = 3; - PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms break; case PID_ID_ACCELEROMETER: @@ -631,7 +631,7 @@ void pidEproom(void) PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; - PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms + PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; From 40454ae608a73e766c787cf5af8404b5d488891b Mon Sep 17 00:00:00 2001 From: mdhsweden Date: Mon, 31 Oct 2016 19:07:45 +0100 Subject: [PATCH 06/24] Casts added to several functions in PID section Signed-off-by: mdhsweden --- UAV-ControlSystem/src/Flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index b7fee19..24a8875 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -104,7 +104,6 @@ float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, co float convertData(int inputRange, int outputRange, int offset, float value) { return ((float)outputRange/(float)inputRange)*(value-(float)offset); - //return 1.0; } /************************************************************************** @@ -113,9 +112,9 @@ float convertData(int inputRange, int outputRange, int offset, float value) **************************************************************************/ float constrainf(float amt, int low, int high) { - if (amt < low) + if (amt < (float)low) return (float)low; - else if (amt > high) + else if (amt > (float)high) return (float)high; else return amt; @@ -313,7 +312,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I) moved before integration to make limiting independent from PID settings - ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); + ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection if (motorLimitReached) @@ -363,8 +362,9 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, { ITerm = 0; pidProfileBuff->lastITerm[axis] = 0; + pidProfileBuff->ITermLimit[axis] = 0; } - pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit); + pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); } /************************************************************************** From 1c0c7fb00f71e3ab57ec88a5aa35e2ae9dc0a1ce Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 09:23:40 +0100 Subject: [PATCH 07/24] First drafto of new parser --- UAV-ControlSystem/inc/drivers/arduino_com.h | 49 +++- UAV-ControlSystem/src/drivers/arduino_com.c | 284 +++++++++++++------- UAV-ControlSystem/src/main.c | 4 +- 3 files changed, 231 insertions(+), 106 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 5b4321d..03641cf 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -11,8 +11,55 @@ #include "drivers/usart.h" #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); +/*********************************************************************** +* 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_ */ diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index a8a0834..68dd612 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -19,25 +19,6 @@ enum packet_ids { 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 { uint8_t size; //Size of the data void * dataPtr; //pointer to the data @@ -51,22 +32,32 @@ enum arduino_data_e { arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { [COMPASS_DATA_ID] = { - .size = sizeof(compass_data), + .size = 8, .dataPtr = &compass_data, }, [GPS_DATA_ID] = { - .size = sizeof(gps_data), + .size = 10, .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 */ @@ -75,6 +66,11 @@ bool arduino_frame_available() 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 = { @@ -82,6 +78,7 @@ arduino_data_t find_packet_from_header(uint8_t header) .size = 0, }; + /* Check what header it is and return the correct datapointer if correct header*/ switch (header) { case COMPASS_PACKET_ID: @@ -96,6 +93,83 @@ arduino_data_t find_packet_from_header(uint8_t header) 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() { static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; @@ -103,96 +177,98 @@ 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_header = 0; static uint8_t crc = 0; - static arduino_data_t msg_header_and_size = {0}; - arduino_frame_available(); + static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; + static bool find_new_header = true; if (raw_dma_data_t.new_data) { - 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]; - msg_header_and_size = find_packet_from_header(msg); - - // Look for the beginning of a frame - if ( message_it == 0 ) - { - - - - if (msg_header_and_size.size != 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 - { - if (msg_header_and_size.size != 0 && 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) - { - 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 CRC does not match */ - else - { - int temp_secondaryHeader = message_it_secondary_head; - message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array - 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 < ARDUINO_DMA_SIZE; i++) - { - int innerCount = i-temp_secondaryHeader; - arduino_arr[innerCount] = arduino_arr[i]; - - //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; - } - } - - } - } - } +// uint8_t msg = raw_dma_data_t.buff[i]; //raw_dma_data_t.buff[i]; +// if(find_new_header) +// { +// msg_header_and_size = find_packet_from_header(msg); +// if(msg_header_and_size.size != 0) +// { +// find_new_header = false; +// arduino_arr[(message_it)] = msg; +// message_it++; +// new_header = false; // Just received one +// crc ^= msg; +// } +// } +// // Look for the end of sbus frame +// 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(msg)).size != 0 && 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 except the crc byte */ +// if ((message_it) < (msg_header_and_size.size - 1)) +// { +// arduino_arr[(message_it)] = msg; +// crc ^= msg; +// 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)] = msg; +// missedMsg++; +// +// /* TODO: Replace with check for CRC */ +// if (crc == msg) +// { +// /* Clear necessary variables in order to fill the buffer with new ones */ +// message_it = 0; +// find_new_header = true; +// missedMsg--; +// crc = 0; +// +// memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size); +// } +// +// /* If CRC does not match */ +// else +// { +// +// int temp_secondaryHeader = message_it_secondary_head; +// int length = msg_header_and_size.size; +// int new_struct_size = 0; +// message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array +// +// if(new_header) +// msg_header_and_size = find_packet_from_header(arduino_arr[temp_secondaryHeader]); +// +// /* Clear necessary variables so that a new message can be read into the buffer correctly */ +// new_header = false; //set new header to false, this is true if there is another header within the buffer +// crc = 0; +// +// //Move all the remaning messages in the buffer to the start of the buffer +// for (int j = temp_secondaryHeader; j < length; j++) +// { +// int innerCount = j - temp_secondaryHeader; +// arduino_arr[innerCount] = arduino_arr[j]; +// +// crc ^= arduino_arr[j]; +// +// //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; +// } +// } +// } +// } +// } } } } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 45675e3..438106f 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -61,7 +61,9 @@ void init_system() // TODO!! TEMP code arduinoCom_init(USART1); while (1) { - arduino_read(); + if(arduino_frame_available()) + arduino_read(); + HAL_Delay(15); } From 50bc7bac304c408d96b7aef0442177a5232736e3 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 09:34:57 +0100 Subject: [PATCH 08/24] Arduino com working with parser and everything (hopefully) --- UAV-ControlSystem/inc/drivers/arduino_com.h | 13 +++ UAV-ControlSystem/src/drivers/arduino_com.c | 101 ++------------------ UAV-ControlSystem/src/main.c | 12 +-- 3 files changed, 28 insertions(+), 98 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 03641cf..64ce846 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -63,3 +63,16 @@ bool arduino_frame_available(); 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; +//} diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 68dd612..d09c013 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -93,6 +93,13 @@ arduino_data_t find_packet_from_header(uint8_t header) 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]; @@ -172,103 +179,13 @@ void arduino_parse_message(uint8_t data) ***********************************************************************/ void arduino_read() { - static uint8_t arduino_arr[ARDUINO_DMA_SIZE]; - static uint8_t message_it = 0; - static uint32_t missedMsg = 0; - static uint8_t message_it_secondary_head = 0; - static bool new_header = false; - static uint8_t crc = 0; - static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; - static bool find_new_header = true; - + //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]); - -// uint8_t msg = raw_dma_data_t.buff[i]; //raw_dma_data_t.buff[i]; -// if(find_new_header) -// { -// msg_header_and_size = find_packet_from_header(msg); -// if(msg_header_and_size.size != 0) -// { -// find_new_header = false; -// arduino_arr[(message_it)] = msg; -// message_it++; -// new_header = false; // Just received one -// crc ^= msg; -// } -// } -// // Look for the end of sbus frame -// 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(msg)).size != 0 && 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 except the crc byte */ -// if ((message_it) < (msg_header_and_size.size - 1)) -// { -// arduino_arr[(message_it)] = msg; -// crc ^= msg; -// 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)] = msg; -// missedMsg++; -// -// /* TODO: Replace with check for CRC */ -// if (crc == msg) -// { -// /* Clear necessary variables in order to fill the buffer with new ones */ -// message_it = 0; -// find_new_header = true; -// missedMsg--; -// crc = 0; -// -// memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size); -// } -// -// /* If CRC does not match */ -// else -// { -// -// int temp_secondaryHeader = message_it_secondary_head; -// int length = msg_header_and_size.size; -// int new_struct_size = 0; -// message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array -// -// if(new_header) -// msg_header_and_size = find_packet_from_header(arduino_arr[temp_secondaryHeader]); -// -// /* Clear necessary variables so that a new message can be read into the buffer correctly */ -// new_header = false; //set new header to false, this is true if there is another header within the buffer -// crc = 0; -// -// //Move all the remaning messages in the buffer to the start of the buffer -// for (int j = temp_secondaryHeader; j < length; j++) -// { -// int innerCount = j - temp_secondaryHeader; -// arduino_arr[innerCount] = arduino_arr[j]; -// -// crc ^= arduino_arr[j]; -// -// //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; -// } -// } -// } -// } -// } } } } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 438106f..b683c4c 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -59,12 +59,12 @@ void init_system() //sbus_init(); // TODO!! TEMP code - arduinoCom_init(USART1); - while (1) { - if(arduino_frame_available()) - arduino_read(); - HAL_Delay(15); - } +// arduinoCom_init(USART1); +// while (1) { +// if(arduino_frame_available()) +// arduino_read(); +// HAL_Delay(15); +// } //init motors to run with oneshot 125, small delay From 1b601f153d274f508d97f857b3965e1906b9af6b Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 09:44:41 +0100 Subject: [PATCH 09/24] Fixes for the master --- UAV-ControlSystem/src/drivers/arduino_com.c | 1 + UAV-ControlSystem/src/main.c | 11 +---------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index d09c013..8e1394d 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -147,6 +147,7 @@ void arduino_parse_message(uint8_t 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); diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index b683c4c..7b5e199 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -56,16 +56,7 @@ void init_system() cliInit(USART3); //init sbus, using USART1 - //sbus_init(); - - // TODO!! TEMP code -// arduinoCom_init(USART1); -// while (1) { -// if(arduino_frame_available()) -// arduino_read(); -// HAL_Delay(15); -// } - + sbus_init(); //init motors to run with oneshot 125, small delay HAL_Delay(1000); From c6d50810cb1c5380546f331afed29cb0e2134960 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 1 Nov 2016 10:03:35 +0100 Subject: [PATCH 10/24] PID take away antiwindup protection --- UAV-ControlSystem/src/Flight/pid.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 24a8875..97a78f8 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -22,7 +22,7 @@ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ -#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ +#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ #define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ #define RADIO_RANGE 500 /*Radio range input*/ @@ -31,7 +31,7 @@ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ -#define PID_MAX_I 256 /*Constrains ITerm*/ +#define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ /*Struct that belongs to a certain PID controller*/ @@ -315,14 +315,14 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection - if (motorLimitReached) - { - ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); - } - else - { - pidProfileBuff->ITermLimit[axis] = abs(ITerm); - } +// if (motorLimitReached) +// { +// ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); +// } +// else +// { +// pidProfileBuff->ITermLimit[axis] = abs(ITerm); +// } pidProfileBuff->lastITerm[axis] = ITerm; @@ -631,10 +631,10 @@ void pidEproom(void) PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; - PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms - PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000; - PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000; - PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000; + PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms + PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000; + PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; + PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; From be8b35a336f7feb1e33e65ec093c70ae02d212e2 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 1 Nov 2016 10:06:56 +0100 Subject: [PATCH 11/24] fixed commenting --- UAV-ControlSystem/src/drivers/arduino_com.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 8e1394d..594ff57 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -11,32 +11,40 @@ #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 = 8, + .size = COMPASS_PACKET_SIZE, .dataPtr = &compass_data, }, [GPS_DATA_ID] = { - .size = 10, + .size = GPS_PACKET_SIZE, .dataPtr = &gps_data, }, }; From 4accab066097f5036795088bff24f509e49584c4 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 1 Nov 2016 12:18:50 +0100 Subject: [PATCH 12/24] Fixed the problem with windup --- UAV-ControlSystem/inc/Flight/pid.h | 3 +-- UAV-ControlSystem/inc/drivers/motormix.h | 3 +++ UAV-ControlSystem/src/Flight/pid.c | 11 ++++++++++- UAV-ControlSystem/src/drivers/motormix.c | 5 +++-- UAV-ControlSystem/src/main.c | 2 +- 5 files changed, 18 insertions(+), 6 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 39728c7..7213f7d 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -67,8 +67,7 @@ extern float accPitchFineTune; extern accel_t accelProfile; /*Struct profile for input data from sensor*/ -/*Is set in motor mix and used in pidUAVcore and mix */ -bool motorLimitReached; + /************************************************************************** * BRIEF: Initializes PID profiles * diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 85522b9..fc999e7 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -46,6 +46,9 @@ typedef struct { /* Global mixerConfig to bee available to EEPROM */ extern mixerConfig_s mixerConfig; +/*Is set in motor mix and used in pidUAVcore and mix */ +extern bool motorLimitReached; + /*********************************************************************** * BRIEF: The motormixer * * INFORMATION: Sums the output from all control loops and adapts the * diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 97a78f8..87370fd 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -62,6 +62,8 @@ pt1Filter_t accelFilter[2] = {0}; float accRollFineTune = 0; float accPitchFineTune = 0; + + /************************************************************************** * BRIEF: Calculates angle from accelerometer * * INFORMATION: * @@ -324,6 +326,12 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, // pidProfileBuff->ITermLimit[axis] = abs(ITerm); // } + if (motorLimitReached) + { + ITerm = pidProfileBuff->lastITerm[axis]; + } + + pidProfileBuff->lastITerm[axis] = ITerm; @@ -611,7 +619,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) void pidInit() { mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/ - motorLimitReached = false; pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO); pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER); @@ -636,6 +643,8 @@ void pidEproom(void) PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; + PidProfile[PID_ID_GYRO].I[YAW] = 40; + } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 1261837..0def06d 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -42,6 +42,9 @@ /* An array containing the calculated motor outputs */ uint16_t motor_output[MOTOR_COUNT]; +/* Bool to see if motors are maxed out. Stops windup in PID implementation */ +bool motorLimitReached = false; + /* Default values for the mixerConfig */ // TODO: Implement in EEPROM mixerConfig_s mixerConfig = { @@ -102,8 +105,6 @@ void mix() int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; // Import throttle value from remote - // Might be used for some debug if necessary - //static bool motorLimitReached; /* Mixer Full Scale enabled */ if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index f678bab..2c9dfaf 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -60,7 +60,7 @@ void init_system() pidEproom(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART6); + cliInit(USART3); //init sbus, using USART1 sbus_init(); From fbae0cf4528eadf5ee820e63792a321ed6f634c8 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 1 Nov 2016 14:05:52 +0100 Subject: [PATCH 13/24] Fixed abs function for float values This could cause the anti windup to work. Needs testing. --- UAV-ControlSystem/inc/utilities.h | 4 ++++ UAV-ControlSystem/src/Flight/pid.c | 27 +++++++++++++-------------- UAV-ControlSystem/src/main.c | 1 + 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index 446ddea..b8fa784 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -25,6 +25,8 @@ #define maxStringSize_CLI 100 //Max sting size used for the messages in the CLI +#define ABS_FLOAT(x) (((x) < 0)? -(x): (x)) + typedef char typeString[maxStringSize_CLI]; typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr; @@ -81,6 +83,8 @@ uint32_t accumulate(uint32_t list[], int length); ***********************************************************************/ void Error_Handler(void); + + uint8_t reverse(uint8_t byte); int16_t constrain(int16_t value, int16_t min, int16_t max); diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 87370fd..c16c8ec 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -19,6 +19,7 @@ #include #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" +#include "utilities.h" #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ @@ -317,19 +318,19 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection -// if (motorLimitReached) -// { -// ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); -// } -// else -// { -// pidProfileBuff->ITermLimit[axis] = abs(ITerm); -// } + if (motorLimitReached) + { + ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); + } + else + { + pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm); + } - if (motorLimitReached) - { - ITerm = pidProfileBuff->lastITerm[axis]; - } +// if (motorLimitReached) +// { +// ITerm = pidProfileBuff->lastITerm[axis]; +// } pidProfileBuff->lastITerm[axis] = ITerm; @@ -645,6 +646,4 @@ void pidEproom(void) PidProfile[PID_ID_GYRO].I[YAW] = 40; - - } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 2c9dfaf..bdefb67 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -124,6 +124,7 @@ int main(void) //Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler initScheduler(); + while (1) { //Run the scheduler, responsible for distributing all the work of the running system From 0152b1400699953e14e07ee8c0ad4fea8a5194de Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 1 Nov 2016 14:42:14 +0100 Subject: [PATCH 14/24] Better standard value for arm state. Not valid anymore from 1500 as this is default. --- UAV-ControlSystem/src/drivers/failsafe_toggles.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c index 58b00ce..2821950 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -28,7 +28,7 @@ boolFlags_t systemFlags = {{0}}; /* Array of flag configurations. These are values that can be set by RC, should be read from eeprom. */ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { [FLAG_CONFIGURATION_ARM] = { - .minRange = 1500, + .minRange = 1800, .maxRange = 2100, .channelNumber = 8, .flagId = systemFlags_armed_id, From 88f09eef257488a414d8a49b59009597a6bfecdf Mon Sep 17 00:00:00 2001 From: philsson Date: Wed, 2 Nov 2016 09:22:22 +0100 Subject: [PATCH 15/24] First commit to new acc implementation --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 3 + UAV-ControlSystem/src/Flight/pid.c | 71 +++++++++---------- UAV-ControlSystem/src/drivers/accel_gyro.c | 82 ++++++++++++++++++++++ 3 files changed, 117 insertions(+), 39 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index 805964d..d1c6560 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -114,6 +114,7 @@ typedef struct accel_t { int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */ int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */ uint16_t accel1G; /* Sensitivity factor */ + float rollAngle, pitchAngle; } accel_t; /*********************************************************************** @@ -164,6 +165,8 @@ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out); ***********************************************************************/ bool mpu6000_who_am_i(); +void mpu6000_read_angle(accel_t* accel, gyro_t* gyro); + #endif /* DRIVERS_ACCEL_GYRO_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index c16c8ec..cee2fc7 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -65,6 +65,7 @@ float accPitchFineTune = 0; +//TODO: Remove as implemented in accel_gyro /************************************************************************** * BRIEF: Calculates angle from accelerometer * * INFORMATION: * @@ -151,56 +152,48 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ - -// -// if (calcGravity(accelProfile) > 1.15) -// { -// -// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT; -// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT; -// -// } -// else -// { + mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/ float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ - float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); +// float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); +// float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); /*TODO add finetune for roll and pitch*/ - X_roll += accRollFineTune; - X_pitch += accPitchFineTune; +// X_roll += accRollFineTune; +// X_pitch += accPitchFineTune; + + sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; + sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; - oldSensorValueRoll[i] = X_roll; - oldSensorValuePitch[i] = X_pitch; - - float RollValue = 0; - float PitchValue = 0; - - for (int ii = 0; ii < 12; ii++) - { - RollValue = RollValue + oldSensorValueRoll[ii]; - PitchValue = PitchValue + oldSensorValuePitch[ii]; - - } - - - i = (i < 11)? i + 1:0; - - sensorValues[ROLL] = RollValue/12; - sensorValues[PITCH] = PitchValue/12; - - sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1]; +// oldSensorValueRoll[i] = X_roll; +// oldSensorValuePitch[i] = X_pitch; // - oldSensorValue[0] = sensorValues[ROLL]; - oldSensorValue[1] = sensorValues[PITCH]; +// float RollValue = 0; +// float PitchValue = 0; +// +// for (int ii = 0; ii < 12; ii++) +// { +// RollValue = RollValue + oldSensorValueRoll[ii]; +// PitchValue = PitchValue + oldSensorValuePitch[ii]; +// +// } +// +// +// i = (i < 11)? i + 1:0; +// sensorValues[ROLL] = RollValue/12; +// sensorValues[PITCH] = PitchValue/12; +// +// sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0]; +// sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1]; +//// +// oldSensorValue[0] = sensorValues[ROLL]; +// oldSensorValue[1] = sensorValues[PITCH]; +// // sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 759dcc5..e8c7e09 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -7,6 +7,8 @@ #include #include "drivers/spi.h" +#include "utilities.h" +#include "math.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; @@ -514,3 +516,83 @@ bool mpu6000_who_am_i() return false; } + + +/* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */ +void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) +{ + static uint32_t last_micros = 0; // Static stores micros measured from last iteration + uint32_t current_micros = clock_get_us(); + uint32_t delta_t = current_micros - last_micros; + last_micros = current_micros; + + static float lpf_Acc[3] = {0}; + static float smooth[3] = {0}; + + /* We read the accelerometer to get fresh data */ + mpu6000_read_accel(accel); + + /* Filter part */ + + for (int i = 0; i < 3; i ++) + smooth[i] = lpf_Acc[i] / 16; + + lpf_Acc[0] += sqrtf(accel->accelXconv) - smooth[0]; + lpf_Acc[1] += sqrtf(accel->accelYconv) - smooth[1]; + lpf_Acc[2] += sqrtf(accel->accelZconv) - smooth[2]; + + accel->accelXconv = smooth[0] * smooth[0]; + accel->accelYconv = smooth[1] * smooth[1]; + accel->accelZconv = smooth[2] * smooth[2]; + + /* The total magnitude of the acceleration */ + float magnitude = sqrtf( \ + ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ + ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ + ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ + ); + + /* Everything is normal. No outer forces */ + if (0.85 < magnitude && magnitude < 1.15) + { + + //TODO: JOHAN FIXAR! Kolla så det är rätt här hela vägen + // Roll + accel->rollAngle = atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; + + if (accel->rollAngle > 0) + { + if (accel->accelYconv < 0 ) + accel->rollAngle = 180 - accel->rollAngle; + } + else + { + if (accel->accelYconv < 0 ) + accel->rollAngle = - 180 - accel->rollAngle; + } + accel->rollAngle = -accel->rollAngle; + + + // Pitch + accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; + + if (accel->pitchAngle > 0) + { + if (accel->accelYconv < 0) + accel->pitchAngle = 180 - accel->pitchAngle; + } + else + { + if (accel->accelYconv < 0 ) + accel->pitchAngle = - 180 - accel->pitchAngle; + } + + } + /* Too big forces to calculate on ACC data. Fallback on gyro integration */ + else + { + accel->rollAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; + accel->pitchAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; + } + +} From 29e621b18e06def7ed6bf8cf948697197bda51ee Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 2 Nov 2016 11:17:59 +0100 Subject: [PATCH 16/24] added new calcualtions to accel gyro --- UAV-ControlSystem/src/Flight/pid.c | 55 -------------------- UAV-ControlSystem/src/drivers/accel_gyro.c | 60 +++++++++------------- 2 files changed, 24 insertions(+), 91 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index cee2fc7..9b5d0cf 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -154,57 +154,9 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/ - - float alpha = 0.5; - /*May need Low pass filter since the accelerometer may drift*/ - -// float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); -// float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - - /*TODO add finetune for roll and pitch*/ -// X_roll += accRollFineTune; -// X_pitch += accPitchFineTune; - sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; - -// oldSensorValueRoll[i] = X_roll; -// oldSensorValuePitch[i] = X_pitch; -// -// float RollValue = 0; -// float PitchValue = 0; -// -// for (int ii = 0; ii < 12; ii++) -// { -// RollValue = RollValue + oldSensorValueRoll[ii]; -// PitchValue = PitchValue + oldSensorValuePitch[ii]; -// -// } -// -// -// i = (i < 11)? i + 1:0; - -// sensorValues[ROLL] = RollValue/12; -// sensorValues[PITCH] = PitchValue/12; -// -// sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0]; -// sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1]; -//// -// oldSensorValue[0] = sensorValues[ROLL]; -// oldSensorValue[1] = sensorValues[PITCH]; -// - - -// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); -// sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - - //float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - //sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT); - - //float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - //sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT); - break; case PID_ID_COMPASS: case PID_ID_BAROMETER: @@ -307,7 +259,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I) moved before integration to make limiting independent from PID settings ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection @@ -320,12 +271,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm); } -// if (motorLimitReached) -// { -// ITerm = pidProfileBuff->lastITerm[axis]; -// } - - pidProfileBuff->lastITerm[axis] = ITerm; diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index e8c7e09..f9c40c9 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -528,6 +528,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) static float lpf_Acc[3] = {0}; static float smooth[3] = {0}; + float sign[3] = {0}; /* We read the accelerometer to get fresh data */ mpu6000_read_accel(accel); @@ -535,15 +536,26 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) /* Filter part */ for (int i = 0; i < 3; i ++) + { smooth[i] = lpf_Acc[i] / 16; + } - lpf_Acc[0] += sqrtf(accel->accelXconv) - smooth[0]; - lpf_Acc[1] += sqrtf(accel->accelYconv) - smooth[1]; - lpf_Acc[2] += sqrtf(accel->accelZconv) - smooth[2]; + sign[0] = (accel->accelXconv< 0) ? -1 : 1; + lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0]; - accel->accelXconv = smooth[0] * smooth[0]; - accel->accelYconv = smooth[1] * smooth[1]; - accel->accelZconv = smooth[2] * smooth[2]; + + sign[1] = (accel->accelYconv< 0) ? -1 : 1; + lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1]; + + sign[2] = (accel->accelZconv< 0) ? -1 : 1; + lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2]; + + + + + accel->accelXconv = smooth[0] * smooth[0] * sign[0]; + accel->accelYconv = smooth[1] * smooth[1] * sign[1]; + accel->accelZconv = smooth[2] * smooth[2] * sign[2]; /* The total magnitude of the acceleration */ float magnitude = sqrtf( \ @@ -555,44 +567,20 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) /* Everything is normal. No outer forces */ if (0.85 < magnitude && magnitude < 1.15) { - - //TODO: JOHAN FIXAR! Kolla så det är rätt här hela vägen // Roll - accel->rollAngle = atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; - - if (accel->rollAngle > 0) - { - if (accel->accelYconv < 0 ) - accel->rollAngle = 180 - accel->rollAngle; - } - else - { - if (accel->accelYconv < 0 ) - accel->rollAngle = - 180 - accel->rollAngle; - } - accel->rollAngle = -accel->rollAngle; - + accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; // Pitch accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; - - if (accel->pitchAngle > 0) - { - if (accel->accelYconv < 0) - accel->pitchAngle = 180 - accel->pitchAngle; - } - else - { - if (accel->accelYconv < 0 ) - accel->pitchAngle = - 180 - accel->pitchAngle; - } - } /* Too big forces to calculate on ACC data. Fallback on gyro integration */ else { - accel->rollAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; - accel->pitchAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; + accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; + accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; + + accel->rollAngle = constrainf(accel->rollAngle, -90,90); + accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } } From 8bd9dc986d33693aa00e9c294a5830493fb5d721 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 2 Nov 2016 13:39:15 +0100 Subject: [PATCH 17/24] PID deleted rows --- UAV-ControlSystem/src/Flight/pid.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 9b5d0cf..ada9904 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -137,8 +137,6 @@ int i = 0; **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { - - switch (ID_profile) { case PID_ID_GYRO: From 3eeb4d3741c2c680fa3730f8576c37709324eed8 Mon Sep 17 00:00:00 2001 From: philsson Date: Thu, 3 Nov 2016 11:58:51 +0100 Subject: [PATCH 18/24] Remove square root from magnitude calculation on accel_gyro.c Motors start now at 990 instead of 1000 to prevent glitches --- UAV-ControlSystem/src/drivers/accel_gyro.c | 7 +++++-- UAV-ControlSystem/src/drivers/motors.c | 7 ++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index f9c40c9..bb567dc 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -245,6 +245,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) HAL_Delay(60); + accel->pitchAngle = 0; + accel->rollAngle = 0; return true; } @@ -558,14 +560,15 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) accel->accelZconv = smooth[2] * smooth[2] * sign[2]; /* The total magnitude of the acceleration */ - float magnitude = sqrtf( \ + float magnitude = ( \ ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ ); /* Everything is normal. No outer forces */ - if (0.85 < magnitude && magnitude < 1.15) + if (0.81 < magnitude && magnitude < 1.21) + //if (false) { // Roll accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index 53bb569..1a44d5c 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -14,6 +14,7 @@ #include "drivers/motors.h" #include "drivers/failsafe_toggles.h" #include "config/eeprom.h" +#include "drivers/motormix.h" const int MotorPWMPeriode = 2000; //Micro seconds const int MotorPWMInitPulse = 1000; @@ -162,7 +163,11 @@ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput) **************************************************************************/ void pwmEnableAllMotors(motorOutput motorOutput) { - for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput); + for (uint8_t i = 1; i < 11; i++ ) + { + pwmEnableMotor(i, motorOutput); + pwmAdjustSpeedOfMotor(i,mixerConfig.minCommand); + } } /************************************************************************** From 280240815da22eec5639719d03a0caa9e56a4dfa Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 10:43:43 +0100 Subject: [PATCH 19/24] Possible fix to the accelerometer problem --- UAV-ControlSystem/inc/utilities.h | 5 ++ UAV-ControlSystem/src/Flight/pid.c | 13 ---- UAV-ControlSystem/src/drivers/accel_gyro.c | 87 ++++++++++++---------- UAV-ControlSystem/src/tasks_main.c | 1 + UAV-ControlSystem/src/utilities.c | 14 ++++ 5 files changed, 68 insertions(+), 52 deletions(-) diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index b8fa784..1f0bba8 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -83,6 +83,11 @@ uint32_t accumulate(uint32_t list[], int length); ***********************************************************************/ void Error_Handler(void); +/************************************************************************** +* BRIEF: Constrain float values within a defined limit * +* INFORMATION: Used in PID loop to limit values * +**************************************************************************/ +float constrainf(float amt, int low, int high); uint8_t reverse(uint8_t byte); diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index ada9904..8274506 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -110,19 +110,6 @@ float convertData(int inputRange, int outputRange, int offset, float value) return ((float)outputRange/(float)inputRange)*(value-(float)offset); } -/************************************************************************** -* BRIEF: Constrain float values within a defined limit * -* INFORMATION: Used in PID loop to limit values * -**************************************************************************/ -float constrainf(float amt, int low, int high) -{ - if (amt < (float)low) - return (float)low; - else if (amt > (float)high) - return (float)high; - else - return amt; -} float oldSensorValue[2] = {0}; diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index f9c40c9..dae9b97 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -9,6 +9,7 @@ #include "drivers/spi.h" #include "utilities.h" #include "math.h" +#include "drivers/system_clock.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; @@ -518,6 +519,17 @@ bool mpu6000_who_am_i() } + + + +/* Set the Gyro Weight for Gyro/Acc complementary filter + Increasing this value would reduce and delay Acc influence on the output of the filter*/ +#define GYRO_ACC_DIV_FACTOR (2^6) // that means a CMP_FACTOR of 1024 (2^10) + +#define GetMagnitude(x) (x*x) +#define Low_Magnitude (GetMagnitude(0.85)) +#define High_Magnitude (GetMagnitude(1.15)) + /* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) { @@ -526,61 +538,58 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) uint32_t delta_t = current_micros - last_micros; last_micros = current_micros; + float deltaGyroAngleFloat[3] = {0}; static float lpf_Acc[3] = {0}; static float smooth[3] = {0}; float sign[3] = {0}; + float magnitude = 0; /* We read the accelerometer to get fresh data */ mpu6000_read_accel(accel); + float accelConv[3] = {accel->accelXconv, accel->accelYconv, accel->accelZconv}; - /* Filter part */ - + /* Filter part, go thorugh each axis */ for (int i = 0; i < 3; i ++) { + //Calculate a new smooth value based on a factor of the LPF value smooth[i] = lpf_Acc[i] / 16; + + //Save the sign(+/-) of the value + sign[i] = (accelConv[i]< 0) ? -1 : 1; + + //Calculate the new LPF value based on the raw sensor data - the smoothing value + lpf_Acc[i] += sign[i]*sqrtf(ABS_FLOAT(accelConv[i])) - smooth[i]; + + //recalculate the accelerometer data based on the smooth value, since we had to take the square root off the original value we square it to get in the original size + accelConv[i] = smooth[i] * smooth[i] * sign[i]; + + //calculate the magnitude of the gravitation for all axis + magnitude += ABS_FLOAT(accelConv[i]) * ABS_FLOAT(accelConv[i]); + } - sign[0] = (accel->accelXconv< 0) ? -1 : 1; - lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0]; + //Calculate the approximate angle increase based on the gyros probable movement since the last invoke + deltaGyroAngleFloat[0] = (delta_t * (float)gyro->gyroX / 1000000.0); + deltaGyroAngleFloat[1] = (delta_t * (float)gyro->gyroY / 1000000.0) ; + deltaGyroAngleFloat[2] = (delta_t * (float)gyro->gyroZ / 1000000.0); + //First integrate the gyro and add that to the angle calculation + accel->rollAngle += deltaGyroAngleFloat[1]; + accel->pitchAngle += deltaGyroAngleFloat[0]; - sign[1] = (accel->accelYconv< 0) ? -1 : 1; - lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1]; - - sign[2] = (accel->accelZconv< 0) ? -1 : 1; - lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2]; - - - - - accel->accelXconv = smooth[0] * smooth[0] * sign[0]; - accel->accelYconv = smooth[1] * smooth[1] * sign[1]; - accel->accelZconv = smooth[2] * smooth[2] * sign[2]; - - /* The total magnitude of the acceleration */ - float magnitude = sqrtf( \ - ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ - ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ - ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ - ); - - /* Everything is normal. No outer forces */ - if (0.85 < magnitude && magnitude < 1.15) + //If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation + if (Low_Magnitude < magnitude && magnitude < High_Magnitude) { - // Roll - accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; + //Calculate the pure angle given by the accelerometer data + float a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI; + float a_PitchAngle = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI; - // Pitch - accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; - } - /* Too big forces to calculate on ACC data. Fallback on gyro integration */ - else - { - accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; - accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; - - accel->rollAngle = constrainf(accel->rollAngle, -90,90); - accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); + //Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value + accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; + accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; } + //Always constran the angular values within the possible ranges 90 to -90 + accel->rollAngle = constrainf(accel->rollAngle, -90,90); + accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 4392405..a3293ae 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -64,6 +64,7 @@ void systemTaskGyroPid(void) void systemTaskAccelerometer(void) { + flags_Set_ID(systemFlags_flightmode_acceleromter_id); pidRun(PID_ID_ACCELEROMETER); //update the accelerometer data // uint8_t c = 97; diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c index 7c2192c..4e824d0 100644 --- a/UAV-ControlSystem/src/utilities.c +++ b/UAV-ControlSystem/src/utilities.c @@ -228,3 +228,17 @@ int16_t constrain(int16_t value, int16_t min, int16_t max) else return value; } + +/************************************************************************** +* BRIEF: Constrain float values within a defined limit * +* INFORMATION: Used in PID loop to limit values * +**************************************************************************/ +float constrainf(float amt, int low, int high) +{ + if (amt < (float)low) + return (float)low; + else if (amt > (float)high) + return (float)high; + else + return amt; +} From e22ad43c3926d7b15dc942f1f21532ee6d8ac82b Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 13:42:43 +0100 Subject: [PATCH 20/24] Removed very very very small amount of stuff --- UAV-ControlSystem/src/Flight/pid.c | 1 - UAV-ControlSystem/src/tasks_main.c | 1 - 2 files changed, 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 8274506..6ef9db6 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -567,6 +567,5 @@ void pidEproom(void) PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; - PidProfile[PID_ID_GYRO].I[YAW] = 40; } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index a3293ae..4392405 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -64,7 +64,6 @@ void systemTaskGyroPid(void) void systemTaskAccelerometer(void) { - flags_Set_ID(systemFlags_flightmode_acceleromter_id); pidRun(PID_ID_ACCELEROMETER); //update the accelerometer data // uint8_t c = 97; From e09c69fbaab847c8001aaeb8178a944ff2019047 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 16:57:59 +0100 Subject: [PATCH 21/24] small things --- UAV-ControlSystem/src/Flight/pid.c | 2 -- UAV-ControlSystem/src/config/cli.c | 12 ++++++------ UAV-ControlSystem/src/drivers/accel_gyro.c | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 6ef9db6..d4cc9d5 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -558,8 +558,6 @@ void pidInit() void pidEproom(void) { - PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; - PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 1d7cb04..5f5bee6 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -31,7 +31,7 @@ #define commandValueError 0xFFFFFFFFFFFFFFFF #define minSimilarCharacters 2 //the minimum amount of characters needed to to a search on a value -#define maxSimilarSearchValues 15 //max amount of values that will be found when doing a search for similar strings based on the written chars +#define maxSimilarSearchValues 18 //max amount of values that will be found when doing a search for similar strings based on the written chars #define CLI_baudRate 115200 //The baudrate used for the CLI usart #define msgArraySize 3 //The number of words that a max command can contain (ex: set looptime 1000) @@ -121,11 +121,11 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| reboot - Exit CLI and reboots the system.\n\r", "| reset - Restore all the values to its default values.\n\r", "| stats - Gives some current stats of the system and tasks.\n\r", - "| calibrate_motors - Calibrates all motors.", - "| calibrate_gyro - Calibrates the gyro.", - "| calibrate_accelerometer - Calibrates the accelerometer.", - "| calibrate_compass - Calibrates the compass.", - "| calibration_info_accelerometer - Provides info on the accelerometer calibration." + "| calibrate_motors - Calibrates all motors.\n\r", + "| calibrate_gyro - Calibrates the gyro.\n\r", + "| calibrate_accelerometer - Calibrates the accelerometer.\n\r", + "| calibrate_compass - Calibrates the compass.\n\r", + "| calibration_info_accelerometer - Provides info on the accelerometer calibration.\n\r" }; /* String array containing all the signature examples for each action */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index bc019df..471f835 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -526,7 +526,7 @@ bool mpu6000_who_am_i() /* Set the Gyro Weight for Gyro/Acc complementary filter Increasing this value would reduce and delay Acc influence on the output of the filter*/ -#define GYRO_ACC_DIV_FACTOR (2^6) // that means a CMP_FACTOR of 1024 (2^10) +#define GYRO_ACC_DIV_FACTOR (2^8) // that means a CMP_FACTOR of 1024 (2^10) #define GetMagnitude(x) (x*x) #define Low_Magnitude (GetMagnitude(0.85)) From 70b70fc5b63b8d26ea29da4ce2c205b9af20fde7 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 16:58:07 +0100 Subject: [PATCH 22/24] ops --- UAV-ControlSystem/src/Flight/pid.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index d4cc9d5..13bd3e9 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -558,6 +558,8 @@ void pidInit() void pidEproom(void) { + PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 4; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 4; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms From 9d1b71fabc6b192616e4ba9eaf44aeebaeb0a6d1 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 08:35:37 +0100 Subject: [PATCH 23/24] PID update, deleted code overflow --- UAV-ControlSystem/src/Flight/pid.c | 45 ------------------------------ 1 file changed, 45 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 13bd3e9..3503841 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -64,43 +64,6 @@ float accRollFineTune = 0; float accPitchFineTune = 0; - -//TODO: Remove as implemented in accel_gyro -/************************************************************************** -* BRIEF: Calculates angle from accelerometer * -* INFORMATION: * -**************************************************************************/ -float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis) -{ - float angle; - - switch (axis) - { - case ROLL: - - angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI; - angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle); - - break; - case PITCH: - - angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/ - angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle; - - break; - default: - angle = 0; - break; - } - - return angle; -} - -float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis) -{ - return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv); -} - /************************************************************************** * BRIEF: Scales data from input range to output range * * INFORMATION: * @@ -110,14 +73,6 @@ float convertData(int inputRange, int outputRange, int offset, float value) return ((float)outputRange/(float)inputRange)*(value-(float)offset); } - - -float oldSensorValue[2] = {0}; -float oldSensorValueRoll[12] = {0}; -float oldSensorValuePitch[12] = {0}; - -int i = 0; - /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * From c791a9ea3bce410735f8d3efbbeed9f6b41fd123 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 10:34:03 +0100 Subject: [PATCH 24/24] PID added barometer settings to CLI, eeprom and PID --- UAV-ControlSystem/inc/config/eeprom.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 24 ++++++++++--- UAV-ControlSystem/src/config/cli.c | 52 +++++++++++++++++++++++++++ UAV-ControlSystem/src/config/eeprom.c | 10 ++++++ 4 files changed, 84 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 1774a35..7c1530c 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -208,6 +208,8 @@ typedef enum { typedef enum { EEPROM_PID_GYRO, EEPROM_PID_ACCELEROMETER, + EEPROM_PID_COMPASS, + EEPROM_PID_BAROMETER, /* Counts the amount of settings in profile */ EEPROM_PROFILE_COUNT diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 3503841..162ceed 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -27,7 +27,7 @@ #define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ #define RADIO_RANGE 500 /*Radio range input*/ -#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ +#define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ #define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ @@ -60,6 +60,7 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso pt1Filter_t accelFilter[2] = {0}; +float throttleRate = 0; float accRollFineTune = 0; float accPitchFineTune = 0; @@ -97,9 +98,22 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; + /*Checks the biggest angle */ + throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180); + break; case PID_ID_COMPASS: + + sensorValues[ROLL] = 0; + sensorValues[PITCH] = 0; + sensorValues[YAW] = 0; + + + break; case PID_ID_BAROMETER: + + + break; default: sensorValues[ROLL] = 0; @@ -157,7 +171,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) break; case PID_ID_BAROMETER: - desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle); + //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); + + desiredCommand[THROTTLE] = 1.9; break; default: @@ -324,9 +340,9 @@ void pidRun(uint8_t ID) break; case PID_ID_BAROMETER: - if (!PidProfile[PID_ID_BAROMETER].pidEnabled) + if (!PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) { - PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle; + PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate; } else { diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 5f5bee6..d77f8a9 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -284,9 +284,22 @@ typedef enum COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, COMMAND_ID_PID_ACCEL_OUT_LIMIT, + //Barometer + COMMAND_ID_PID_BARO_P_HEIGHT, + + COMMAND_ID_PID_BARO_I_HEIGHT, + + COMMAND_ID_PID_BARO_D_HEIGHT, + + COMMAND_ID_PID_BARO_DTERM_LPF, + COMMAND_ID_PID_BARO_PTERM_YAW_LPF, + COMMAND_ID_PID_BARO_YAW_P_LIMIT, + COMMAND_ID_PID_BARO_OUT_LIMIT, + /* Enable the different pid loops */ COMMAND_ID_PID_GYRO_ISENABLED, COMMAND_ID_PID_ACCEL_ISENABLED, + COMMAND_ID_PID_BARO_ISENABLED, /* Counter for the amount of commands */ COMMAND_ID_COUNT, @@ -659,6 +672,41 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "pid_accel_out_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} }, + //BARO P + [COMMAND_ID_PID_BARO_P_HEIGHT] = + { + "pid_baro_p_height", COMMAND_ID_PID_BARO_P_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255} + }, + + //BARO I + [COMMAND_ID_PID_ACCEL_I_PITCH] = + { + "pid_baro_i_height", COMMAND_ID_PID_BARO_I_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255} + }, + //BARO D + [COMMAND_ID_PID_ACCEL_D_PITCH] = + { + "pid_baro_d_height", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255} + }, + + //BARO Filters and limits + [COMMAND_ID_PID_BARO_DTERM_LPF] = + { + "pid_baro_dterm_lpf", COMMAND_ID_PID_BARO_DTERM_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_PTERM_YAW_LPF] = + { + "pid_baro_pterm_yaw_lpf", COMMAND_ID_PID_BARO_PTERM_YAW_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_YAW_P_LIMIT] = + { + "pid_baro_yaw_p_limit", COMMAND_ID_PID_BARO_YAW_P_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_OUT_LIMIT] = + { + "pid_baro_out_limit", COMMAND_ID_PID_BARO_OUT_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} + }, + /* Enable pid loops */ [COMMAND_ID_PID_GYRO_ISENABLED] = { @@ -668,6 +716,10 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { { "pid_accel_isenabled", COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} }, + [COMMAND_ID_PID_BARO_ISENABLED] = + { + "pid_baro_isenabled", COMMAND_ID_PID_BARO_ISENABLED, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} + }, }; diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 7852cc1..d727865 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -291,6 +291,16 @@ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { .size = sizeof(pidProfile_t), .dataPtr = &(PidProfile[PID_ID_ACCELEROMETER]), }, + [EEPROM_PID_COMPASS] = + { + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_COMPASS]), + }, + [EEPROM_PID_BAROMETER] = + { + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_BAROMETER]), + }, }; /* Data pointers and sizes for footer content */