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*/
/*Is set in motor mix and used in pidUAVcore and mix */
bool motorLimitReached;
/**************************************************************************
* BRIEF: Initializes PID profiles *

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

@ -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

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

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

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 ABS_FLOAT(x) (((x) < 0)? -(x): (x))
typedef char typeString[maxStringSize_CLI];
typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr;
@ -81,6 +83,13 @@ 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);
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 *
* AUTHOR: Johan Gärtner *
* AUTHOR: Johan G<EFBFBD>rtner *
*
* PURPOSE: This file contains pid functions *
* INFORMATION: pidUAV is the final pid controller which only takes *
@ -19,6 +19,7 @@
#include <math.h>
#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*/
@ -26,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)*/
@ -59,43 +60,10 @@ 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;
/**************************************************************************
* 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 *
@ -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)
{
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 *
* INFORMATION: *
**************************************************************************/
void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{
switch (ID_profile)
{
case PID_ID_GYRO:
@ -149,57 +93,27 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break;
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;
/*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;
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);
/*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;
@ -257,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:
@ -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];
// 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)
@ -309,7 +224,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
}
else
{
pidProfileBuff->ITermLimit[axis] = abs(ITerm);
pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
}
pidProfileBuff->lastITerm[axis] = ITerm;
@ -350,8 +265,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);
}
/**************************************************************************
@ -424,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
{
@ -456,23 +372,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; //<2F>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:
@ -598,7 +514,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);
@ -614,8 +529,14 @@ void pidInit()
void pidEproom(void)
{
PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
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; //<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 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 */
@ -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}
},
};

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),
.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 */

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

@ -7,6 +7,9 @@
#include <drivers/accel_gyro.h>
#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;
@ -243,6 +246,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
HAL_Delay(60);
accel->pitchAngle = 0;
accel->rollAngle = 0;
return true;
}
@ -514,3 +519,79 @@ bool mpu6000_who_am_i()
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. */
flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
[FLAG_CONFIGURATION_ARM] = {
.minRange = 1500,
.minRange = 1800,
.maxRange = 2100,
.channelNumber = 8,
.flagId = systemFlags_armed_id,

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

@ -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))

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

@ -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);
}
}
/**************************************************************************

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

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

@ -28,8 +28,7 @@
#include "drivers/motormix.h"
#include "drivers/motors.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
@ -124,6 +123,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

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