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

Merge remote-tracking branch 'refs/remotes/origin/PID-Improved' into baro2

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-01 09:57:55 +01:00
commit 195b311b88
2 changed files with 29 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,18 +18,20 @@
#include "scheduler/scheduler.h"
#include <math.h>
#include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h"
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
#define ITERM_SCALE 0.244381f /*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 RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/
#define ACCELEROMETER_RANGE 35 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/
#define 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*/
/*Struct that belongs to a certain PID controller*/
@ -67,7 +69,6 @@ float accPitchFineTune = 0;
float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
{
float angle;
float angle_offset = (z_axis < 0 )? 90: 0;
switch (axis)
{
@ -91,6 +92,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
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 *
* INFORMATION: *
@ -117,8 +123,8 @@ float constrainf(float amt, int low, int high)
float oldSensorValue[2] = {0};
float oldSensorValueRoll[50] = {0};
float oldSensorValuePitch[50] = {0};
float oldSensorValueRoll[12] = {0};
float oldSensorValuePitch[12] = {0};
int i = 0;
@ -146,7 +152,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
@ -164,7 +169,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
float RollValue = 0;
float PitchValue = 0;
for (int ii = 0; ii < 50; ii++)
for (int ii = 0; ii < 12; ii++)
{
RollValue = RollValue + oldSensorValueRoll[ii];
PitchValue = PitchValue + oldSensorValuePitch[ii];
@ -172,13 +177,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
}
i = (i < 49)? i + 1:0;
i = (i < 11)? i + 1:0;
sensorValues[ROLL] = RollValue/50;
sensorValues[PITCH] = PitchValue/50;
sensorValues[ROLL] = RollValue/12;
sensorValues[PITCH] = PitchValue/12;
sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1];
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];
@ -291,7 +296,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/* -----calculate I component ---- */
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * 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.
// I coefficient (I) moved before integration to make limiting independent from PID settings
@ -341,6 +346,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/*----PID OUT----*/
if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck))
{
ITerm = 0;
pidProfileBuff->lastITerm[axis] = 0;
}
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
}

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

@ -47,6 +47,10 @@ void init_system()
//Configure the clock
system_clock_config();
//init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125);
//Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer.
pidInit();
@ -97,9 +101,7 @@ void init_system()
#endif
//init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125);
}