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

Merge remote-tracking branch 'refs/remotes/origin/ACC-fix' into baro2

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-09 10:36:30 +01:00
commit 4fe18abcec
17 changed files with 513 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
UAV-ControlSystem/inc/Flight/pid.h
View File

@ -67,8 +67,7 @@ extern float accPitchFineTune;
extern accel_t accelProfile; /*Struct profile for input data from sensor*/ 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 * * BRIEF: Initializes PID profiles *

2
UAV-ControlSystem/inc/config/eeprom.h
View File

@ -208,6 +208,8 @@ typedef enum {
typedef enum { typedef enum {
EEPROM_PID_GYRO, EEPROM_PID_GYRO,
EEPROM_PID_ACCELEROMETER, EEPROM_PID_ACCELEROMETER,
EEPROM_PID_COMPASS,
EEPROM_PID_BAROMETER,
/* Counts the amount of settings in profile */ /* Counts the amount of settings in profile */
EEPROM_PROFILE_COUNT EEPROM_PROFILE_COUNT

3
UAV-ControlSystem/inc/drivers/accel_gyro.h
View File

@ -114,6 +114,7 @@ typedef struct accel_t {
int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */ int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */
int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */ int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */
uint16_t accel1G; /* Sensitivity factor */ uint16_t accel1G; /* Sensitivity factor */
float rollAngle, pitchAngle;
} accel_t; } accel_t;
/*********************************************************************** /***********************************************************************
@ -164,6 +165,8 @@ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out);
***********************************************************************/ ***********************************************************************/
bool mpu6000_who_am_i(); bool mpu6000_who_am_i();
void mpu6000_read_angle(accel_t* accel, gyro_t* gyro);
#endif /* DRIVERS_ACCEL_GYRO_H_ */ #endif /* DRIVERS_ACCEL_GYRO_H_ */

78
UAV-ControlSystem/inc/drivers/arduino_com.h Normal file
View File

@ -0,0 +1,78 @@
/*
* arduino_com.h
*
* Created on: 26 okt. 2016
* Author: Philip
*/
#ifndef DRIVERS_ARDUINO_COM_H_
#define DRIVERS_ARDUINO_COM_H_
#include "drivers/usart.h"
#define ARDUINO_BAUD 115200
#define ARDUINO_DMA_SIZE 20
/***********************************************************************
* BRIEF: RX packet structure from arduino com *
* INFORMATION: Contains the whole compass message *
***********************************************************************/
typedef struct compass_data_t {
uint8_t header;
int16_t x;
int16_t y;
int16_t z;
uint8_t crc;
} compass_data_t;
/***********************************************************************
* BRIEF: RX packet structure from arduino com *
* INFORMATION: Contains the whole gps data message *
***********************************************************************/
typedef struct gps_data_t {
uint8_t header;
float latitude;
float longitude;
uint8_t crc;
} gps_data_t;
/* An instance of the GPS data read from Arduino Com */
gps_data_t gps_data;
/* An instance of the compass data read from Arduino Com */
compass_data_t compass_data;
/***********************************************************************
* BRIEF: Initializes the UART for Arduino com *
* INFORMATION: A DMA Buffer starts storing the bytes received from RX *
***********************************************************************/
void arduinoCom_init(USART_TypeDef* usart_inst);
/***********************************************************************
* BRIEF: Checks if new RX packet is available *
* INFORMATION: Is called by the scheduler *
***********************************************************************/
bool arduino_frame_available();
/***********************************************************************
* BRIEF: Updates "gps_data" and "compass_data" *
* INFORMATION: Is called by the scheduler *
***********************************************************************/
void arduino_read();
#endif /* DRIVERS_ARDUINO_COM_H_ */
//----------------------- Example code for the parser --------------------------
//arduinoCom_init(USART1);
//while (1) {
// if(arduino_frame_available())
// arduino_read();
// HAL_Delay(15);
// float lng = gps_data.longitude;
// float lat = gps_data.latitude;
//}

3
UAV-ControlSystem/inc/drivers/motormix.h
View File

@ -46,6 +46,9 @@ typedef struct {
/* Global mixerConfig to bee available to EEPROM */ /* Global mixerConfig to bee available to EEPROM */
extern mixerConfig_s mixerConfig; extern mixerConfig_s mixerConfig;
/*Is set in motor mix and used in pidUAVcore and mix */
extern bool motorLimitReached;
/*********************************************************************** /***********************************************************************
* BRIEF: The motormixer * * BRIEF: The motormixer *
* INFORMATION: Sums the output from all control loops and adapts the * * INFORMATION: Sums the output from all control loops and adapts the *

9
UAV-ControlSystem/inc/utilities.h
View File

@ -25,6 +25,8 @@
#define maxStringSize_CLI 100 //Max sting size used for the messages in the CLI #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 char typeString[maxStringSize_CLI];
typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr; typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr;
@ -81,6 +83,13 @@ uint32_t accumulate(uint32_t list[], int length);
***********************************************************************/ ***********************************************************************/
void Error_Handler(void); 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); uint8_t reverse(uint8_t byte);
int16_t constrain(int16_t value, int16_t min, int16_t max); int16_t constrain(int16_t value, int16_t min, int16_t max);

159
UAV-ControlSystem/src/Flight/pid.c
View File

@ -1,6 +1,6 @@
/************************************************************************** /**************************************************************************
* NAME: pid.c * * NAME: pid.c *
* AUTHOR: Johan Gärtner * * AUTHOR: Johan G<EFBFBD>rtner *
* *
* PURPOSE: This file contains pid functions * * PURPOSE: This file contains pid functions *
* INFORMATION: pidUAV is the final pid controller which only takes * * INFORMATION: pidUAV is the final pid controller which only takes *
@ -19,6 +19,7 @@
#include <math.h> #include <math.h>
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "utilities.h"
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
@ -26,7 +27,7 @@
#define DTERM_SCALE 0.000529f /*D-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*/ #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 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 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 COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
@ -59,43 +60,10 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso
pt1Filter_t accelFilter[2] = {0}; pt1Filter_t accelFilter[2] = {0};
float throttleRate = 0;
float accRollFineTune = 0; float accRollFineTune = 0;
float accPitchFineTune = 0; float accPitchFineTune = 0;
/**************************************************************************
* 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 * * BRIEF: Scales data from input range to output range *
@ -104,38 +72,14 @@ float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, co
float convertData(int inputRange, int outputRange, int offset, float value) float convertData(int inputRange, int outputRange, int offset, float value)
{ {
return ((float)outputRange/(float)inputRange)*(value-(float)offset); return ((float)outputRange/(float)inputRange)*(value-(float)offset);
//return 1.0;
} }
/**************************************************************************
* 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 < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
float oldSensorValue[2] = {0};
float oldSensorValueRoll[12] = {0};
float oldSensorValuePitch[12] = {0};
int i = 0;
/************************************************************************** /**************************************************************************
* BRIEF: Update current sensor values * * BRIEF: Update current sensor values *
* INFORMATION: * * INFORMATION: *
**************************************************************************/ **************************************************************************/
void getCurrentValues(float sensorValues[3], uint8_t ID_profile) void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{ {
switch (ID_profile) switch (ID_profile)
{ {
case PID_ID_GYRO: case PID_ID_GYRO:
@ -149,57 +93,27 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break; break;
case PID_ID_ACCELEROMETER: case PID_ID_ACCELEROMETER:
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/
sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
float alpha = 0.5; /*Checks the biggest angle */
/*May need Low pass filter since the accelerometer may drift*/ throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180);
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;
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; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:
sensorValues[ROLL] = 0;
sensorValues[PITCH] = 0;
sensorValues[YAW] = 0;
break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
break;
default: default:
sensorValues[ROLL] = 0; sensorValues[ROLL] = 0;
@ -257,7 +171,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
break; break;
case PID_ID_BAROMETER: 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; break;
default: default:
@ -299,8 +215,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; 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. // 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);
ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
// Anti windup protection // Anti windup protection
if (motorLimitReached) if (motorLimitReached)
@ -309,7 +224,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
} }
else else
{ {
pidProfileBuff->ITermLimit[axis] = abs(ITerm); pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
} }
pidProfileBuff->lastITerm[axis] = ITerm; pidProfileBuff->lastITerm[axis] = ITerm;
@ -350,8 +265,9 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
{ {
ITerm = 0; ITerm = 0;
pidProfileBuff->lastITerm[axis] = 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);
} }
/************************************************************************** /**************************************************************************
@ -424,9 +340,9 @@ void pidRun(uint8_t ID)
break; break;
case PID_ID_BAROMETER: 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 else
{ {
@ -456,23 +372,23 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
case PID_ID_GYRO: case PID_ID_GYRO:
PidProfileBuff[ID].DOF = 3; 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; //<2F>NDRA TILL SEKUNDER inte ms
break; break;
case PID_ID_ACCELEROMETER: case PID_ID_ACCELEROMETER:
PidProfileBuff[ID].DOF = 2; PidProfileBuff[ID].DOF = 2;
PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000; PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
break; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:
PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
break; break;
default: default:
@ -598,7 +514,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
void pidInit() void pidInit()
{ {
mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/ mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
motorLimitReached = false;
pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO); pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER); pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
@ -614,8 +529,14 @@ void pidInit()
void pidEproom(void) void pidEproom(void)
{ {
PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 4;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 4;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //<2F>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;
} }

64
UAV-ControlSystem/src/config/cli.c
View File

@ -31,7 +31,7 @@
#define commandValueError 0xFFFFFFFFFFFFFFFF #define commandValueError 0xFFFFFFFFFFFFFFFF
#define minSimilarCharacters 2 //the minimum amount of characters needed to to a search on a value #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 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) #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", "| reboot - Exit CLI and reboots the system.\n\r",
"| reset - Restore all the values to its default values.\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", "| stats - Gives some current stats of the system and tasks.\n\r",
"| calibrate_motors - Calibrates all motors.", "| calibrate_motors - Calibrates all motors.\n\r",
"| calibrate_gyro - Calibrates the gyro.", "| calibrate_gyro - Calibrates the gyro.\n\r",
"| calibrate_accelerometer - Calibrates the accelerometer.", "| calibrate_accelerometer - Calibrates the accelerometer.\n\r",
"| calibrate_compass - Calibrates the compass.", "| calibrate_compass - Calibrates the compass.\n\r",
"| calibration_info_accelerometer - Provides info on the accelerometer calibration." "| calibration_info_accelerometer - Provides info on the accelerometer calibration.\n\r"
}; };
/* String array containing all the signature examples for each action */ /* String array containing all the signature examples for each action */
@ -284,9 +284,22 @@ typedef enum
COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, COMMAND_ID_PID_ACCEL_YAW_P_LIMIT,
COMMAND_ID_PID_ACCEL_OUT_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 */ /* Enable the different pid loops */
COMMAND_ID_PID_GYRO_ISENABLED, COMMAND_ID_PID_GYRO_ISENABLED,
COMMAND_ID_PID_ACCEL_ISENABLED, COMMAND_ID_PID_ACCEL_ISENABLED,
COMMAND_ID_PID_BARO_ISENABLED,
/* Counter for the amount of commands */ /* Counter for the amount of commands */
COMMAND_ID_COUNT, 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} "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 */ /* Enable pid loops */
[COMMAND_ID_PID_GYRO_ISENABLED] = [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} "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}
},
}; };

10
UAV-ControlSystem/src/config/eeprom.c
View File

@ -291,6 +291,16 @@ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = {
.size = sizeof(pidProfile_t), .size = sizeof(pidProfile_t),
.dataPtr = &(PidProfile[PID_ID_ACCELEROMETER]), .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 */ /* Data pointers and sizes for footer content */

81
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -7,6 +7,9 @@
#include <drivers/accel_gyro.h> #include <drivers/accel_gyro.h>
#include "drivers/spi.h" #include "drivers/spi.h"
#include "utilities.h"
#include "math.h"
#include "drivers/system_clock.h"
spi_profile mpu6000_spi_profile; spi_profile mpu6000_spi_profile;
uint8_t num_failed_receive = 0; uint8_t num_failed_receive = 0;
@ -243,6 +246,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
HAL_Delay(60); HAL_Delay(60);
accel->pitchAngle = 0;
accel->rollAngle = 0;
return true; return true;
} }
@ -514,3 +519,79 @@ bool mpu6000_who_am_i()
return false; return false;
} }
/* 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^8) // 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)
{
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;
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, 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]);
}
//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];
//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)
{
//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;
//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);
}

200
UAV-ControlSystem/src/drivers/arduino_com.c Normal file
View File

@ -0,0 +1,200 @@
/*
* arduino_com.c
*
* Created on: 26 okt. 2016
* Author: Philip
*/
#include "drivers/arduino_com.h"
#include "utilities.h"
#include "string.h"
#include "stm32f4xx_revo.h"
#define COMPASS_PACKET_SIZE 8
#define GPS_PACKET_SIZE 10
usart_dma_profile dmaHandler;
dma_usart_return raw_dma_data_t;
// enumeration to hold the id:s of the different packages
enum packet_ids {
COMPASS_PACKET_ID = 0xA1,
GPS_PACKET_ID = 0xB1,
};
// Structure used to hold the data for "data_arr"
typedef struct arduino_data_t {
uint8_t size; //Size of the data
void * dataPtr; //pointer to the data
} arduino_data_t ;
// An enumeration of the array positions of the data in the "data_arr"
enum arduino_data_e {
COMPASS_DATA_ID,
GPS_DATA_ID,
ARDUINO_DATA_COUNT,
};
// An array to hold the pointers to the different data structures that are used in the rest of the system;
arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
[COMPASS_DATA_ID] = {
.size = COMPASS_PACKET_SIZE,
.dataPtr = &compass_data,
},
[GPS_DATA_ID] = {
.size = GPS_PACKET_SIZE,
.dataPtr = &gps_data,
},
};
/***********************************************************************
* BRIEF: Initializes the UART for Arduino com *
* INFORMATION: A DMA Buffer starts storing the bytes received from RX *
***********************************************************************/
void arduinoCom_init(USART_TypeDef* usart_inst)
{
/* initialize the USART with a dma buffer */
usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0);
// usart_transmit(&dmaHandler.usart_pro, "data", 4, 100000);
}
/***********************************************************************
* BRIEF: Checks if new RX packet is available *
* INFORMATION: Is called by the scheduler *
***********************************************************************/
bool arduino_frame_available()
{
/* We read data from DMA */
raw_dma_data_t = usart_get_dma_buffer(&dmaHandler);
return raw_dma_data_t.new_data;
}
/***********************************************************************
* BRIEF: Private function that looks for the correct data struct from *
* the header data *
* INFORMATION: *
***********************************************************************/
arduino_data_t find_packet_from_header(uint8_t header)
{
arduino_data_t arduino_data = {
.dataPtr = NULL,
.size = 0,
};
/* Check what header it is and return the correct datapointer if correct header*/
switch (header)
{
case COMPASS_PACKET_ID:
arduino_data = data_arr[COMPASS_DATA_ID];
break;
case GPS_PACKET_ID:
arduino_data = data_arr[GPS_DATA_ID];
break;
default:
break;
}
return arduino_data;
}
/***********************************************************************
* BRIEF: A function that parses the message byte per byte and then
* if it was wrong it will do recursive call and parse the rest
* of the message
* INFORMATION: *
***********************************************************************/
void arduino_parse_message(uint8_t data)
{
static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
static bool find_header = true;
static uint8_t message_it = 0;
static uint8_t secondary_message_it = 0;
static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL };
static uint8_t crc = 0;
if(find_header)
{
msg_header_and_size = find_packet_from_header(data);
if(msg_header_and_size.size != 0)
{
find_header = false;
arduino_arr[(message_it)] = data;
message_it++;
crc ^= data;
}
}
else
{
/* If we find any new possible header then we should be able to return to that point in time */
if (((arduino_data_t)find_packet_from_header(data)).size != 0 && secondary_message_it == 0)
{
secondary_message_it = message_it; //save the value of the position in The buffer array, not the dma array index
}
/* Reading the message except the crc byte */
if ((message_it) < (msg_header_and_size.size - 1))
{
arduino_arr[(message_it)] = data;
crc ^= data;
message_it++;
}
else if ((message_it) == (msg_header_and_size.size - 1))
{
/* put the crc code into the data buffer as well */
arduino_arr[(message_it)] = data;
/* TODO: Replace with check for CRC */
if (crc == data)
{
/* Clear necessary variables in order to fill the buffer with new ones */
message_it = 0;
find_header = true;
secondary_message_it = 0;
crc = 0;
memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size);
}
else
{
int size = msg_header_and_size.size;
int new_iter = secondary_message_it;
crc = 0;
find_header = true;
message_it = 0;
secondary_message_it = 0;
if(new_iter > 0)
{
for(int i = new_iter; i < size; i++)
{
arduino_parse_message(arduino_arr[i]);
}
}
}
}
}
}
/***********************************************************************
* BRIEF: Updates "gps_data" and "compass_data" *
* INFORMATION: Is called by the scheduler *
***********************************************************************/
void arduino_read()
{
//If the DMA has come to a new buffer
if (raw_dma_data_t.new_data)
{
// parse the entire message to the gps_data and compass_data
for (int i = 0; i < ARDUINO_DMA_SIZE; i++)
{
arduino_parse_message(raw_dma_data_t.buff[i]);
}
}
}

2
UAV-ControlSystem/src/drivers/failsafe_toggles.c
View File

@ -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. */ /* 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] = { flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
[FLAG_CONFIGURATION_ARM] = { [FLAG_CONFIGURATION_ARM] = {
.minRange = 1500, .minRange = 1800,
.maxRange = 2100, .maxRange = 2100,
.channelNumber = 8, .channelNumber = 8,
.flagId = systemFlags_armed_id, .flagId = systemFlags_armed_id,

5
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -42,6 +42,9 @@
/* An array containing the calculated motor outputs */ /* An array containing the calculated motor outputs */
uint16_t motor_output[MOTOR_COUNT]; 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 */ /* Default values for the mixerConfig */
// TODO: Implement in EEPROM // TODO: Implement in EEPROM
mixerConfig_s mixerConfig = { mixerConfig_s mixerConfig = {
@ -102,8 +105,6 @@ void mix()
int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor 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 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 */ /* Mixer Full Scale enabled */
if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) if (flags_IsSet_ID(systemFlags_mixerfullscale_id))

7
UAV-ControlSystem/src/drivers/motors.c
View File

@ -14,6 +14,7 @@
#include "drivers/motors.h" #include "drivers/motors.h"
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#include "config/eeprom.h" #include "config/eeprom.h"
#include "drivers/motormix.h"
const int MotorPWMPeriode = 2000; //Micro seconds const int MotorPWMPeriode = 2000; //Micro seconds
const int MotorPWMInitPulse = 1000; const int MotorPWMInitPulse = 1000;
@ -162,7 +163,11 @@ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput)
**************************************************************************/ **************************************************************************/
void pwmEnableAllMotors(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);
}
} }
/************************************************************************** /**************************************************************************

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

@ -28,7 +28,6 @@
/* This instance is read by the whole system and should contain actual RX data */ /* This instance is read by the whole system and should contain actual RX data */
sbusFrame_s sbusChannelData = {0}; sbusFrame_s sbusChannelData = {0};
dma_usart_return raw_dma_data_t;
dma_usart_return raw_dma_data_t = {0}; dma_usart_return raw_dma_data_t = {0};
rc_input_t rc_input = {0}; rc_input_t rc_input = {0};
float rc_rate = 1.0; float rc_rate = 1.0;
@ -122,16 +121,12 @@ void sbus_read()
// If continue only if we get new data from DMA // If continue only if we get new data from DMA
if (raw_dma_data_t.new_data) if (raw_dma_data_t.new_data)
{ {
for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++) for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++)
{ {
uint8_t msg = raw_dma_data_t.buff[i]; uint8_t msg = raw_dma_data_t.buff[i];
// Look for the beginning of a sbus frame // Look for the beginning of a sbus frame
if ( message_it == 0 ) //&& stop_bit_read) if ( message_it == 0 ) //&& stop_bit_read)
{ {
//message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
if (msg == ((uint8_t)SBUS_HEADER)) if (msg == ((uint8_t)SBUS_HEADER))
{ {
sbus_arr[(message_it)] = msg; sbus_arr[(message_it)] = msg;
@ -142,12 +137,7 @@ void sbus_read()
{ {
message_it = 0; message_it = 0;
} }
// sbus_arr_iterator = 0;
// stop_bit_read = false;
} }
// Look for the end of sbus frame
//else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER)
else else
{ {
if (msg == (uint8_t)SBUS_HEADER && new_header == false) if (msg == (uint8_t)SBUS_HEADER && new_header == false)
@ -169,10 +159,6 @@ void sbus_read()
{ {
message_it = 0; message_it = 0;
missedMsg--; missedMsg--;
//stop_bit_read = true;
// If the expected byte is stop byte, then we overwrite to the return value.
//if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
//{
sbusChannelData = *(sbusFrame_s*)sbus_arr; sbusChannelData = *(sbusFrame_s*)sbus_arr;
// Linear fitting // Linear fitting
@ -229,17 +215,10 @@ void sbus_read()
message_it_secondary_head = innerCount; message_it_secondary_head = innerCount;
} }
} }
} }
} }
} }
// // Copy next byte into the sbus_arr
// if (sbus_arr_iterator < SBUS_FRAME_SIZE)
// sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i];
// sbus_arr_iterator++;
} }
} }
} }

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

@ -28,8 +28,7 @@
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "drivers/motors.h" #include "drivers/motors.h"
#include "Flight/pid.h" #include "Flight/pid.h"
#include "drivers/barometer.h" #include "drivers/barometer.h"#include "drivers/arduino_com.h"
/************************************************************************** /**************************************************************************
* BRIEF: Should contain all the initializations of the system, needs to * BRIEF: Should contain all the initializations of the system, needs to
@ -124,6 +123,7 @@ int main(void)
//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler //Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
initScheduler(); initScheduler();
while (1) while (1)
{ {
//Run the scheduler, responsible for distributing all the work of the running system //Run the scheduler, responsible for distributing all the work of the running system

14
UAV-ControlSystem/src/utilities.c
View File

@ -228,3 +228,17 @@ int16_t constrain(int16_t value, int16_t min, int16_t max)
else else
return value; 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;
}