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:
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;
|
|
||||||
}
|
}
|
||||||
|
Loading…
x
Reference in New Issue
Block a user