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

PID baro fix loosing heoight to fast

Browse Source
This commit is contained in:
johan9107 2016-11-11 17:49:01 +01:00
parent 4a1ac00dc1
commit 84b3a729e1
1 changed files with 40 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,7 +26,7 @@
#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 20 #define BAROMETER_SCALE 11
#define RADIO_RANGE 500 /*Radio range input*/ #define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ #define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/
@ -79,12 +79,19 @@ 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;
float VelocityCompensation = 0;
/************************************************************************** /**************************************************************************
* BRIEF: Update current sensor values * * BRIEF: Update current sensor values *
* INFORMATION: * * INFORMATION: *
**************************************************************************/ **************************************************************************/
void getCurrentValues(float sensorValues[3], uint8_t ID_profile) void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{ {
static float last_micros = 0;
static float oldHeightValue = 0;
static uint8_t counter = 0;
switch (ID_profile) switch (ID_profile)
{ {
case PID_ID_GYRO: case PID_ID_GYRO:
@ -117,7 +124,36 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*BAROMETER_SCALE; sensorValues[0] = barometer_GetCurrentAveragedtAltitude();
counter += 1;
if (counter > 2)
{
float current_micros = clock_get_us()/1000000;
float delta_t = (current_micros - last_micros);
last_micros = current_micros;
counter = 0;
float current_velocity = (sensorValues[0] - oldHeightValue)/delta_t;
if( current_velocity<= -10)
{
VelocityCompensation = ABS_FLOAT(current_velocity*3);
}
else
{
VelocityCompensation = 0;
}
}
oldHeightValue = sensorValues[0];
sensorValues[0]*=BAROMETER_SCALE;
break; break;
default: default:
@ -353,7 +389,8 @@ void pidRun(uint8_t ID)
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], -15, (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 + (int)VelocityCompensation);
} }