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

Merge remote-tracking branch 'refs/remotes/origin/baro2'

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-29 12:24:34 +01:00
commit 64b2208ad8
24 changed files with 832 additions and 256 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,7 +65,10 @@ extern pidProfile_t PidProfile[PID_COUNT];
extern float accRollFineTune; extern float accRollFineTune;
extern float accPitchFineTune; extern float accPitchFineTune;
extern accel_t accelProfile; /*Struct profile for input data from sensor*/ extern accel_t accelProfile;
extern float throttleRate;
extern int HoverForce;/*Struct profile for input data from sensor*/
@ -81,6 +84,8 @@ void pidInit();
**************************************************************************/ **************************************************************************/
void pidRun(uint8_t ID); void pidRun(uint8_t ID);
void readAcc(void);
void pidEproom(void); void pidEproom(void);
#endif /* FLIGHT_PID_H_ */ #endif /* FLIGHT_PID_H_ */

4
UAV-ControlSystem/inc/Scheduler/scheduler.h
View File

@ -122,8 +122,8 @@ typedef enum
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
TASK_ALTITUDE, TASK_ALTITUDE,
#endif #endif
#if BEEPER #ifdef BEEPER
TASK_BEEPER TASK_BEEPER,
#endif #endif
//Debug tasks, ONLY to be used when testing //Debug tasks, ONLY to be used when testing

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

@ -174,7 +174,7 @@ typedef enum {
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
EEPROM_PERIOD_ALTITUDE, EEPROM_PERIOD_ALTITUDE,
#endif #endif
#if BEEPER #ifdef BEEPER
EEPROM_PERIOD_BEEPER, EEPROM_PERIOD_BEEPER,
#endif #endif
@ -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_ */

105
UAV-ControlSystem/inc/drivers/barometer.h
View File

@ -1,26 +1,113 @@
/* /**************************************************************************
* barometer.h * NAME: barometer.h *
* * *
* Created on: 18 okt. 2016 * AUTHOR: Jonas Holmberg *
* Author: holmis * *
*/ * PURPOSE: Used to provide an estimated altitude, in regards to the *
* lift of height that would represent zero meters in height. *
* *
* INFORMATION: Using I2C to communicate with the barometer a pressure and *
* temperature value can be obtained. These values can then be*
* used to estimate an altitude. Note that this is not an *
* altitude value relative to the ground underneath it. It is *
* relative to the position where the system was started from.*
* The start position of the system will indicate the zero *
* height. It is that position and only that one which will *
* be the compared height. *
* *
* GLOBAL VARIABLES: *
* Variable Type Description *
* -------- ---- ----------- *
***************************************************************************/
#ifndef DRIVERS_BAROMETER_H_ #ifndef DRIVERS_BAROMETER_H_
#define DRIVERS_BAROMETER_H_ #define DRIVERS_BAROMETER_H_
typedef enum {
CALCSTATE_D2_CALCULATION = 0, //Tell the sensor that we want to read D2
CALCSTATE_D2_READ, //Read D2 from the sensor
CALCSTATE_D1_CALCULATION, //Tell the sensor that we want to read D1
CALCSTATE_D1_READ, //Read D1 from the sensor
CALCSTATE_CALCULATE_PTA //preassure, temp, altidute calc
}calculationState;
/***********************************************************************
* BRIEF: Initializes the barometer. *
* INFORMATION: Initializes the barometer and it needs to be called *
* before anything else when using the barometer. *
***********************************************************************/
bool barometer_init(); bool barometer_init();
/***********************************************************************
* BRIEF: Resets the barometer. *
* INFORMATION: Resets the barometer needs to be called after the init.*
* It will send a reset message over the I2C to the *
* barometer telling it that is should perform a reset. *
* This needs to be done or it wont be possible to read *
* data from the barometer. *
***********************************************************************/
bool barometer_reset(); bool barometer_reset();
/***********************************************************************
* BRIEF: Calculates the values of the preassure, temperature and*
* altitude. *
* INFORMATION: This function needs to be called five times for the *
* data to be updated. This is because of some limitations*
* and to ensure the schedulability of the system it needs*
* to be divided. Firstly there is an inherit delay inside*
* the barometer sensor. To get data from the barometer a *
* message needs to be sent that tells the barometer to *
* prepare the data. This takes, depending on the amount *
* of sampling that is done up to 10 ms for the highest *
* amount of sampling. This also needs to be done two *
* times before that data can be calculated. Also since *
* the implementation uses a software I2C at the moment *
* because of some problems with the DMA implementation *
* the speed is not very high. Therefore sending several *
* messages and reading at the same time may take to long *
* time and could cause the system to be unschedulable. *
* Because of this the function is divided into different *
* cases: *
* 1: Prepare data. *
* 2: Read data. *
* 3: Prepare data. *
* 4: Read data. *
* 5: Calculate temperature, pressure and altitude. *
***********************************************************************/
void barometer_CaclulateValues(); void barometer_CaclulateValues();
double barometer_GetCurrentPreassure(); /***********************************************************************
* BRIEF: Retrieves the previously calculated pressure. *
* INFORMATION: Returns the last calculated pressure value. No *
* calculation is performed here so calling this will give*
* the same value until a new calculation has been *
* performed. *
***********************************************************************/
double barometer_GetCurrentPressure();
/***********************************************************************
* BRIEF: Retrieves the previously calculated temperature. *
* INFORMATION: Returns the last calculated temperature value. No *
* calculation is performed here so calling this will give*
* the same value until a new calculation has been *
* performed. *
***********************************************************************/
double barometer_GetCurrentTemperature(); double barometer_GetCurrentTemperature();
float barometer_GetCurrentAltitudeBasedOnSeaLevel(); /***********************************************************************
* BRIEF: Retrieves the previously calculated altitude. *
* INFORMATION: Returns the last calculated altitude value. No *
* calculation is performed here so calling this will give*
* the same value until a new calculation has been *
* performed. *
***********************************************************************/
float barometer_GetCurrentAltitude();
/***********************************************************************
* BRIEF: Gets the altitude based on the last number of values. *
* INFORMATION: Averages the value on the last few reading to get a more*
* accurate reading. *
***********************************************************************/
float barometer_GetCurrentAveragedtAltitude();
#endif /* DRIVERS_BAROMETER_H_ */ #endif /* DRIVERS_BAROMETER_H_ */

19
UAV-ControlSystem/inc/drivers/beeper.h Normal file
View File

@ -0,0 +1,19 @@
/*
* beeper.h
*
* Created on: 14 nov. 2016
* Author: holmis
*/
#ifndef DRIVERS_BEEPER_H_
#define DRIVERS_BEEPER_H_
#include "stm32f4xx_revo.h"
void initBeeper(uint16_t led_pin, GPIO_TypeDef* led_port);
void busyWaitBeep(uint16_t beepTimeMs);
#endif /* DRIVERS_BEEPER_H_ */

41
UAV-ControlSystem/inc/drivers/i2c_soft.h
View File

@ -1,15 +1,24 @@
/* /**************************************************************************
* i2c_soft.h * NAME: i2c_soft.h *
* * *
* Created on: 27 okt. 2016 * AUTHOR: Jonas Holmberg *
* Author: holmis * *
*/ * PURPOSE: Used to communicate via I2C in a SW simulated manner. *
* *
* INFORMATION: A software implementation of the I2C. It toggles the pins *
* that are used on and of to generate I2C messages. *
* *
* GLOBAL VARIABLES: *
* Variable Type Description *
* -------- ---- ----------- *
***************************************************************************/
#ifndef DRIVERS_I2C_SOFT_H_ #ifndef DRIVERS_I2C_SOFT_H_
#define DRIVERS_I2C_SOFT_H_ #define DRIVERS_I2C_SOFT_H_
#include "stm32f4xx.h" #include "stm32f4xx.h"
/* Struct used to create a soft i2c handler */
typedef struct typedef struct
{ {
GPIO_TypeDef * i2c_Port; GPIO_TypeDef * i2c_Port;
@ -17,12 +26,28 @@ typedef struct
uint16_t i2c_sda_pin; uint16_t i2c_sda_pin;
}I2C_SOFT_handle_t; }I2C_SOFT_handle_t;
/***********************************************************************
* BRIEF: Initializes the SW I2C.
* INFORMATION: Initializes the SW I2C, needs to be done before any
* thing else.
***********************************************************************/
void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile); void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile);
/***********************************************************************
* BRIEF: Writes a message.
* INFORMATION: Tries to write to an address. reg is the message that is
* written to the addr. data is the size of the data that
* is written.
***********************************************************************/
bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data); bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data);
/***********************************************************************
* BRIEF: Reads a message.
* INFORMATION: Tries to read a message from addr. reg is the message
* that says a read is desired. len is the length of the
* message that should be read and buf is the buffer that
* will store the read data.
***********************************************************************/
bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf); bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf);
#endif /* DRIVERS_I2C_SOFT_H_ */ #endif /* DRIVERS_I2C_SOFT_H_ */

5
UAV-ControlSystem/inc/stm32f4xx_revo.h
View File

@ -156,8 +156,9 @@
/* Beeper */ /* Beeper */
//#define BEEPER #define BEEPER
#define BEEPER_PIN GPIO_PIN_12
#define BEEPER_PORT GPIOB
/* Define all the moter of the system, servos + extra */ /* Define all the moter of the system, servos + extra */

2
UAV-ControlSystem/inc/system_variables.h
View File

@ -14,7 +14,7 @@
#ifndef SYSTEM_VARIABLES_H_ #ifndef SYSTEM_VARIABLES_H_
#define SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_
#define EEPROM_SYS_VERSION 107 #define EEPROM_SYS_VERSION 114
#define ADC_STATE #define ADC_STATE
#include "stm32f4xx.h" #include "stm32f4xx.h"

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

@ -83,6 +83,11 @@ 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);

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

@ -20,21 +20,26 @@
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "utilities.h" #include "utilities.h"
#include "drivers/barometer.h"
#include "drivers/system_clock.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*/
#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/
#define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ #define DTERM_SCALE 0.0529f /*D-term used as a scale value to the PID controller*/
#define BAROMETER_SCALE 5
#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 4 /*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 30 /*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)*/
#define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_I 256 /*Constrains ITerm*/
#define PID_MAX_D 512 /*Constrains DTerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/
#define DESIRED_HEIGHT 5 /*Height value in meters*/
/*Struct that belongs to a certain PID controller*/ /*Struct that belongs to a certain PID controller*/
typedef struct pidProfileBuff_s { typedef struct pidProfileBuff_s {
@ -63,42 +68,8 @@ pt1Filter_t accelFilter[2] = {0};
float accRollFineTune = 0; float accRollFineTune = 0;
float accPitchFineTune = 0; float accPitchFineTune = 0;
float throttleRate = 1;
int HoverForce = 1475; /*Struct profile for input data from sensor*/
/**************************************************************************
* 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 *
@ -109,26 +80,8 @@ 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);
} }
/************************************************************************** uint8_t FlagVelocityLimit = 0;
* BRIEF: Constrain float values within a defined limit * float VelocityCompensation = 0;
* INFORMATION: Used in PID loop to limit values *
**************************************************************************/
float constrainf(float amt, int low, int high)
{
if (amt < (float)low)
return (float)low;
else if (amt > (float)high)
return (float)high;
else
return amt;
}
float oldSensorValue[2] = {0};
float oldSensorValueRoll[12] = {0};
float oldSensorValuePitch[12] = {0};
int i = 0;
/************************************************************************** /**************************************************************************
* BRIEF: Update current sensor values * * BRIEF: Update current sensor values *
@ -136,6 +89,11 @@ int i = 0;
**************************************************************************/ **************************************************************************/
void getCurrentValues(float sensorValues[3], uint8_t ID_profile) void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{ {
static float last_micros = 0;
static float oldHeightValue = 0;
float current_micros = 0;
float delta_t_baro = 0;
float current_height = 0;
switch (ID_profile) switch (ID_profile)
@ -151,71 +109,41 @@ 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*/ sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
// /*Checks the biggest angle */
// if (calcGravity(accelProfile) > 1.15) throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - cos(sensorValues[PITCH]*M_PI/180);
// {
//
// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT;
// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT;
//
// }
// else
// {
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
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:
current_micros = clock_get_us();
current_micros = current_micros/1000000;
delta_t_baro = (current_micros - last_micros);
current_height = barometer_GetCurrentAveragedtAltitude();
last_micros = current_micros;
sensorValues[0] = ((current_height - oldHeightValue)/delta_t_baro);
//sensorValues[0] = ((sensorValues[0] < - 2) || (sensorValues[0] > 2))? sensorValues[0]:0;
oldHeightValue = current_height;
sensorValues[0]*=BAROMETER_SCALE;
break;
default: default:
current_micros = clock_get_us();
current_micros = current_micros/1000000;
last_micros = current_micros;
sensorValues[ROLL] = 0; sensorValues[ROLL] = 0;
sensorValues[PITCH] = 0; sensorValues[PITCH] = 0;
@ -232,6 +160,10 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
**************************************************************************/ **************************************************************************/
void getPointRate(float *desiredCommand, uint8_t ID_profile) void getPointRate(float *desiredCommand, uint8_t ID_profile)
{ {
float currentThrottle = 0;
float velocity = 0;
//*Do something smart*// //*Do something smart*//
switch (ID_profile) switch (ID_profile)
{ {
@ -272,7 +204,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); currentThrottle = rc_input.Throttle - 1500;
velocity = (currentThrottle < - 20 || currentThrottle > 20 )? currentThrottle:0;
desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, velocity)*BAROMETER_SCALE;
break; break;
default: default:
@ -286,9 +220,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
* the controller * the controller
**************************************************************************/ **************************************************************************/
void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
float desiredValue, float errorAxis, uint8_t axis) float desiredValue, float sensorValue, uint8_t axis)
{ {
const float rateError = desiredValue - errorAxis; const float rateError = desiredValue - sensorValue;
/* -----calculate P component ---- */ /* -----calculate P component ---- */
@ -309,12 +243,10 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
} }
} }
/* -----calculate I component ---- */ /* -----calculate I component ---- */
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, -(int)PID_MAX_I, (int)PID_MAX_I);
// Anti windup protection // Anti windup protection
@ -327,12 +259,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm); pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
} }
// if (motorLimitReached)
// {
// ITerm = pidProfileBuff->lastITerm[axis];
// }
pidProfileBuff->lastITerm[axis] = ITerm; pidProfileBuff->lastITerm[axis] = ITerm;
@ -383,16 +309,34 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
**************************************************************************/ **************************************************************************/
void pidUAV(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff) void pidUAV(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff)
{ {
float errorAxis[3] = { 0 }; /*Array of errors for each axis*/ float sensorValue[3] = { 0 }; /*Array of errors for each axis*/
float pointRate[3] = { 0 }; /*Array of desired values for each axis*/ float desiredValue[3] = { 0 }; /*Array of desired values for each axis*/
getCurrentValues(errorAxis, pidProfile->ID_profile); /*Get sensor values*/ getCurrentValues(sensorValue, pidProfile->ID_profile); /*Get sensor values*/
getPointRate(pointRate, pidProfile->ID_profile); /*Get reference values or desired values*/ getPointRate(desiredValue, pidProfile->ID_profile); /*Get reference values or desired values*/
/* -------------PID controller------------- */ /* -------------PID controller------------- */
for (int axis = 0; axis < pidProfileBuff->DOF; axis++) for (int axis = 0; axis < pidProfileBuff->DOF; axis++)
{ {
pidUAVcore(pidProfile, pidProfileBuff, pointRate[axis], errorAxis[axis], axis); pidUAVcore(pidProfile, pidProfileBuff, desiredValue[axis], sensorValue[axis], axis);
}
}
uint8_t flagAccBuff = 0;
void pidAccelerometer(void)
{
static uint8_t counterAcc = 0;
counterAcc = ((flagAccBuff == 0))? 0: counterAcc;
pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]);
if (counterAcc < 80)
{
PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
counterAcc +=1;
} }
} }
@ -425,10 +369,13 @@ void pidRun(uint8_t ID)
{ {
PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
flagAccBuff = 0;
throttleRate = 1;
} }
else else
{ {
pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); pidAccelerometer();
flagAccBuff = 1;
} }
break; break;
@ -446,13 +393,19 @@ 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;
} }
else else
{ {
pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]); 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] = 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);
} }
break; break;
@ -461,6 +414,12 @@ void pidRun(uint8_t ID)
} }
} }
void readAcc(void)
{
/*Reads data from accelerometer*/
mpu6000_read_angle(&accelProfile,&gyroProfile);
}
/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/ /*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
/************************************************************************** /**************************************************************************
@ -548,6 +507,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
PidProfile[ID].P[PITCH] = 135; PidProfile[ID].P[PITCH] = 135;
PidProfile[ID].P[YAW] = 150; PidProfile[ID].P[YAW] = 150;
PidProfile[ID].I[ROLL] = 50;
PidProfile[ID].I[PITCH] = 50;
PidProfile[ID].I[YAW] = 50;
PidProfile[ID].D[ROLL] = 75; PidProfile[ID].D[ROLL] = 75;
PidProfile[ID].D[PITCH] = 95; PidProfile[ID].D[PITCH] = 95;
PidProfile[ID].D[YAW] = 50; PidProfile[ID].D[YAW] = 50;
@ -563,16 +526,16 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break; break;
case PID_ID_ACCELEROMETER: case PID_ID_ACCELEROMETER:
PidProfile[ID].P[ROLL] = 90; PidProfile[ID].P[ROLL] = 120;
PidProfile[ID].P[PITCH] = 90; PidProfile[ID].P[PITCH] = 250;
PidProfile[ID].P[YAW] = 0; PidProfile[ID].P[YAW] = 0;
PidProfile[ID].D[ROLL] = 40; PidProfile[ID].D[ROLL] = 0;
PidProfile[ID].D[PITCH] = 40; PidProfile[ID].D[PITCH] = 0;
PidProfile[ID].D[YAW] = 0; PidProfile[ID].D[YAW] = 0;
PidProfile[ID].PIDweight[ROLL] = 2; PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 2; PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = true; PidProfile[ID].pidEnabled = true;
@ -583,27 +546,21 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
case PID_ID_COMPASS: case PID_ID_COMPASS:
PidProfile[ID].P[ROLL] = 10; PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
PidProfile[ID].PIDweight[ROLL] = 100; PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = false; PidProfile[ID].pidEnabled = false;
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
PidProfile[ID].P[ROLL] = 10; PidProfile[ID].P[ROLL] = 1;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
PidProfile[ID].PIDweight[ROLL] = 100; PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = false; PidProfile[ID].pidEnabled = false;
PidProfile[ID].dterm_lpf = 90;
PidProfile[ID].pid_out_limit = 2000;
break; break;
default: default:
@ -634,9 +591,10 @@ void pidInit()
void pidEproom(void) void pidEproom(void)
{ {
PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 20;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 20;
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_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //<2F>NDRA TILL SEKUNDER inte ms
@ -644,6 +602,5 @@ void pidEproom(void)
PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
PidProfile[PID_ID_GYRO].I[YAW] = 40;
} }

2
UAV-ControlSystem/src/Scheduler/scheduler.c
View File

@ -359,7 +359,7 @@ void initSchedulerTasks(void)
enableTask(TASK_ALTITUDE, true); enableTask(TASK_ALTITUDE, true);
#endif #endif
#if BEEPER #ifdef BEEPER
enableTask(TASK_BEEPER, true); enableTask(TASK_BEEPER, true);
#endif #endif
} }

68
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 */
@ -198,7 +198,7 @@ typedef enum
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
COMMAND_ID_PERIOD_ALTITUDE, COMMAND_ID_PERIOD_ALTITUDE,
#endif #endif
#if BEEPER #ifdef BEEPER
COMMAND_ID_PERIOD_BEEPER, COMMAND_ID_PERIOD_BEEPER,
#endif #endif
@ -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,
@ -401,7 +414,7 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
"task_period_altitude", COMMAND_ID_PERIOD_ALTITUDE, EEPROM_PERIOD_ALTITUDE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000} "task_period_altitude", COMMAND_ID_PERIOD_ALTITUDE, EEPROM_PERIOD_ALTITUDE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000}
}, },
#endif #endif
#if BEEPER #ifdef BEEPER
[COMMAND_ID_PERIOD_BEEPER] = [COMMAND_ID_PERIOD_BEEPER] =
{ {
"task_period_beeper", COMMAND_ID_PERIOD_BEEPER, EEPROM_PERIOD_BEEPER, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000} "task_period_beeper", COMMAND_ID_PERIOD_BEEPER, EEPROM_PERIOD_BEEPER, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000}
@ -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_BARO_I_HEIGHT] =
{
"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_BARO_D_HEIGHT] =
{
"pid_baro_d_height", COMMAND_ID_PID_BARO_D_HEIGHT, 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}
},
}; };

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

@ -189,7 +189,7 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
.dataPtr = &(SystemTasks[TASK_ALTITUDE].desiredPeriod), .dataPtr = &(SystemTasks[TASK_ALTITUDE].desiredPeriod),
}, },
#endif #endif
#if BEEPER #ifdef BEEPER
[EEPROM_PERIOD_BEEPER] = [EEPROM_PERIOD_BEEPER] =
{ {
.size = sizeof(SystemTasks[TASK_BEEPER].desiredPeriod), .size = sizeof(SystemTasks[TASK_BEEPER].desiredPeriod),
@ -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 */

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

@ -7,6 +7,10 @@
#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"
#include "config/cli.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 +247,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 +520,107 @@ bool mpu6000_who_am_i()
return false; return false;
} }
#define degToRad(x) ((x/180)*M_PI)
#define radToDeg(x) ((x/M_PI)*180)
void gyroEstimation(float p, float q, float r, float dT, accel_t* accel)
{
p = degToRad(p);
q = degToRad(q);
r = degToRad(r);
float radPitchAngle = degToRad(accel->pitchAngle);
float radRollAngle = degToRad(accel->rollAngle);
float pitchAngleRate = p + q*sinf(radPitchAngle)*tanf(radRollAngle) + r*cosf(radPitchAngle)*tanf(radRollAngle);
float rollAngleRate = q*cosf(radPitchAngle) - r*sinf(radPitchAngle);
accel->rollAngle = accel->rollAngle + (radToDeg(rollAngleRate)/1000000)*dT;
accel->pitchAngle = accel->pitchAngle + (radToDeg(pitchAngleRate)/1000000)*dT;
}
/* 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 (float)(2^30) // that means a CMP_FACTOR of 1024 (2^10)
#define ACCEL_LPF_FACTOR 16
#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 a_RollAngle = 0;
float a_PitchAngle = 0;
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] / ACCEL_LPF_FACTOR;
//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];
//New implementatioin ot apprximate the ange from gyro data
gyroEstimation((float)gyro->gyroX,(float)gyro->gyroY,(float)gyro->gyroZ, delta_t, accel);
//If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation
/*Use complementary filter to add gyro together with acc data to avoid drift over time
* fliter looks like: angle = 0.98*(angle + gyro * dt) + 0.02 * (accdata))*/
if (Low_Magnitude < magnitude && magnitude < High_Magnitude)
{
//Calculate the pure angle given by the accelerometer data
a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI;
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) / (float)4000;
//accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / (float)4000;
//Combine the gyro approximation with the accelerometer data to avoid drift
float ratio = 0.9996;
accel->rollAngle = (accel->rollAngle * ratio) + (a_RollAngle * (float)(1-ratio));
accel->pitchAngle = (accel->pitchAngle * ratio) + (a_PitchAngle * (float)(1-ratio));
}
}

242
UAV-ControlSystem/src/drivers/barometer.c
View File

@ -1,9 +1,24 @@
/* /**************************************************************************
* barometer.c * NAME: barometer.c *
* * *
* Created on: 18 okt. 2016 * AUTHOR: Jonas Holmberg *
* Author: holmis * *
*/ * PURPOSE: Used to provide an estimated altitude, in regards to the *
* lift of height that would represent zero meters in height. *
* *
* INFORMATION: Using I2C to communicate with the barometer a pressure and *
* temperature value can be obtained. These values can then be*
* used to estimate an altitude. Note that this is not an *
* altitude value relative to the ground underneath it. It is *
* relative to the position where the system was started from.*
* The start position of the system will indicate the zero *
* height. It is that position and only that one which will *
* be the compared height. *
* *
* GLOBAL VARIABLES: *
* Variable Type Description *
* -------- ---- ----------- *
***************************************************************************/
#include "drivers/barometer.h" #include "drivers/barometer.h"
#include "drivers/I2C.h" #include "drivers/I2C.h"
@ -13,7 +28,7 @@
#include "drivers/i2c_soft.h" #include "drivers/i2c_soft.h"
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#define Device_address_1 0x56 #define Device_address_1 0x56 //Address of our device, not really important in this case
#define ADDR_WRITE 0xEE // Module address write mode #define ADDR_WRITE 0xEE // Module address write mode
#define ADDR_READ 0xEF // Module address read mode #define ADDR_READ 0xEF // Module address read mode
@ -34,35 +49,65 @@
#define SEA_PRESS 1013.25 //default sea level pressure level in mb #define SEA_PRESS 1013.25 //default sea level pressure level in mb
#define FTMETERS 0.3048 //convert feet to meters #define FTMETERS 0.3048 //convert feet to meters
#define CALIBRATION_VAL_AMOUNT 30 #define CALIBRATION_VAL_AMOUNT 40
#define NUMB_AVERAGE_VALS 10
I2C_HandleTypeDef baroI2C_handle; I2C_HandleTypeDef baroI2C_handle; //Handle for the HW i2c (NOT USED ATM)
DMA_HandleTypeDef baroI2C_Rx_DMA_handle; DMA_HandleTypeDef baroI2C_Rx_DMA_handle; //Dma handle receive (NOT USED ATM)
DMA_HandleTypeDef baroI2C_Tx_DMA_handle; DMA_HandleTypeDef baroI2C_Tx_DMA_handle; //Dma handle for transmit (NOT USED ATM)
I2C_SOFT_handle_t baroI2C_soft_handle; I2C_SOFT_handle_t baroI2C_soft_handle; //Handle for the SW i2c
uint8_t sampleAmount;
uint8_t sampleAmount; //The amount of samples to be used when by the barometer to calculate the needed variables
double baro_Preassure; // compensated pressure value (mB) double baro_Preassure; // compensated pressure value (mB)
double baro_Temperature; // compensated temperature value (degC) double baro_Temperature; // compensated temperature value (degC)
double baro_Altitude; // altitude (ft) double baro_Altitude; // altitude
double baro_S; // sea level barometer (mB) double baro_S; // sea level barometer (mB)
/* Calibration variables */
float altitudeCalibrationValue = 0; //Value used as calibration value float altitudeCalibrationValue = 0; //Value used as calibration value
float calibrationSamples[CALIBRATION_VAL_AMOUNT]; //array of stored values to be used for calibration, only samples calibration values when machine is not armed float calibrationSamples[CALIBRATION_VAL_AMOUNT];//array of stored values to be used for calibration, only samples calibration values when machine is not armed
int calibrationSamplesCount = 0; int calibrationSamplesCount = 0; //Counter for the amount of calibration samples
int calibrationSamplesIterator = 0; int calibrationSamplesIterator = 0; //Iterator for when going through all the samples
//TODO: remove when not used for testing any more /* average altitude variables */
uint32_t tempTestCounterStart = 0; float averageAltitude[NUMB_AVERAGE_VALS];
uint8_t averageAltitudeIterator = 0;
uint8_t averageAltitudeCount = 0;
uint8_t cobuf[3] = {0};
/* address: 0 = factory data and the setup /* address: 0 = factory data and the setup
* address: 1-6 = calibration coefficients * address: 1-6 = calibration coefficients
* address: 7 = serial code and CRC */ * address: 7 = serial code and CRC */
uint32_t coefficients_arr[8]; //coefficient storage uint32_t coefficients_arr[8]; //coefficient storage
uint8_t cobuf[3] = {0}; //Array used when writing and reading data over the I2C
/***********************************************************************
* BRIEF: Adds a new altitude value to the average buffer vals *
* INFORMATION: Will add the last calculated altitude value to the *
* buffer used to provide a average calc of altitude *
***********************************************************************/
void barometer_addAverageAltitudeSample()
{
//fisrt check if the amount of samples is greater than the array
if (!(averageAltitudeCount >= NUMB_AVERAGE_VALS))
averageAltitudeCount++; //if not increase the counter
//Check if the iterator should restart from the beginning because of overflow
if (averageAltitudeIterator >= NUMB_AVERAGE_VALS)
averageAltitudeIterator = 0; //if it is set it to zero
//Add the lates calculated altitude value to the samples
averageAltitude[averageAltitudeIterator] = baro_Altitude;
//increase the iterator
averageAltitudeIterator ++;
}
/***********************************************************************
* BRIEF: Adds a new altitude value to the calibration samples. *
* INFORMATION: Will add the last calculated altitude value to the *
* buffer used to provide a calibration value. *
***********************************************************************/
void barometer_addCalibrationSample() void barometer_addCalibrationSample()
{ {
//fisrt check if the amount of samples is greater than the array //fisrt check if the amount of samples is greater than the array
@ -80,6 +125,16 @@ void barometer_addCalibrationSample()
calibrationSamplesIterator ++; calibrationSamplesIterator ++;
} }
/***********************************************************************
* BRIEF: Calibrates the barometers start position. *
* INFORMATION: An array of values are sampled as long as the system *
* is not armed. Upon arming the system the values in *
* the buffer will be averaged and this will give the *
* calibration value. In other words it will give the *
* height that represents zero. This value will be *
* subtracted from every altitude calculation that is *
* performed. *
***********************************************************************/
bool barometer_Calibrate() bool barometer_Calibrate()
{ {
//Check if any calibration values exist //Check if any calibration values exist
@ -108,6 +163,11 @@ bool barometer_Calibrate()
return true; return true;
} }
/***********************************************************************
* BRIEF: Initializes the barometer. *
* INFORMATION: Initializes the barometer and it needs to be called *
* before anything else when using the barometer. *
***********************************************************************/
bool barometer_init() bool barometer_init()
{ {
//Set the sample rate of the data that will be calculated on the barometer peripheral //Set the sample rate of the data that will be calculated on the barometer peripheral
@ -118,12 +178,20 @@ bool barometer_init()
i2c_soft_Init(I2C1, &baroI2C_soft_handle); i2c_soft_Init(I2C1, &baroI2C_soft_handle);
#endif #endif
#ifdef BARO_USE_I2C_HARD #ifdef BARO_USE_I2C_HARD
//Todo: needs implementation
#endif #endif
return true; return true;
} }
/***********************************************************************
* BRIEF: Resets the barometer. *
* INFORMATION: Resets the barometer needs to be called after the init.*
* It will send a reset message over the I2C to the *
* barometer telling it that is should perform a reset. *
* This needs to be done or it wont be possible to read *
* data from the barometer. *
***********************************************************************/
bool barometer_reset() bool barometer_reset()
{ {
/* Send a reset command to the baromter /* Send a reset command to the baromter
@ -142,6 +210,8 @@ bool barometer_reset()
#endif #endif
#ifdef BARO_USE_I2C_HARD #ifdef BARO_USE_I2C_HARD
//Todo: needs implementation
uint8_t cobuf2[3] = {0}; uint8_t cobuf2[3] = {0};
/* Change to hardware polling mode */ /* Change to hardware polling mode */
cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount); cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
@ -193,21 +263,21 @@ bool barometer_reset()
/* Set the inital calibration value */ /* Set the inital calibration value */
barometer_Calibrate(); barometer_Calibrate();
//force bakc the iscalibrated status to false //force bakc the iscalibrated status to false
flags_Clear_ID(systemFlags_barometerIsCalibrated_id); flags_Clear_ID(systemFlags_barometerIsCalibrated_id);
tempTestCounterStart = clock_get_ms();
return true; return true;
} }
typedef enum { /***********************************************************************
CALCSTATE_D2_CALCULATION = 0, //Tell the sensor that we want to read D2 * BRIEF: Calculates the values of temp, pres, and altitude. *
CALCSTATE_D2_READ, //Read D2 from the sensor * INFORMATION: It takes in two values D1 and D2 which are the values *
CALCSTATE_D1_CALCULATION, //Tell the sensor that we want to read D1 * that have been read from the barometer. This values are*
CALCSTATE_D1_READ, //Read D1 from the sensor * then used to perform the calculations together with *
CALCSTATE_CALCULATE_PTA //preassure, temp, altidute calc * the coefficients that have been read in the reset *
}calculationState; * function. *
***********************************************************************/
void barometer_CalculatePTA(uint32_t D1, uint32_t D2) void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
{ {
/* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */ /* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */
@ -243,10 +313,39 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
/* Calculate the altitude */ /* Calculate the altitude */
float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45; float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45;
baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS); baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS);
/* Add altitude values to altitude buffer containing the last few readings */
barometer_addAverageAltitudeSample();
} }
/***********************************************************************
* BRIEF: Calculates the values of the preassure, temperature and*
* altitude. *
* INFORMATION: This function needs to be called five times for the *
* data to be updated. This is because of some limitations*
* and to ensure the schedulability of the system it needs*
* to be divided. Firstly there is an inherit delay inside*
* the barometer sensor. To get data from the barometer a *
* message needs to be sent that tells the barometer to *
* prepare the data. This takes, depending on the amount *
* of sampling that is done up to 10 ms for the highest *
* amount of sampling. This also needs to be done two *
* times before that data can be calculated. Also since *
* the implementation uses a software I2C at the moment *
* because of some problems with the DMA implementation *
* the speed is not very high. Therefore sending several *
* messages and reading at the same time may take to long *
* time and could cause the system to be unschedulable. *
* Because of this the function is divided into different *
* cases: *
* 1: Prepare data. *
* 2: Read data. *
* 3: Prepare data. *
* 4: Read data. *
* 5: Calculate temperature, pressure and altitude. *
***********************************************************************/
void barometer_CaclulateValues() void barometer_CaclulateValues()
{ {
/*the calculation is in need of different states. This is because the /*the calculation is in need of different states. This is because the
@ -254,7 +353,8 @@ void barometer_CaclulateValues()
* use a delay wait we need to do parts of the calculation every time * use a delay wait we need to do parts of the calculation every time
* the function is called. The "delay" is handled by the period of * the function is called. The "delay" is handled by the period of
* the task that handles the calculation. It cant have a period faster * the task that handles the calculation. It cant have a period faster
* that 10 ms, or the wait will not be enough in some cases according * that 10 ms or 5ms or 2.5 ms and so on, depending on the CMD_ADC_ assigned
* to the variable "sampleAmount" The wait will not be enough in some cases according
* to the datasheet of the sensor http://www.amsys.info/sheets/amsys.en.ms5611_01ba03.pdf*/ * to the datasheet of the sensor http://www.amsys.info/sheets/amsys.en.ms5611_01ba03.pdf*/
static uint8_t currentCalculationState = CALCSTATE_D2_CALCULATION; static uint8_t currentCalculationState = CALCSTATE_D2_CALCULATION;
static uint32_t D1 = 0; static uint32_t D1 = 0;
@ -263,7 +363,6 @@ void barometer_CaclulateValues()
uint32_t startTime; uint32_t startTime;
uint32_t endTime; uint32_t endTime;
//If the machine is armed and not calibrated we perform a calibraton //If the machine is armed and not calibrated we perform a calibraton
if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id))
{ {
@ -273,8 +372,6 @@ void barometer_CaclulateValues()
} }
} }
switch (currentCalculationState) switch (currentCalculationState)
{ {
case CALCSTATE_D2_CALCULATION: case CALCSTATE_D2_CALCULATION:
@ -365,19 +462,88 @@ void barometer_CaclulateValues()
} }
double barometer_GetCurrentPreassure() /***********************************************************************
* BRIEF: Retrieves the previously calculated pressure. *
* INFORMATION: Returns the last calculated pressure value. No *
* calculation is performed here so calling this will give*
* the same value until a new calculation has been *
* performed. *
***********************************************************************/
double barometer_GetCurrentPressure()
{ {
return baro_Preassure; return baro_Preassure;
} }
/***********************************************************************
* BRIEF: Retrieves the previously calculated temperature. *
* INFORMATION: Returns the last calculated temperature value. No *
* calculation is performed here so calling this will give*
* the same value until a new calculation has been *
* performed. *
***********************************************************************/
double barometer_GetCurrentTemperature() double barometer_GetCurrentTemperature()
{ {
return baro_Temperature; return baro_Temperature;
} }
float barometer_GetCurrentAltitudeBasedOnSeaLevel() /***********************************************************************
* BRIEF: Retrieves the previously calculated altitude. *
* INFORMATION: Returns the last calculated altitude value. No *
* calculation is performed here so calling this will give*
* the same value until a new calculation has been *
* performed. *
***********************************************************************/
float barometer_GetCurrentAltitude()
{ {
return baro_Altitude; return baro_Altitude;
} }
/***********************************************************************
* BRIEF: Gets the altitude based on the last number of values. *
* INFORMATION: Averages the value on the last few reading to get a more*
* accurate reading. *
***********************************************************************/
float barometer_GetCurrentAveragedtAltitude()
{
// float toReturn = 0;
// /* Go through all the values in the buffer */
// for (int i = 0; i < averageAltitudeCount; i++)
// {
// toReturn += averageAltitude[i];
// }
//
// /* Return the average of the stored values */
// toReturn = toReturn/averageAltitudeCount;
// return toReturn;
//
static float lpf_Acc = 0;
static float smooth = 0;
float toReturn = 0;
/* Filter part, go thorugh each axis */
//Calculate a new smooth value based on a factor of the LPF value
smooth = lpf_Acc / 16;
//Calculate the new LPF value based on the raw sensor data - the smoothing value
lpf_Acc += baro_Altitude - smooth;
//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
// toReturn = smooth * smooth;
return smooth;
// static float prevVal = 0;
// float toRet = (prevVal*6 + baro_Altitude) / 8;
// prevVal = baro_Altitude;
// return toRet;
}

34
UAV-ControlSystem/src/drivers/beeper.c Normal file
View File

@ -0,0 +1,34 @@
/*
* beeper.c
*
* Created on: 14 nov. 2016
* Author: holmis
*/
#include "drivers/beeper.h"
uint16_t beeperPin;
GPIO_TypeDef* beeperPort;
void initBeeper(uint16_t beeper_pin, GPIO_TypeDef* beeper_port)
{
beeperPin = beeper_pin;
beeperPort = beeper_port;
GPIO_InitTypeDef gpinit;
gpinit.Pin = beeper_pin;
gpinit.Mode = GPIO_MODE_OUTPUT_PP;
gpinit.Pull = GPIO_PULLUP;
gpinit.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(beeper_port, &gpinit);
}
void busyWaitBeep(uint16_t beepTimeMs)
{
/* If you use this in the scheduled part of the code, you might face a problem with a little bit of a crash ok? */
HAL_GPIO_WritePin(beeperPort, beeperPin, SET);
HAL_Delay(beepTimeMs);
HAL_GPIO_WritePin(beeperPort, beeperPin, RESET);
}

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

@ -40,9 +40,9 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
.flagId = systemFlags_flightmode_acceleromter_id, .flagId = systemFlags_flightmode_acceleromter_id,
}, },
[FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = { [FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = {
.minRange = 0, .minRange = 1900,
.maxRange = 0, .maxRange = 2100,
.channelNumber = 0, .channelNumber = 6,
.flagId = systemFlags_flightmode_barometer_id, .flagId = systemFlags_flightmode_barometer_id,
}, },
[FLAG_CONFIGURATION_FLIGHTMODE_COMPASS] = { [FLAG_CONFIGURATION_FLIGHTMODE_COMPASS] = {

62
UAV-ControlSystem/src/drivers/i2c_soft.c
View File

@ -11,21 +11,37 @@
#define WRITE_INDICATOR 0 #define WRITE_INDICATOR 0
#define READ_INDICATOR 1 #define READ_INDICATOR 1
/***********************************************************************
* BRIEF: set given pin to high
* INFORMATION:
***********************************************************************/
static void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) static void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{ {
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
} }
/***********************************************************************
* BRIEF: Set given pin to low
* INFORMATION:
***********************************************************************/
static void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) static void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{ {
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
} }
/***********************************************************************
* BRIEF: Read given ii pin
* INFORMATION:
***********************************************************************/
static bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) static bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{ {
return !! (GPIOx->IDR & GPIO_Pin); return !! (GPIOx->IDR & GPIO_Pin);
} }
/***********************************************************************
* BRIEF: Delay for a few micros
* INFORMATION:
***********************************************************************/
static void i2c_soft_delay(void) static void i2c_soft_delay(void)
{ {
volatile int i = 1; volatile int i = 1;
@ -34,6 +50,11 @@ static void i2c_soft_delay(void)
} }
} }
/***********************************************************************
* BRIEF: Initializes the SW I2C.
* INFORMATION: Initializes the SW I2C, needs to be done before any
* thing else.
***********************************************************************/
void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile) void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile)
{ {
uint16_t sda_pin, scl_pin; uint16_t sda_pin, scl_pin;
@ -67,6 +88,10 @@ void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile)
out_profile->i2c_sda_pin = sda_pin; out_profile->i2c_sda_pin = sda_pin;
} }
/***********************************************************************
* BRIEF: Starts the i2c
* INFORMATION:
***********************************************************************/
static bool i2c_soft_Start(I2C_SOFT_handle_t *handle) static bool i2c_soft_Start(I2C_SOFT_handle_t *handle)
{ {
IOHi(handle->i2c_Port, handle->i2c_sda_pin); IOHi(handle->i2c_Port, handle->i2c_sda_pin);
@ -85,6 +110,10 @@ static bool i2c_soft_Start(I2C_SOFT_handle_t *handle)
return true; return true;
} }
/***********************************************************************
* BRIEF: Stops the i2c
* INFORMATION:
***********************************************************************/
static void i2c_soft_Stop(I2C_SOFT_handle_t *handle) static void i2c_soft_Stop(I2C_SOFT_handle_t *handle)
{ {
IOLo(handle->i2c_Port, handle->i2c_scl_pin); IOLo(handle->i2c_Port, handle->i2c_scl_pin);
@ -97,6 +126,10 @@ static void i2c_soft_Stop(I2C_SOFT_handle_t *handle)
asm ("nop"); // i2c_soft_delay(); asm ("nop"); // i2c_soft_delay();
} }
/***********************************************************************
* BRIEF: Sends ack
* INFORMATION:
***********************************************************************/
static void i2c_soft_Ack(I2C_SOFT_handle_t *handle) static void i2c_soft_Ack(I2C_SOFT_handle_t *handle)
{ {
IOLo(handle->i2c_Port, handle->i2c_scl_pin); IOLo(handle->i2c_Port, handle->i2c_scl_pin);
@ -109,6 +142,10 @@ static void i2c_soft_Ack(I2C_SOFT_handle_t *handle)
asm ("nop"); // i2c_soft_delay(); asm ("nop"); // i2c_soft_delay();
} }
/***********************************************************************
* BRIEF: Sends no ack
* INFORMATION:
***********************************************************************/
static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle) static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle)
{ {
IOLo(handle->i2c_Port, handle->i2c_scl_pin); IOLo(handle->i2c_Port, handle->i2c_scl_pin);
@ -121,6 +158,10 @@ static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle)
asm ("nop"); // i2c_soft_delay(); asm ("nop"); // i2c_soft_delay();
} }
/***********************************************************************
* BRIEF: Wait for an acknowledge.
* INFORMATION: Waits for an acknowledge when a message has been sent.
***********************************************************************/
static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle) static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle)
{ {
IOLo(handle->i2c_Port, handle->i2c_scl_pin); IOLo(handle->i2c_Port, handle->i2c_scl_pin);
@ -137,6 +178,10 @@ static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle)
return true; return true;
} }
/***********************************************************************
* BRIEF: Sends a byte.
* INFORMATION: Sends the value byte over the i2c.
***********************************************************************/
static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte) static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte)
{ {
uint8_t i = 8; uint8_t i = 8;
@ -157,6 +202,10 @@ static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte)
IOLo(handle->i2c_Port, handle->i2c_scl_pin); IOLo(handle->i2c_Port, handle->i2c_scl_pin);
} }
/***********************************************************************
* BRIEF: Receives a byte.
* INFORMATION: Receives a byte and stores is in the byte value.
***********************************************************************/
static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle) static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle)
{ {
uint8_t i = 8; uint8_t i = 8;
@ -177,6 +226,13 @@ static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle)
return byte; return byte;
} }
/***********************************************************************
* BRIEF: Reads a message.
* INFORMATION: Tries to read a message from addr. reg is the message
* that says a read is desired. len is the length of the
* message that should be read and buf is the buffer that
* will store the read data.
***********************************************************************/
bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{ {
//just send the addres 0x77 //just send the addres 0x77
@ -212,6 +268,12 @@ bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t
return true; return true;
} }
/***********************************************************************
* BRIEF: Writes a message.
* INFORMATION: Tries to write to an address. reg is the message that is
* written to the addr. data is the size of the data that
* is written.
***********************************************************************/
bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data) bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data)
{ {
//just send the addres 0x77 //just send the addres 0x77

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

@ -103,8 +103,9 @@ void mix()
int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array
int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor
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];//*throttleRate;
if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;
/* Mixer Full Scale enabled */ /* Mixer Full Scale enabled */
if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
@ -182,7 +183,11 @@ void mix()
// Now we add desired throttle // Now we add desired throttle
for (int i = 0; i < MOTOR_COUNT; i++) for (int i = 0; i < MOTOR_COUNT; i++)
// Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure // Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure
// TODO: This line i>>>>>>>>>s backup as we discovered that motors could stop at times in airmode on M-UAV. But we have not seen this before
motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax); motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
//motor_output[i] = constrain(RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax), throttleMin, throttleMax);
} }
else // Mixer full scale NOT active else // Mixer full scale NOT active
{ {
@ -287,6 +292,13 @@ void mix()
else else
motor_output[i] = mixerConfig.minCommand; motor_output[i] = mixerConfig.minCommand;
/* TODO: This is temp fix to be able to disable all motors but one */
// int enabled_motorA = 0;
// int enabled_motorB = 5;
// if (i != enabled_motorA && i != enabled_motorB)
// motor_output[i] = mixerConfig.minCommand;
/* Update actuators */ /* Update actuators */
pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]); pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]);
} }

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

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

@ -29,6 +29,7 @@
#include "drivers/motors.h" #include "drivers/motors.h"
#include "Flight/pid.h" #include "Flight/pid.h"
#include "drivers/barometer.h"#include "drivers/arduino_com.h" #include "drivers/barometer.h"#include "drivers/arduino_com.h"
#include "drivers/beeper.h"
/************************************************************************** /**************************************************************************
* BRIEF: Should contain all the initializations of the system, needs to * BRIEF: Should contain all the initializations of the system, needs to
@ -74,8 +75,8 @@ void init_system()
#endif #endif
#ifdef BARO #ifdef BARO
//barometer_init(); barometer_init();
//barometer_reset(); barometer_reset();
#endif #endif
#ifdef COMPASS #ifdef COMPASS
@ -94,8 +95,8 @@ void init_system()
#endif #endif
#if BEEPER #ifdef BEEPER
initBeeper(BEEPER_PIN, BEEPER_PORT);
#endif #endif
@ -118,6 +119,9 @@ int main(void)
//Light the yellow led //Light the yellow led
ledOnInverted(Led1, Led1_GPIO_PORT); ledOnInverted(Led1, Led1_GPIO_PORT);
//beep that it has been initialized
busyWaitBeep(1000);
//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();

31
UAV-ControlSystem/src/tasks_main.c
View File

@ -42,21 +42,14 @@
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "Flight/pid.h" #include "Flight/pid.h"
#include "drivers/barometer.h" #include "drivers/barometer.h"
#include "drivers/beeper.h"
void systemTaskGyroPid(void) void systemTaskGyroPid(void)
{ {
//Read gyro and update PID and finally update the motors. The most important task in the system
//Update Gyro
//Convert?
//PID Gyro //PID Gyro
pidRun(PID_ID_GYRO); pidRun(PID_ID_GYRO);
//MIX GO
//call the motor mix //call the motor mix
mix(); mix();
} }
@ -64,10 +57,8 @@ void systemTaskGyroPid(void)
void systemTaskAccelerometer(void) void systemTaskAccelerometer(void)
{ {
readAcc();
pidRun(PID_ID_ACCELEROMETER); pidRun(PID_ID_ACCELEROMETER);
//update the accelerometer data
// uint8_t c = 97;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
} }
void systemTaskAttitude(void) void systemTaskAttitude(void)
@ -112,7 +103,6 @@ void systemTaskRx(void)
continuousMissedFrames = (frame.flag_FrameLost) ? continuousMissedFrames + 1 : 0; continuousMissedFrames = (frame.flag_FrameLost) ? continuousMissedFrames + 1 : 0;
(continuousMissedFrames > 10) ? flags_Set_ID(systemFlags_Failsafe_toManyMissedFrames_id) : flags_Clear_ID(systemFlags_Failsafe_toManyMissedFrames_id); (continuousMissedFrames > 10) ? flags_Set_ID(systemFlags_Failsafe_toManyMissedFrames_id) : flags_Clear_ID(systemFlags_Failsafe_toManyMissedFrames_id);
} }
bool systemTaskRxCheck(uint32_t currentDeltaTime) bool systemTaskRxCheck(uint32_t currentDeltaTime)
@ -163,6 +153,7 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
return false; return false;
} }
//TODO: change the name of this task. Could be something like
void systemTaskSerial(void) void systemTaskSerial(void)
{ {
static bool readyToCalibrate = true; static bool readyToCalibrate = true;
@ -180,23 +171,30 @@ void systemTaskSerial(void)
// mpu6000_read_acc_offset(&accelProfile); // mpu6000_read_acc_offset(&accelProfile);
// } // }
// } // }
//If we are ready to accept a new calibration value. You can only perform one calibration until the sticks have been centered once until the next calibration
if(readyToCalibrate) if(readyToCalibrate)
{ {
//If any calibration is performed set readyToCalibrate to false so it cant just increase indefinitely when holding the sticks in a certain position
if (flags_IsSet_ID(systemFlags_stickLeft_id)) if (flags_IsSet_ID(systemFlags_stickLeft_id))
{ {
accRollFineTune -= calibrationAmount; accRollFineTune -= calibrationAmount;
readyToCalibrate = false;
} }
else if (flags_IsSet_ID(systemFlags_stickRight_id)) else if (flags_IsSet_ID(systemFlags_stickRight_id))
{ {
accRollFineTune += calibrationAmount; accRollFineTune += calibrationAmount;
readyToCalibrate = false;
} }
else if (flags_IsSet_ID(systemFlags_stickUp_id)) else if (flags_IsSet_ID(systemFlags_stickUp_id))
{ {
accPitchFineTune -= calibrationAmount; accPitchFineTune -= calibrationAmount;
readyToCalibrate = false;
} }
else if (flags_IsSet_ID(systemFlags_stickDown_id)) else if (flags_IsSet_ID(systemFlags_stickDown_id))
{ {
accPitchFineTune += calibrationAmount; accPitchFineTune += calibrationAmount;
readyToCalibrate = false;
} }
} }
@ -226,7 +224,7 @@ void systemTaskBattery(void)
void systemTaskBaro(void) void systemTaskBaro(void)
{ {
//barometer_CaclulateValues(); barometer_CaclulateValues();
} }
void systemTaskCompass(void) void systemTaskCompass(void)
@ -246,11 +244,16 @@ void systemTaskSonar(void)
void systemTaskAltitude(void) void systemTaskAltitude(void)
{ {
// uint8_t c[50];
// sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle);
// usart_transmit(&cliUsart, &c, 50, 1000000000);
//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar //Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
//double temperature = barometer_GetCurrentTemperature(); //double temperature = barometer_GetCurrentTemperature();
//double pressure = barometer_GetCurrentPreassure(); //double pressure = barometer_GetCurrentPreassure();
//float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel(); //float altitute = barometer_GetCurrentAltitude();
//float altitute = barometer_GetCurrentAveragedtAltitude();
//pid run, should probably be moved to systemTaskAltitude //pid run, should probably be moved to systemTaskAltitude
pidRun(PID_ID_BAROMETER); pidRun(PID_ID_BAROMETER);

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