rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Separately smooth the error estimate from the derivative

Browse Source
This commit is contained in:
James Cotton 2012-02-08 10:56:39 -06:00
parent 60c951843d
commit ab41bae071
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -105,6 +105,7 @@ float tau;
float velocity, lastAltitude; float velocity, lastAltitude;
float throttleIntegral; float throttleIntegral;
float decay; float decay;
float velocity_decay;
bool running = false; bool running = false;
float error; float error;
@ -123,6 +124,8 @@ static void altitudeHoldTask(void *parameters)
// Force update of the settings // Force update of the settings
SettingsUpdatedCb(&ev); SettingsUpdatedCb(&ev);
running = false;
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) { while (1) {
@ -153,11 +156,12 @@ 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"
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 // Estimate velocity by smoothing derivative
decay = expf(-dT / altitudeHoldSettings.Tau); velocity_decay = expf(-dT / altitudeHoldSettings.DerivativeTau);
velocity = velocity * decay + (baroAltitude.Altitude - lastAltitude) / dT * (1.0f-decay); // m/s velocity = velocity * velocity_decay + (baroAltitude.Altitude - lastAltitude) / dT * (1.0f-velocity_decay); // m/s
lastAltitude = baroAltitude.Altitude; lastAltitude = baroAltitude.Altitude;
// Compute integral off altitude error // Compute integral off altitude error
@ -190,6 +194,8 @@ 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);
BaroAltitudeAltitudeGet(&lastAltitude);
error = 0;
running = true; running = true;
} }

1
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -5,6 +5,7 @@
<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0.025"/> <field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0.025"/>
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.25"/> <field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="Tau" units="s" type="float" elements="1" defaultvalue="0.1"/> <field name="Tau" units="s" type="float" elements="1" defaultvalue="0.1"/>
<field name="DerivativeTau" units="s" type="float" elements="1" defaultvalue="0.1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>