From e2c8ffee5ada53e1f08253f2dd82e2e55985ceda Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Wed, 23 Nov 2016 13:56:55 +0100 Subject: [PATCH 1/7] Got compass to work again --- UAV-ControlSystem/inc/Flight/pid.h | 2 + UAV-ControlSystem/inc/Scheduler/scheduler.h | 1 + UAV-ControlSystem/inc/Scheduler/tasks.h | 1 + UAV-ControlSystem/inc/config/eeprom.h | 1 + UAV-ControlSystem/inc/system_variables.h | 2 +- UAV-ControlSystem/src/Flight/pid.c | 75 +++++++++++++++++++-- UAV-ControlSystem/src/Scheduler/scheduler.c | 2 + UAV-ControlSystem/src/Scheduler/tasks.c | 8 +++ UAV-ControlSystem/src/config/cli.c | 5 ++ UAV-ControlSystem/src/config/eeprom.c | 6 ++ UAV-ControlSystem/src/drivers/arduino_com.c | 22 +++--- UAV-ControlSystem/src/drivers/usart.c | 2 +- UAV-ControlSystem/src/main.c | 6 +- UAV-ControlSystem/src/tasks_main.c | 7 ++ 14 files changed, 121 insertions(+), 19 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 7213f7d..ee9b57e 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -67,6 +67,8 @@ extern float accPitchFineTune; extern accel_t accelProfile; /*Struct profile for input data from sensor*/ +extern float Yaw; +extern float YawU; /************************************************************************** diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index 945f953..c1dae17 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -107,6 +107,7 @@ typedef enum TASK_RX_CLI, TASK_SERIAL, TASK_BATTERY, + TASK_ARDUINO, #ifdef BARO TASK_BARO, #endif diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index 2a6c79c..cbe0756 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -38,6 +38,7 @@ void systemTaskRxCli(void); bool systemTaskRxCliCheck(uint32_t currentDeltaTime); void systemTaskSerial(void); void systemTaskBattery(void); +void systemTaskArduino(void); void systemTaskBaro(void); void systemTaskCompass(void); void systemTaskGps(void); diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 1774a35..c1a66b0 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -159,6 +159,7 @@ typedef enum { EEPROM_PERIOD_RX_CLI, EEPROM_PERIOD_SERIAL, EEPROM_PERIOD_BATTERY, + EEPROM_PERIOD_ARDUINO, #ifdef BARO EEPROM_PERIOD_BARO, #endif diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 0217fd9..b570055 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -14,7 +14,7 @@ #ifndef SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_ -#define EEPROM_SYS_VERSION 107 +#define EEPROM_SYS_VERSION 111 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index c16c8ec..1d5d870 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -20,6 +20,10 @@ #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" #include "utilities.h" +#include "drivers/arduino_com.h" + +#define sq(x) ((x)*(x)) +#define map(x, in_min, in_max, out_min, out_max) (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ @@ -63,7 +67,16 @@ pt1Filter_t accelFilter[2] = {0}; float accRollFineTune = 0; float accPitchFineTune = 0; - +float oldSensorValue[2] = {0}; +float oldSensorValueRoll[12] = {0}; +float oldSensorValuePitch[12] = {0}; +float MagnetFilteredOld[3]; +float alphaMagnet = 0.4; +int MagnetMax[3]; +int MagnetMin[3]; +float MagnetMap[3]; +float Yaw; +float YawU; /************************************************************************** * BRIEF: Calculates angle from accelerometer * @@ -124,9 +137,6 @@ float constrainf(float amt, int low, int high) } -float oldSensorValue[2] = {0}; -float oldSensorValueRoll[12] = {0}; -float oldSensorValuePitch[12] = {0}; int i = 0; @@ -214,6 +224,59 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_COMPASS: + { + float xMagnetFiltered = 0; + float yMagnetFiltered = 0; + float zMagnetFiltered = 0; + xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]); + yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]); + zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]); + + MagnetFilteredOld[0] = xMagnetFiltered; + MagnetFilteredOld[1] = yMagnetFiltered; + MagnetFilteredOld[2] = zMagnetFiltered; + + + //this part is required to normalize the magnetic vector + if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; } + if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; } + if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; } + + if (xMagnetFiltered Date: Fri, 25 Nov 2016 11:36:11 +0100 Subject: [PATCH 2/7] Compass starting to be better --- UAV-ControlSystem/.custom.cfg | 13 ++ UAV-ControlSystem/inc/drivers/arduino_com.h | 18 +-- UAV-ControlSystem/src/Flight/pid.c | 127 ++++++++++---------- 3 files changed, 83 insertions(+), 75 deletions(-) create mode 100644 UAV-ControlSystem/.custom.cfg diff --git a/UAV-ControlSystem/.custom.cfg b/UAV-ControlSystem/.custom.cfg new file mode 100644 index 0000000..c77723e --- /dev/null +++ b/UAV-ControlSystem/.custom.cfg @@ -0,0 +1,13 @@ +# This is an .custom board with a single STM32F411RETx chip. +# Generated by System Workbench for STM32 + +source [find interface/stlink-v2-1.cfg] + +set WORKAREASIZE 0x20000 +transport select "hla_swd" + + +source [find target/stm32f4x_stlink.cfg] + +# use hardware reset, connect under reset +reset_config srst_only srst_nogate diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 627692a..e9d739c 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -19,11 +19,11 @@ * 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; + uint8_t header __attribute__((packed)); + int16_t x __attribute__((packed)); + int16_t y __attribute__((packed)); + int16_t z __attribute__((packed)); + uint8_t crc __attribute__((packed)); } compass_data_t; /*********************************************************************** @@ -31,10 +31,10 @@ typedef struct compass_data_t { * INFORMATION: Contains the whole gps data message * ***********************************************************************/ typedef struct gps_data_t { - uint8_t header; - float latitude; - float longitude; - uint8_t crc; + uint8_t header __attribute__((packed)); + float latitude; __attribute__((packed)) + float longitude __attribute__((packed)); + uint8_t crc __attribute__((packed)); } gps_data_t; /* An instance of the GPS data read from Arduino Com */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 9585fb2..d7e44ea 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -133,7 +133,7 @@ float convertData(int inputRange, int outputRange, int offset, float 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) +float constrainfu(float amt, int low, int high) { if (amt < (float)low) return (float)low; @@ -181,14 +181,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) /*Checks the biggest angle */ throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - 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: @@ -208,59 +200,62 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_COMPASS: { - float xMagnetFiltered = 0; - float yMagnetFiltered = 0; - float zMagnetFiltered = 0; - xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]); - yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]); - zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]); - - MagnetFilteredOld[0] = xMagnetFiltered; - MagnetFilteredOld[1] = yMagnetFiltered; - MagnetFilteredOld[2] = zMagnetFiltered; - - - //this part is required to normalize the magnetic vector - if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; } - if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; } - if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; } - - if (xMagnetFilteredMagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; } +// if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; } +// if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; } +// +// if (xMagnetFilteredyaw_p_limit) { - PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + PTerm = constrainfu(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } } @@ -368,12 +363,12 @@ 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. - ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); + ITerm = constrainfu(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection if (motorLimitReached) { - ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); + ITerm = constrainfu(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); } else { @@ -409,7 +404,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, } DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0; - DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); + DTerm = constrainfu(DTerm, -PID_MAX_D, PID_MAX_D); } @@ -420,7 +415,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, pidProfileBuff->lastITerm[axis] = 0; pidProfileBuff->ITermLimit[axis] = 0; } - pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); + pidProfile->PID_Out[axis] = constrainfu(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); } /************************************************************************** @@ -522,11 +517,11 @@ void pidRun(uint8_t ID) else { pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]); - PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); + PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); - //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); - //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); + //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); + //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); } break; From 3200feaa26cf7c425ad9f75cfd35b442bdd8473a Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Mon, 28 Nov 2016 08:54:46 +0100 Subject: [PATCH 3/7] Temp checkin --- UAV-ControlSystem/inc/drivers/arduino_com.h | 6 +++--- UAV-ControlSystem/inc/drivers/compass.h | 11 +++++++++++ UAV-ControlSystem/src/drivers/compass.c | 14 ++++++++++++++ 3 files changed, 28 insertions(+), 3 deletions(-) create mode 100644 UAV-ControlSystem/inc/drivers/compass.h create mode 100644 UAV-ControlSystem/src/drivers/compass.c diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 09d3518..ff196a4 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -42,9 +42,9 @@ typedef struct gps_data_t { * INFORMATION: Contains the whole ping sensor data message * ***********************************************************************/ typedef struct ping_data_t { - uint8_t header; - uint16_t distance_mm; - uint8_t crc; + uint8_t header __attribute__((packed)); + uint16_t distance_mm __attribute__((packed)); + uint8_t crc __attribute__((packed)); }ping_data_t; /* An instance of the GPS data read from Arduino Com */ diff --git a/UAV-ControlSystem/inc/drivers/compass.h b/UAV-ControlSystem/inc/drivers/compass.h new file mode 100644 index 0000000..69b1141 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/compass.h @@ -0,0 +1,11 @@ +#ifndef DRIVERS_COMPASS_H +#define DRIVERS_COMPASS_H + + + +bool initialize_compass(); + +void calibrate_compass(); + + +#endif //DRIVERS_COMPASS_H diff --git a/UAV-ControlSystem/src/drivers/compass.c b/UAV-ControlSystem/src/drivers/compass.c new file mode 100644 index 0000000..79f196c --- /dev/null +++ b/UAV-ControlSystem/src/drivers/compass.c @@ -0,0 +1,14 @@ +#include "drivers/compass.h" + +#include "drivers/arduino_com.h" + + +bool initialize_compass() +{ + +} + +void calibrate_compass() +{ + +} From ad58ecee851c98a5d21004379b830eed7b139804 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 29 Nov 2016 09:28:43 +0100 Subject: [PATCH 4/7] Heartbeat implementation --- UAV-ControlSystem/src/drivers/arduino_com.c | 37 +++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 8e0c246..7d97c1b 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -161,6 +161,14 @@ arduino_data_t find_packet_from_header(uint8_t header) } +#define BYTE1_32BITS_VALUE(x) ((x & 0xFF000000) >> 24) +#define BYTE2_32BITS_VALUE(x) ((x & 0x00FF0000) >> 16) +#define BYTE3_32BITS_VALUE(x) ((x & 0x0000FF00) >> 8) +#define BYTE4_32BITS_VALUE(x) ((x & 0x000000FF) >> 0) + +const uint32_t heartbeat_msg = 0xDEADBEEF; +const uint32_t heartbeat_rsp = 0xBA1DFACE; + /*********************************************************************** * 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 @@ -175,6 +183,35 @@ void arduino_parse_message(uint8_t data) static uint8_t secondary_message_it = 0; static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; static uint8_t crc = 0; + static uint8_t heartbeatiterator = 0; + + if (heartbeatiterator == 0 && data == BYTE1_32BITS_VALUE(heartbeat_msg)) + heartbeatiterator = 1; + + if (heartbeatiterator > 0) + { + switch (heartbeatiterator) + { + case 1: + heartbeatiterator = 2; + break; + case 2: + heartbeatiterator = (data == BYTE2_32BITS_VALUE(heartbeat_msg)) ? 3 : 0; + break; + case 3: + heartbeatiterator = (data == BYTE3_32BITS_VALUE(heartbeat_msg)) ? 4 : 0; + break; + case 4: + heartbeatiterator = 0; + if(data == BYTE4_32BITS_VALUE(heartbeat_msg)) + { + usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000); + } + break; + default: + break; + } + } if(find_header) { From d224a2f82d21a9f968121a6cac2f36130b2ef1fb Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 29 Nov 2016 14:55:27 +0100 Subject: [PATCH 5/7] temp --- UAV-ControlSystem/inc/drivers/arduino_com.h | 25 ++++- UAV-ControlSystem/src/Flight/pid.c | 4 +- UAV-ControlSystem/src/drivers/arduino_com.c | 112 +++++++++++++------- 3 files changed, 98 insertions(+), 43 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index ff196a4..cbeabd1 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -74,13 +74,36 @@ bool arduino_frame_available(); ***********************************************************************/ void arduino_read(); - /*********************************************************************** * BRIEF: Update the output sensor values and sends them to the Arduino * * INFORMATION: * ***********************************************************************/ void arduino_send_sensor_values(); +/*********************************************************************** +* BRIEF: Check so that the heartbeat messages are comming with a * +* steady stream * +* INFORMATION: Check the last time a heart beat message was received * +* and checks against a pre defined time before declaring * +* the communication as dead * +***********************************************************************/ +bool arduino_com_alive(); + +/*********************************************************************** +* BRIEF: Set a color on a specific led on the neo ledstrip attached * +* to the arduino * +* INFORMATION: Send a command with the led index and RGB color to the * +* Arduino * +***********************************************************************/ +void arduino_set_led_color(uint8_t index, uint8_t r, uint8_t g, uint8_t b); + +/*********************************************************************** +* BRIEF: Tell the arduino that the FC system is OK and everything is * +* working * +* INFORMATION: Set the datavalue to 0xBA1DFACE for the led data * +***********************************************************************/ +void arduino_im_ok(); + #endif /* DRIVERS_ARDUINO_COM_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index d7e44ea..3152743 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -481,7 +481,7 @@ void pidRun(uint8_t ID) break; case PID_ID_ACCELEROMETER: - if ((PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id))) + if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id))) { PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; @@ -497,7 +497,7 @@ void pidRun(uint8_t ID) break; case PID_ID_COMPASS: - if (PidProfile[PID_ID_COMPASS].pidEnabled) + if (!PidProfile[PID_ID_COMPASS].pidEnabled) { PidProfile[PID_ID_COMPASS].PID_Out[0] = rc_input.Yaw; } diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 7d97c1b..81ec0f2 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -16,6 +16,23 @@ #define GPS_PACKET_SIZE 10 #define PING_PACKET_SIZE 4 #define ARDUINO_SENSOR_SIZE 6 +#define TIME_BEFORE_DEATH_MS 500 + +#define BYTE1_32BITS_VALUE(x) ((x & 0xFF000000) >> 24) +#define BYTE2_32BITS_VALUE(x) ((x & 0x00FF0000) >> 16) +#define BYTE3_32BITS_VALUE(x) ((x & 0x0000FF00) >> 8) +#define BYTE4_32BITS_VALUE(x) ((x & 0x000000FF) >> 0) + +#define SET_BYTE1_32BITS_VALUE(x) ((x & 0xFF) << 24) +#define SET_BYTE2_32BITS_VALUE(x) ((x & 0xFF) << 16) +#define SET_BYTE3_32BITS_VALUE(x) ((x & 0xFF) << 8) +#define SET_BYTE4_32BITS_VALUE(x) ((x & 0xFF) << 0) + +const uint32_t heartbeat_msg = 0xDEADBEEF; +const uint32_t heartbeat_rsp = 0xBA1DFACE; +uint32_t time_since_heartbeat = 0; + + typedef struct arduino_sensor_t { uint8_t ID __attribute__((packed)); @@ -30,20 +47,28 @@ enum smartport_packets_e { FSSP_DATA_FRAME = 0x10, // Sensor replies with this as start byte // ID of sensors. Must be something polled by FrSky RX - FSS_SENSOR_CURRENT = 0xA1, - FSS_SENSOR_VOLTAGE = 0x22, + FSS_SENSOR_3 = 0xA1, + FSS_SENSOR_2 = 0x22, FSS_SENSOR_BAROMETER = 0x1B, FSS_TUNE_PITCH = 0x0D, FSS_TUNE_ROLL = 0x34, FSS_SENSOR_6 = 0x67, + + //This is for handeling the LED strip and has nothing to do with the smartport + //It is only handled here since the information comes from the FC and is sent + //over the same bus as the rest of the messages + LED_STRIP_DATA = 0xED }; enum smartportID { - CURRENT_SENSOR_ID = 0, - VOLTAGE_SENSOR_ID, - BAROMETER_SENSOR_ID, + BAROMETER_SENSOR_ID = 0, TUNE_PITCH_ID, TUNE_ROLL_ID, + + //LED_STRIP_ID should only be on the flight controller side + LED_STRIP_ID, + + //the number of sensors to send data from . SENSOR_COUNT, }; @@ -102,12 +127,6 @@ void arduinoCom_init(USART_TypeDef* usart_inst) usart_init_dma(usart_inst, &usartdmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0); /*Initialize the sensors to be sent over smartport*/ - sensors[CURRENT_SENSOR_ID].ID = FSS_SENSOR_CURRENT; - sensors[CURRENT_SENSOR_ID].value = 0; - - sensors[VOLTAGE_SENSOR_ID].ID = FSS_SENSOR_VOLTAGE; - sensors[VOLTAGE_SENSOR_ID].value = 0; - sensors[BAROMETER_SENSOR_ID].ID = FSS_SENSOR_BAROMETER; sensors[BAROMETER_SENSOR_ID].value = 0; @@ -116,6 +135,9 @@ void arduinoCom_init(USART_TypeDef* usart_inst) sensors[TUNE_ROLL_ID].ID = FSS_TUNE_ROLL; sensors[TUNE_ROLL_ID].value = 0; + + sensors[LED_STRIP_ID].ID = LED_STRIP_DATA; + sensors[LED_STRIP_ID].value = 0; } @@ -161,14 +183,6 @@ arduino_data_t find_packet_from_header(uint8_t header) } -#define BYTE1_32BITS_VALUE(x) ((x & 0xFF000000) >> 24) -#define BYTE2_32BITS_VALUE(x) ((x & 0x00FF0000) >> 16) -#define BYTE3_32BITS_VALUE(x) ((x & 0x0000FF00) >> 8) -#define BYTE4_32BITS_VALUE(x) ((x & 0x000000FF) >> 0) - -const uint32_t heartbeat_msg = 0xDEADBEEF; -const uint32_t heartbeat_rsp = 0xBA1DFACE; - /*********************************************************************** * 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 @@ -206,6 +220,7 @@ void arduino_parse_message(uint8_t data) if(data == BYTE4_32BITS_VALUE(heartbeat_msg)) { usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000); + time_since_heartbeat = HAL_GetTick(); } break; default: @@ -314,12 +329,6 @@ void update_sensor_values() { /* TODO: Add the correct data to the value parameters here*/ - sensors[CURRENT_SENSOR_ID].value = Yaw * (180 / 3.1415) + 180; - sensors[CURRENT_SENSOR_ID].crc = calculate_crc(&sensors[CURRENT_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); - - sensors[VOLTAGE_SENSOR_ID].value = YawU * (180 / 3.1415) + 180; - sensors[VOLTAGE_SENSOR_ID].crc = calculate_crc(&sensors[VOLTAGE_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); - sensors[BAROMETER_SENSOR_ID].value = 0; sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); @@ -344,21 +353,44 @@ void arduino_send_sensor_values() } } - - - - - - - - - - - - - - - +/*********************************************************************** +* BRIEF: Check so that the heartbeat messages are comming with a * +* steady stream * +* INFORMATION: Check the last time a heart beat message was received * +* and checks against a pre defined time before declaring * +* the communication as dead * +***********************************************************************/ +bool arduino_com_alive() +{ + return (HAL_GetTick() < (time_since_heartbeat + TIME_BEFORE_DEATH_MS)); +} + +/*********************************************************************** +* BRIEF: Set a color on a specific led on the neo ledstrip attached * +* to the arduino * +* INFORMATION: Send a command with the led index and RGB color to the * +* Arduino * +***********************************************************************/ +void arduino_set_led_color(uint8_t index, uint8_t r, uint8_t g, uint8_t b) +{ + sensors[LED_STRIP_ID].value = SET_BYTE1_32BITS_VALUE(index) | + SET_BYTE2_32BITS_VALUE(r) | + SET_BYTE3_32BITS_VALUE(g) | + SET_BYTE4_32BITS_VALUE(b); + + sensors[LED_STRIP_ID].crc = calculate_crc(&sensors[LED_STRIP_ID], ARDUINO_SENSOR_SIZE - 1); +} + +/*********************************************************************** +* BRIEF: Tell the arduino that the FC system is OK and everything is * +* working * +* INFORMATION: Set the datavalue to 0xBA1DFACE for the led data * +***********************************************************************/ +void arduino_im_ok() +{ + sensors[LED_STRIP_ID].value = heartbeat_rsp; + sensors[LED_STRIP_ID].crc = calculate_crc(&sensors[LED_STRIP_ID], ARDUINO_SENSOR_SIZE - 1); +} From c9b23acaa2317e48f8d4c5649c33431ad30f4125 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Fri, 2 Dec 2016 17:23:32 +0100 Subject: [PATCH 6/7] Ping sensor implementation and moved compass code --- UAV-ControlSystem/inc/drivers/arduino_com.h | 9 +- UAV-ControlSystem/inc/drivers/compass.h | 3 + UAV-ControlSystem/inc/drivers/usart.h | 9 + UAV-ControlSystem/src/Flight/pid.c | 67 +------ UAV-ControlSystem/src/drivers/arduino_com.c | 201 +++++++++++++++----- UAV-ControlSystem/src/drivers/compass.c | 72 +++++++ UAV-ControlSystem/src/drivers/usart.c | 7 +- 7 files changed, 245 insertions(+), 123 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index cbeabd1..18b6b82 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -31,10 +31,11 @@ typedef struct compass_data_t { * INFORMATION: Contains the whole gps data message * ***********************************************************************/ typedef struct gps_data_t { - uint8_t header __attribute__((packed)); - float latitude; __attribute__((packed)) - float longitude __attribute__((packed)); - uint8_t crc __attribute__((packed)); + uint8_t header __attribute__((packed)); + float latitude __attribute__((packed)); + float longitude __attribute__((packed)); + uint8_t num_of_sats __attribute__((packed)); + uint8_t crc __attribute__((packed)); } gps_data_t; /*********************************************************************** diff --git a/UAV-ControlSystem/inc/drivers/compass.h b/UAV-ControlSystem/inc/drivers/compass.h index 69b1141..4201866 100644 --- a/UAV-ControlSystem/inc/drivers/compass.h +++ b/UAV-ControlSystem/inc/drivers/compass.h @@ -7,5 +7,8 @@ bool initialize_compass(); void calibrate_compass(); +void calculate_heading(); + + #endif //DRIVERS_COMPASS_H diff --git a/UAV-ControlSystem/inc/drivers/usart.h b/UAV-ControlSystem/inc/drivers/usart.h index 4417cdf..42bf1a1 100644 --- a/UAV-ControlSystem/inc/drivers/usart.h +++ b/UAV-ControlSystem/inc/drivers/usart.h @@ -46,6 +46,14 @@ typedef enum parity PARITY_ODD = 0x3 } parity; +typedef enum usart_index +{ + USART1_IDX = 0, + USART3_IDX, + USART6_IDX, + USART_INDEX_COUNT, +}usart_index; + // Struct to be used for regular USART with polling typedef struct usart_profile { @@ -61,6 +69,7 @@ typedef struct usart_dma_profile uint8_t* dma_rx_buffer1; // The first rx buffer used in double buffering uint8_t* dma_rx_buffer2; // The second rx buffer used in double buffering uint8_t* dma_tx_buffer; // The tx buffer used for sending messages + void* dma_rx_prev_buffer; // Keep track of the previous read buffer } usart_dma_profile; diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 5da5b03..e15b1cd 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -24,8 +24,6 @@ #include "drivers/barometer.h" #include "drivers/system_clock.h" -#define sq(x) ((x)*(x)) -#define map(x, in_min, in_max, out_min, out_max) (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ @@ -75,13 +73,6 @@ float accPitchFineTune = 0; float oldSensorValue[2] = {0}; float oldSensorValueRoll[12] = {0}; float oldSensorValuePitch[12] = {0}; -float MagnetFilteredOld[3]; -float alphaMagnet = 0.4; -int MagnetMax[3]; -int MagnetMin[3]; -float MagnetMap[3]; -float Yaw; -float YawU; /************************************************************************** * BRIEF: Calculates angle from accelerometer * @@ -199,63 +190,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_COMPASS: - { - readAcc(); -// float xMagnetFiltered = 0; -// float yMagnetFiltered = 0; -// float zMagnetFiltered = 0; -// xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]); -// yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]); -// zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]); -// -// MagnetFilteredOld[0] = xMagnetFiltered; -// MagnetFilteredOld[1] = yMagnetFiltered; -// MagnetFilteredOld[2] = zMagnetFiltered; -// -// -// //this part is required to normalize the magnetic vector -// if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; } -// if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; } -// if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; } -// -// if (xMagnetFiltered> 24) -#define BYTE2_32BITS_VALUE(x) ((x & 0x00FF0000) >> 16) -#define BYTE3_32BITS_VALUE(x) ((x & 0x0000FF00) >> 8) -#define BYTE4_32BITS_VALUE(x) ((x & 0x000000FF) >> 0) #define SET_BYTE1_32BITS_VALUE(x) ((x & 0xFF) << 24) #define SET_BYTE2_32BITS_VALUE(x) ((x & 0xFF) << 16) #define SET_BYTE3_32BITS_VALUE(x) ((x & 0xFF) << 8) #define SET_BYTE4_32BITS_VALUE(x) ((x & 0xFF) << 0) -const uint32_t heartbeat_msg = 0xDEADBEEF; -const uint32_t heartbeat_rsp = 0xBA1DFACE; + +#define USE_STORED_WP +#define USE_CURR_POS +#define USE_CURR_HEADING +#define USE_DISTANCE_TO_HOME +#define USE_CURRENT_SPEED +#define USE_CURRENT_ALTITUDE + +const uint8_t heartbeat_msg[4] = { 0xDE, 0xAD, 0xBE, 0xEF }; +const uint8_t heartbeat_rsp[4] = { 0xBA, 0x1D, 0xFA, 0xCE }; uint32_t time_since_heartbeat = 0; + typedef struct arduino_sensor_t { uint8_t ID __attribute__((packed)); uint32_t value __attribute__((packed)); @@ -47,12 +52,51 @@ enum smartport_packets_e { FSSP_DATA_FRAME = 0x10, // Sensor replies with this as start byte // ID of sensors. Must be something polled by FrSky RX - FSS_SENSOR_3 = 0xA1, - FSS_SENSOR_2 = 0x22, - FSS_SENSOR_BAROMETER = 0x1B, - FSS_TUNE_PITCH = 0x0D, - FSS_TUNE_ROLL = 0x34, - FSS_SENSOR_6 = 0x67, +#ifdef USE_STORED_WP + FSS_WP_LON = 0xA1, //Physical 2 + FSS_WP_LAT = 0x22, //Physical 3 +#endif + +#ifdef USE_CURR_POS + FSS_CURR_POS_LON = 0x83, //Physical 4 + FSS_CURR_POS_LAT = 0xE4, //Physical 5 +#endif + +#ifdef USE_CURR_HEADING + FSS_CURR_HEADING = 0x45, //Physical 6 +#endif + +#ifdef USE_DISTANCE_TO_HOME + FSS_DIST_HOME = 0xC6, //Physical 7 +#endif + +#ifdef USE_CURRENT_SPEED + FSS_SPEED = 0x67, //Physical 8 +#endif + +#ifdef USE_CURRENT_ALTITUDE + FSS_ALTITUDE = 0x48, //Physical 9 +#endif + + FSS_SENSOR_10 = 0xE9, //Physical 10 + FSS_SENSOR_11 = 0x6A, //Physical 11 + FSS_SENSOR_12 = 0xCB, //Physical 12 + FSS_SENSOR_13 = 0xAC, //Physical 13 + FSS_SENSOR_14 = 0x0D, //Physical 14 + FSS_SENSOR_15 = 0x8E, //Physical 15 + FSS_SENSOR_16 = 0x2F, //Physical 16 + FSS_SENSOR_17 = 0xD0, //Physical 17 + FSS_SENSOR_18 = 0x71, //Physical 18 + FSS_SENSOR_19 = 0xF2, //Physical 19 + FSS_SENSOR_20 = 0x53, //Physical 20 + FSS_SENSOR_21 = 0x34, //Physical 21 + FSS_SENSOR_22 = 0x95, //Physical 22 + FSS_SENSOR_23 = 0x16, //Physical 23 + FSS_SENSOR_24 = 0xB7, //Physical 24 + FSS_SENSOR_25 = 0x98, //Physical 25 + FSS_SENSOR_26 = 0x39, //Physical 26 + FSS_SENSOR_27 = 0xBA, //Physical 27 + FSS_SENSOR_28 = 0x1B, //Physical 28 //This is for handeling the LED strip and has nothing to do with the smartport //It is only handled here since the information comes from the FC and is sent @@ -61,9 +105,26 @@ enum smartport_packets_e { }; enum smartportID { - BAROMETER_SENSOR_ID = 0, - TUNE_PITCH_ID, - TUNE_ROLL_ID, +#ifdef USE_STORED_WP + WP_LON_ID, + WP_LAT_ID, +#endif +#ifdef USE_CURR_POS + CURR_LON_ID, + CURR_LAT_ID, +#endif +#ifdef USE_CURR_HEADING + CURR_HEADING_ID, +#endif +#ifdef USE_DISTANCE_TO_HOME + DIST_HOME_ID, +#endif +#ifdef USE_CURRENT_SPEED + SPEED_ID, +#endif +#ifdef USE_CURRENT_ALTITUDE + ALTITUDE_ID, +#endif //LED_STRIP_ID should only be on the flight controller side LED_STRIP_ID, @@ -77,7 +138,7 @@ arduino_sensor_t sensors[SENSOR_COUNT]; usart_dma_profile usartdmaHandler; -dma_usart_return raw_dma_data_t; +dma_usart_return raw_arduino_dma_data_t; // enumeration to hold the id:s of the different packages enum packet_ids { @@ -127,14 +188,34 @@ void arduinoCom_init(USART_TypeDef* usart_inst) usart_init_dma(usart_inst, &usartdmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0); /*Initialize the sensors to be sent over smartport*/ - sensors[BAROMETER_SENSOR_ID].ID = FSS_SENSOR_BAROMETER; - sensors[BAROMETER_SENSOR_ID].value = 0; - - sensors[TUNE_PITCH_ID].ID = FSS_TUNE_PITCH; - sensors[TUNE_PITCH_ID].value = 0; - - sensors[TUNE_ROLL_ID].ID = FSS_TUNE_ROLL; - sensors[TUNE_ROLL_ID].value = 0; +#ifdef USE_STORED_WP + sensors[WP_LON_ID].ID = FSS_WP_LON; + sensors[WP_LON_ID].value = 0; + sensors[WP_LAT_ID].ID = FSS_WP_LAT; + sensors[WP_LAT_ID].value = 0; +#endif +#ifdef USE_CURR_POS + sensors[CURR_LON_ID].ID = FSS_CURR_POS_LON; + sensors[CURR_LON_ID].value = 0; + sensors[CURR_LAT_ID].ID = FSS_CURR_POS_LAT; + sensors[CURR_LAT_ID].value = 0; +#endif +#ifdef USE_CURR_HEADING + sensors[CURR_HEADING_ID].ID = FSS_CURR_HEADING; + sensors[CURR_HEADING_ID].value = 0; +#endif +#ifdef USE_DISTANCE_TO_HOME + sensors[DIST_HOME_ID].ID = FSS_DIST_HOME; + sensors[DIST_HOME_ID].value = 0; +#endif +#ifdef USE_CURRENT_SPEED + sensors[SPEED_ID].ID = FSS_SPEED; + sensors[SPEED_ID].value = 0; +#endif +#ifdef USE_CURRENT_ALTITUDE + sensors[ALTITUDE_ID].ID = FSS_ALTITUDE; + sensors[ALTITUDE_ID].value = 0; +#endif sensors[LED_STRIP_ID].ID = LED_STRIP_DATA; sensors[LED_STRIP_ID].value = 0; @@ -148,8 +229,8 @@ void arduinoCom_init(USART_TypeDef* usart_inst) bool arduino_frame_available() { /* We read data from DMA */ - raw_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); - return raw_dma_data_t.new_data; + raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); + return raw_arduino_dma_data_t.new_data; } /*********************************************************************** @@ -199,7 +280,7 @@ void arduino_parse_message(uint8_t data) static uint8_t crc = 0; static uint8_t heartbeatiterator = 0; - if (heartbeatiterator == 0 && data == BYTE1_32BITS_VALUE(heartbeat_msg)) + if (heartbeatiterator == 0 && data == heartbeat_msg[0]) heartbeatiterator = 1; if (heartbeatiterator > 0) @@ -210,14 +291,14 @@ void arduino_parse_message(uint8_t data) heartbeatiterator = 2; break; case 2: - heartbeatiterator = (data == BYTE2_32BITS_VALUE(heartbeat_msg)) ? 3 : 0; + heartbeatiterator = (data == heartbeat_msg[1]) ? 3 : 0; break; case 3: - heartbeatiterator = (data == BYTE3_32BITS_VALUE(heartbeat_msg)) ? 4 : 0; + heartbeatiterator = (data == heartbeat_msg[2]) ? 4 : 0; break; case 4: heartbeatiterator = 0; - if(data == BYTE4_32BITS_VALUE(heartbeat_msg)) + if(data == heartbeat_msg[3]) { usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000); time_since_heartbeat = HAL_GetTick(); @@ -299,15 +380,15 @@ void arduino_parse_message(uint8_t data) ***********************************************************************/ void arduino_read() { - raw_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); + raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); //If the DMA has come to a new buffer - if (raw_dma_data_t.new_data) + if (raw_arduino_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]); + arduino_parse_message(raw_arduino_dma_data_t.buff[i]); } } } @@ -327,16 +408,38 @@ uint8_t calculate_crc(uint8_t *data, uint8_t length) ***********************************************************************/ void update_sensor_values() { - /* TODO: Add the correct data to the value parameters here*/ - sensors[BAROMETER_SENSOR_ID].value = 0; - sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); - sensors[TUNE_PITCH_ID].value = 0; - sensors[TUNE_PITCH_ID].crc = calculate_crc(&sensors[TUNE_PITCH_ID], ARDUINO_SENSOR_SIZE - 1); +#ifdef USE_STORED_WP + sensors[WP_LON_ID].value = 0; + sensors[WP_LON_ID].crc = calculate_crc(&sensors[WP_LON_ID], ARDUINO_SENSOR_SIZE - 1); - sensors[TUNE_ROLL_ID].value = 0; - sensors[TUNE_ROLL_ID].crc = calculate_crc(&sensors[TUNE_ROLL_ID], ARDUINO_SENSOR_SIZE - 1); + sensors[WP_LAT_ID].value = 0; + sensors[WP_LAT_ID].crc = calculate_crc(&sensors[WP_LAT_ID], ARDUINO_SENSOR_SIZE - 1); +#endif +#ifdef USE_CURR_POS + sensors[CURR_LON_ID].value = 0; + sensors[CURR_LON_ID].crc = calculate_crc(&sensors[CURR_LON_ID], ARDUINO_SENSOR_SIZE - 1); + + sensors[CURR_LAT_ID].value = 0; + sensors[CURR_LAT_ID].crc = calculate_crc(&sensors[CURR_LAT_ID], ARDUINO_SENSOR_SIZE - 1); +#endif +#ifdef USE_CURR_HEADING + sensors[CURR_HEADING_ID].value = 0; + sensors[CURR_HEADING_ID].crc = calculate_crc(&sensors[CURR_HEADING_ID], ARDUINO_SENSOR_SIZE - 1); +#endif +#ifdef USE_DISTANCE_TO_HOME + sensors[DIST_HOME_ID].value = 0; + sensors[DIST_HOME_ID].crc = calculate_crc(&sensors[DIST_HOME_ID], ARDUINO_SENSOR_SIZE - 1); +#endif +#ifdef USE_CURRENT_SPEED + sensors[SPEED_ID].value = 0; + sensors[SPEED_ID].crc = calculate_crc(&sensors[SPEED_ID], ARDUINO_SENSOR_SIZE - 1); +#endif +#ifdef USE_CURRENT_ALTITUDE + sensors[ALTITUDE_ID].value = 0; + sensors[ALTITUDE_ID].crc = calculate_crc(&sensors[ALTITUDE_ID], ARDUINO_SENSOR_SIZE - 1); +#endif } @@ -346,11 +449,11 @@ void update_sensor_values() ***********************************************************************/ void arduino_send_sensor_values() { + static int sensor_send_index = 0; + update_sensor_values(); - for (int i = 0; i < SENSOR_COUNT; i++) - { - usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &sensors[i], 6, 10000); - } + usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &sensors[sensor_send_index], 6, 10000); + sensor_send_index = (sensor_send_index + 1) % SENSOR_COUNT; } /*********************************************************************** diff --git a/UAV-ControlSystem/src/drivers/compass.c b/UAV-ControlSystem/src/drivers/compass.c index 79f196c..c8910c3 100644 --- a/UAV-ControlSystem/src/drivers/compass.c +++ b/UAV-ControlSystem/src/drivers/compass.c @@ -3,6 +3,19 @@ #include "drivers/arduino_com.h" +#define sq(x) ((x)*(x)) +#define map(x, in_min, in_max, out_min, out_max) (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + + +float MagnetFilteredOld[3]; +float alphaMagnet = 0.4; +int MagnetMax[3]; +int MagnetMin[3]; +float MagnetMap[3]; +float Yaw; +float YawU; + + bool initialize_compass() { @@ -12,3 +25,62 @@ void calibrate_compass() { } + +void calculate_heading() +{ + +// readAcc(); +// float xMagnetFiltered = 0; +// float yMagnetFiltered = 0; +// float zMagnetFiltered = 0; +// xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]); +// yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]); +// zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]); +// +// MagnetFilteredOld[0] = xMagnetFiltered; +// MagnetFilteredOld[1] = yMagnetFiltered; +// MagnetFilteredOld[2] = zMagnetFiltered; +// +// +// //this part is required to normalize the magnetic vector +// if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; } +// if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; } +// if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; } +// +// if (xMagnetFiltereddma_usart_rx_instance = dma_rx_instance; profile_out->dma_usart_tx_instance = dma_tx_instance; + profile_out->dma_rx_prev_buffer = NULL; // Enable the DMA on the USARTon register level profile_out->usart_pro.usart_instance->CR3 |= DMAR | DMAT; @@ -404,8 +403,8 @@ dma_usart_return usart_get_dma_buffer(usart_dma_profile *profile) { data.buff = profile->dma_rx_buffer2; } - data.new_data = (data.buff != prevBuf); - prevBuf = data.buff; + data.new_data = (data.buff != profile->dma_rx_prev_buffer); + profile->dma_rx_prev_buffer = data.buff; return data; } From 4d8e6db7edc2754d0f74174b13ee2ebce3111521 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Mon, 5 Dec 2016 08:46:32 +0100 Subject: [PATCH 7/7] some small things for the radio controller --- UAV-ControlSystem/src/drivers/arduino_com.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 3397236..c0f1c42 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -418,10 +418,10 @@ void update_sensor_values() sensors[WP_LAT_ID].crc = calculate_crc(&sensors[WP_LAT_ID], ARDUINO_SENSOR_SIZE - 1); #endif #ifdef USE_CURR_POS - sensors[CURR_LON_ID].value = 0; + sensors[CURR_LON_ID].value = gps_data.longitude; sensors[CURR_LON_ID].crc = calculate_crc(&sensors[CURR_LON_ID], ARDUINO_SENSOR_SIZE - 1); - sensors[CURR_LAT_ID].value = 0; + sensors[CURR_LAT_ID].value = gps_data.latitude; sensors[CURR_LAT_ID].crc = calculate_crc(&sensors[CURR_LAT_ID], ARDUINO_SENSOR_SIZE - 1); #endif #ifdef USE_CURR_HEADING @@ -437,7 +437,7 @@ void update_sensor_values() sensors[SPEED_ID].crc = calculate_crc(&sensors[SPEED_ID], ARDUINO_SENSOR_SIZE - 1); #endif #ifdef USE_CURRENT_ALTITUDE - sensors[ALTITUDE_ID].value = 0; + sensors[ALTITUDE_ID].value = ping_data.distance_mm / 100; sensors[ALTITUDE_ID].crc = calculate_crc(&sensors[ALTITUDE_ID], ARDUINO_SENSOR_SIZE - 1); #endif }