1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-28 17:54:15 +01:00

Use backtracking to limit integral in altitude hold and smooth the velocity

error.  Initialize the throttle integral better.
This commit is contained in:
James Cotton 2012-02-08 10:38:04 -06:00
parent d5207c19f0
commit 60c951843d

@ -106,13 +106,13 @@ float velocity, lastAltitude;
float throttleIntegral; float throttleIntegral;
float decay; float decay;
bool running = false; bool running = false;
float error;
/** /**
* Module thread, should not return. * Module thread, should not return.
*/ */
static void altitudeHoldTask(void *parameters) static void altitudeHoldTask(void *parameters)
{ {
AltitudeHoldSettingsData altitudeHoldSettings;
AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredData altitudeHoldDesired;
BaroAltitudeData baroAltitude; BaroAltitudeData baroAltitude;
StabilizationDesiredData stabilizationDesired; StabilizationDesiredData stabilizationDesired;
@ -130,6 +130,9 @@ static void altitudeHoldTask(void *parameters)
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE ) if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE )
{ {
if(!running)
throttleIntegral = 0;
// Todo: Add alarm if it should be running // Todo: Add alarm if it should be running
continue; continue;
} else if (ev.obj == BaroAltitudeHandle()) { } else if (ev.obj == BaroAltitudeHandle()) {
@ -150,11 +153,11 @@ static void altitudeHoldTask(void *parameters)
lastSysTime = thisTime; lastSysTime = thisTime;
// Flipping sign on error since altitude is "down" // Flipping sign on error since altitude is "down"
float error = (altitudeHoldDesired.Altitude - baroAltitude.Altitude); error = (altitudeHoldDesired.Altitude - baroAltitude.Altitude);
// Estimate velocity by smoothing derivative // Estimate velocity by smoothing derivative
decay = expf(-dT / tau); decay = expf(-dT / altitudeHoldSettings.Tau);
velocity = velocity * decay + (baroAltitude.Altitude - lastAltitude) / dT * (1-decay); // m/s velocity = velocity * decay + (baroAltitude.Altitude - lastAltitude) / dT * (1.0f-decay); // m/s
lastAltitude = baroAltitude.Altitude; lastAltitude = baroAltitude.Altitude;
// Compute integral off altitude error // Compute integral off altitude error
@ -187,7 +190,6 @@ static void altitudeHoldTask(void *parameters)
BaroAltitudeConnectQueue(queue); BaroAltitudeConnectQueue(queue);
// Copy the current throttle as a starting point for integral // Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral); StabilizationDesiredThrottleGet(&throttleIntegral);
throttleIntegral /= altitudeHoldSettings.Ki;
running = true; running = true;
} }
@ -201,6 +203,4 @@ static void altitudeHoldTask(void *parameters)
static void SettingsUpdatedCb(UAVObjEvent * ev) static void SettingsUpdatedCb(UAVObjEvent * ev)
{ {
AltitudeHoldSettingsGet(&altitudeHoldSettings); AltitudeHoldSettingsGet(&altitudeHoldSettings);
tau = altitudeHoldSettings.Tau / 1000.0f;
} }