rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

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

Browse Source 
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
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/Modules/AltitudeHold/altitudehold.c
View File

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