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

Now the desired altitude is relative to when the switch was flipped so that the

Browse Source 
baro altitude module can use a smoothed altitude estimate for the starting
point.
This commit is contained in:
James Cotton 2012-02-08 23:02:29 -06:00
parent 80d602ef7b
commit 3026527801
2 changed files with 39 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,7 @@
*/ */
#include "openpilot.h" #include "openpilot.h"
#include <math.h>
#include "altitudeholdsettings.h" #include "altitudeholdsettings.h"
#include "altitudeholddesired.h" // object that will be updated by the module #include "altitudeholddesired.h" // object that will be updated by the module
#include "baroaltitude.h" #include "baroaltitude.h"
@ -52,9 +53,9 @@
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
// Private constants // Private constants
#define MAX_QUEUE_SIZE 1 #define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY+2) #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
// Private types // Private types
// Private variables // Private variables
@ -93,6 +94,7 @@ int32_t AltitudeHoldInitialize()
// Listen for updates. // Listen for updates.
AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldDesiredConnectQueue(queue);
BaroAltitudeConnectQueue(queue);
FlightStatusConnectQueue(queue); FlightStatusConnectQueue(queue);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
@ -110,7 +112,7 @@ bool running = false;
float error; float error;
float switchThrottle; float switchThrottle;
float smoothed_altitude; float smoothed_altitude;
float starting_altitude;
/** /**
* Module thread, should not return. * Module thread, should not return.
*/ */
@ -120,12 +122,13 @@ static void altitudeHoldTask(void *parameters)
BaroAltitudeData baroAltitude; BaroAltitudeData baroAltitude;
StabilizationDesiredData stabilizationDesired; StabilizationDesiredData stabilizationDesired;
portTickType thisTime, lastSysTime; portTickType thisTime, lastSysTime, lastUpdateTime;
UAVObjEvent ev; UAVObjEvent ev;
// Force update of the settings // Force update of the settings
SettingsUpdatedCb(&ev); SettingsUpdatedCb(&ev);
BaroAltitudeAltitudeGet(&smoothed_altitude);
running = false; running = false;
// Main task loop // Main task loop
@ -145,29 +148,44 @@ static void altitudeHoldTask(void *parameters)
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
float dT; float dT;
// Verify that we are still in altitude hold mode // Update dT
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
UAVObjDisconnectQueue(BaroAltitudeHandle(), queue);
running = false;
}
thisTime = xTaskGetTickCount(); thisTime = xTaskGetTickCount();
dT = ((portTickType)(thisTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f; dT = ((portTickType)(thisTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisTime; lastSysTime = thisTime;
// Flipping sign on error since altitude is "down" // Initialize when it was NAN
if((smoothed_altitude == smoothed_altitude) == false)
smoothed_altitude = baroAltitude.Altitude;
// Verify that we are still in altitude hold mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
running = false;
}
// Smooth the altitude
decay = expf(-dT / altitudeHoldSettings.Tau); decay = expf(-dT / altitudeHoldSettings.Tau);
error = error * decay + (altitudeHoldDesired.Altitude - baroAltitude.Altitude) * (1.0f - decay); smoothed_altitude = smoothed_altitude * decay + baroAltitude.Altitude * (1.0f - decay);
if (!running)
continue;
// Compute the altitude error
error = (starting_altitude + altitudeHoldDesired.Altitude) - smoothed_altitude;
// Estimate velocity by smoothing derivative // Estimate velocity by smoothing derivative
velocity_decay = expf(-dT / altitudeHoldSettings.DerivativeTau); velocity_decay = expf(-dT / altitudeHoldSettings.DerivativeTau);
velocity = velocity * velocity_decay + (baroAltitude.Altitude - lastAltitude) / dT * (1.0f-velocity_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
throttleIntegral += error * altitudeHoldSettings.Ki * dT; throttleIntegral += ((starting_altitude + altitudeHoldDesired.Altitude) - baroAltitude.Altitude) * altitudeHoldSettings.Ki * dT;
// Only update stabilizationDesired less frequently
if((thisTime - lastUpdateTime) < 20)
continue;
lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback // Instead of explicit limit on integral you output limit feedback
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
@ -199,14 +217,15 @@ static void altitudeHoldTask(void *parameters)
error = 0; error = 0;
velocity = 0; velocity = 0;
running = true; running = true;
starting_altitude = smoothed_altitude;
BaroAltitudeAltitudeGet(&lastAltitude); BaroAltitudeAltitudeGet(&lastAltitude);
BaroAltitudeConnectQueue(queue); } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
} running = false;
} else if (ev.obj == AltitudeHoldDesiredHandle()) { } else if (ev.obj == AltitudeHoldDesiredHandle()) {
AltitudeHoldDesiredGet(&altitudeHoldDesired); AltitudeHoldDesiredGet(&altitudeHoldDesired);
} }
} }
} }

4
flight/Modules/ManualControl/manualcontrol.c
View File

@ -627,9 +627,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd)
PositionActualDownGet(&currentDown); PositionActualDownGet(&currentDown);
if(dT > 1) { if(dT > 1) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
BaroAltitudeData baroAltitude; altitudeHoldDesired.Altitude = 0;
BaroAltitudeGet(&baroAltitude);
altitudeHoldDesired.Altitude = baroAltitude.Altitude;
zeroed = false; zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) } else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT; altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;