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

Added filter to the barometer data. Also some fixes in PID for barometer. Also added simple beeper code.

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-17 10:18:39 +01:00
parent 497e00aa2f
commit 3407813f1e
6 changed files with 100 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -157,7 +157,7 @@
/* Beeper */ /* Beeper */
#define BEEPER #define BEEPER
#define BEEPER_PIN 12 #define BEEPER_PIN GPIO_PIN_12
#define BEEPER_PORT GPIOB #define BEEPER_PORT GPIOB

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 110 #define EEPROM_SYS_VERSION 114
#define ADC_STATE #define ADC_STATE
#include "stm32f4xx.h" #include "stm32f4xx.h"

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

@ -21,12 +21,13 @@
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "utilities.h" #include "utilities.h"
#include "drivers/barometer.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.000529f /*D-term used as a scale value to the PID controller*/
#define BAROMETER_SCALE 11 #define BAROMETER_SCALE 5
#define RADIO_RANGE 500 /*Radio range input*/ #define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/ #define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/
@ -90,6 +91,10 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{ {
static float last_micros = 0; static float last_micros = 0;
static float oldHeightValue = 0; static float oldHeightValue = 0;
float current_micros = 0;
float delta_t_baro = 0;
float current_height = 0;
switch (ID_profile) switch (ID_profile)
{ {
@ -123,20 +128,24 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
float current_micros = clock_get_us()/1000000; current_micros = clock_get_us();
float delta_t = (current_micros - last_micros); current_micros = current_micros/1000000;
float current_height = barometer_GetCurrentAveragedtAltitude(); delta_t_baro = (current_micros - last_micros);
current_height = barometer_GetCurrentAveragedtAltitude();
last_micros = current_micros; last_micros = current_micros;
sensorValues[0] = ((current_height - oldHeightValue)/delta_t); sensorValues[0] = ((current_height - oldHeightValue)/delta_t_baro);
sensorValues[0] = (sensorValues[0] < - 6 || sensorValues[0] > 6)? sensorValues[0]:0; //sensorValues[0] = ((sensorValues[0] < - 2) || (sensorValues[0] > 2))? sensorValues[0]:0;
oldHeightValue = current_height; oldHeightValue = current_height;
sensorValues[0]*=BAROMETER_SCALE; sensorValues[0]*=BAROMETER_SCALE;
break; 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;
@ -153,6 +162,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)
{ {
@ -193,9 +206,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
float currentThrottle = rc_input.Throttle - 1500; currentThrottle = rc_input.Throttle - 1500;
float velocity = (currentThrottle < - 10 || currentThrottle > 10 )? currentThrottle:0; velocity = (currentThrottle < - 20 || currentThrottle > 20 )? currentThrottle:0;
desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle)*BAROMETER_SCALE; desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, velocity)*BAROMETER_SCALE;
break; break;
default: default:
@ -209,9 +222,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 ---- */
@ -299,16 +312,16 @@ 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);
} }
} }
@ -371,8 +384,8 @@ void pidRun(uint8_t ID)
{ {
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], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit + (int)VelocityCompensation); //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;
@ -552,7 +565,7 @@ void pidInit()
void pidEproom(void) void pidEproom(void)
{ {
PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 254; PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;

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

@ -315,6 +315,7 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
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 */ /* Add altitude values to altitude buffer containing the last few readings */
barometer_addAverageAltitudeSample(); barometer_addAverageAltitudeSample();
} }
@ -504,16 +505,45 @@ float barometer_GetCurrentAltitude()
***********************************************************************/ ***********************************************************************/
float barometer_GetCurrentAveragedtAltitude() 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; 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; /* Filter part, go thorugh each axis */
return toReturn;
//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;
} }

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

@ -11,17 +11,17 @@
uint16_t beeperPin; uint16_t beeperPin;
GPIO_TypeDef* beeperPort; GPIO_TypeDef* beeperPort;
void initBeeper(uint16_t led_pin, GPIO_TypeDef* led_port) void initBeeper(uint16_t beeper_pin, GPIO_TypeDef* beeper_port)
{ {
beeperPin = led_pin; beeperPin = beeper_pin;
beeperPort = led_port; beeperPort = beeper_port;
GPIO_InitTypeDef gpinit; GPIO_InitTypeDef gpinit;
gpinit.Pin = led_pin; gpinit.Pin = beeper_pin;
gpinit.Mode = GPIO_MODE_OUTPUT_OD; gpinit.Mode = GPIO_MODE_OUTPUT_PP;
gpinit.Pull = GPIO_NOPULL; gpinit.Pull = GPIO_PULLUP;
gpinit.Speed = GPIO_SPEED_HIGH; gpinit.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(led_port, &gpinit); HAL_GPIO_Init(beeper_port, &gpinit);
} }

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

@ -74,30 +74,17 @@ typedef struct {
*/ */
static const motorMixer_s mixerUAV[] = { static const motorMixer_s mixerUAV[] = {
/* Throttle, Roll, Pitch, Yaw */ /* Throttle, Roll, Pitch, Yaw */
//
// { 1.0f, 1.0f, 1.0f, -1.0f}, //M1
// { 1.0f, 1.0f, 1.0f, 1.0f}, //M2
// { 1.0f, 0.0f, 1.0f, 0.0f}, //M3
// { 1.0f, 0.0f, 1.0f, 0.0f}, //M4
// { 1.0f, -1.0f, 1.0f, 1.0f}, //M5
// { 1.0f, -1.0f, 1.0f, -1.0f}, //M6
// { 1.0f, 1.0f, -1.0f, 1.0f}, //M7
// { 1.0f, 1.0f, -1.0f, -1.0f}, //M8
// { 1.0f, -1.0f, -1.0f, -1.0f}, //M9
// { 1.0f, -1.0f, -1.0f, 1.0f}, //M10
{ 1.0f, 1.0f, 1.0f, -1.0f}, //M1 { 1.0f, 1.0f, 1.0f, -1.0f}, //M1
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M2 { 1.0f, 1.0f, 1.0f, 1.0f}, //M2
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M3 { 1.0f, 0.0f, 1.0f, 0.0f}, //M3
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M4 { 1.0f, 0.0f, 1.0f, 0.0f}, //M4
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M5 { 1.0f, -1.0f, 1.0f, 1.0f}, //M5
{ 1.0f, -1.0f, 1.0f, -1.0f}, //M6 { 1.0f, -1.0f, 1.0f, -1.0f}, //M6
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M7 { 1.0f, 1.0f, -1.0f, 1.0f}, //M7
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M8 { 1.0f, 1.0f, -1.0f, -1.0f}, //M8
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M9 { 1.0f, -1.0f, -1.0f, -1.0f}, //M9
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M10 { 1.0f, -1.0f, -1.0f, 1.0f}, //M10
}; };
@ -197,9 +184,9 @@ void mix()
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 is backup as we discovered that motors could stop at times in airmode on M-UAV. But we have not seen this before // 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); //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
@ -305,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]);
} }