From ab41bae071182073448ed93e78f27791f62f79db Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 8 Feb 2012 10:56:39 -0600 Subject: [PATCH] Separately smooth the error estimate from the derivative --- flight/Modules/AltitudeHold/altitudehold.c | 12 +++++++++--- shared/uavobjectdefinition/altitudeholdsettings.xml | 1 + 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/flight/Modules/AltitudeHold/altitudehold.c b/flight/Modules/AltitudeHold/altitudehold.c index ce435196e..d45d5016f 100644 --- a/flight/Modules/AltitudeHold/altitudehold.c +++ b/flight/Modules/AltitudeHold/altitudehold.c @@ -105,6 +105,7 @@ float tau; float velocity, lastAltitude; float throttleIntegral; float decay; +float velocity_decay; bool running = false; float error; @@ -123,6 +124,8 @@ static void altitudeHoldTask(void *parameters) // Force update of the settings SettingsUpdatedCb(&ev); + running = false; + // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { @@ -153,11 +156,12 @@ static void altitudeHoldTask(void *parameters) lastSysTime = thisTime; // Flipping sign on error since altitude is "down" - error = (altitudeHoldDesired.Altitude - baroAltitude.Altitude); + decay = expf(-dT / altitudeHoldSettings.Tau); + error = error * decay + (altitudeHoldDesired.Altitude - baroAltitude.Altitude) * (1.0f - decay); // Estimate velocity by smoothing derivative - decay = expf(-dT / altitudeHoldSettings.Tau); - velocity = velocity * decay + (baroAltitude.Altitude - lastAltitude) / dT * (1.0f-decay); // m/s + velocity_decay = expf(-dT / altitudeHoldSettings.DerivativeTau); + velocity = velocity * velocity_decay + (baroAltitude.Altitude - lastAltitude) / dT * (1.0f-velocity_decay); // m/s lastAltitude = baroAltitude.Altitude; // Compute integral off altitude error @@ -190,6 +194,8 @@ static void altitudeHoldTask(void *parameters) BaroAltitudeConnectQueue(queue); // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&throttleIntegral); + BaroAltitudeAltitudeGet(&lastAltitude); + error = 0; running = true; } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index b2e6af9c8..8b63e8b5d 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -5,6 +5,7 @@ +