mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
OP-1019 add a simple altitude hold
This commit is contained in:
Alessio Morale
parent
8a6ef3c6d6
commit
5318c20de6
@ -139,11 +139,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
BaroAltitudeConnectQueue(queue);
|
||||
FlightStatusConnectQueue(queue);
|
||||
AccelsConnectQueue(queue);
|
||||
|
||||
bool altitudeHoldFlightMode = false;
|
||||
BaroAltitudeAltitudeGet(&smoothed_altitude);
|
||||
running = false;
|
||||
enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
|
||||
|
||||
uint8_t flightMode;
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
// initialize enable flag
|
||||
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
|
||||
// Main task loop
|
||||
bool baro_updated = false;
|
||||
while (1) {
|
||||
@ -161,10 +165,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
|
||||
} else if (ev.obj == FlightStatusHandle()) {
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) {
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
|
||||
if (altitudeHoldFlightMode && !running) {
|
||||
// Copy the current throttle as a starting point for integral
|
||||
StabilizationDesiredThrottleGet(&throttleIntegral);
|
||||
switchThrottle = throttleIntegral;
|
||||
@ -175,7 +178,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
AltHoldSmoothedData altHold;
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
starting_altitude = altHold.Altitude;
|
||||
} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
|
||||
} else if (!altitudeHoldFlightMode) {
|
||||
running = false;
|
||||
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
@ -348,11 +351,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
|
||||
// Verify that we are in altitude hold mode
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ||
|
||||
flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
|
||||
// Verify that we are in altitude hold mode
|
||||
uint8_t armed;
|
||||
FlightStatusArmedGet(&armed);
|
||||
if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
|
||||
running = false;
|
||||
}
|
||||
|
||||
|
||||
@ -460,6 +460,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
|
||||
case FLIGHTMODE_GUIDANCE:
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
|
||||
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
@ -820,17 +821,24 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
|
||||
uint8_t throttleRate;
|
||||
uint8_t throttleExp;
|
||||
|
||||
static uint8_t flightMode;
|
||||
static portTickType lastSysTimeAH;
|
||||
static bool zeroed = false;
|
||||
portTickType thisSysTime;
|
||||
float dT;
|
||||
|
||||
AltitudeHoldDesiredData altitudeHoldDesiredData;
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
|
||||
AltitudeHoldDesiredData altitudeHoldDesiredData;
|
||||
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
|
||||
|
||||
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
|
||||
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
|
||||
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
|
||||
throttleExp = 128;
|
||||
throttleRate = 0;
|
||||
} else {
|
||||
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
|
||||
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
|
||||
}
|
||||
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
@ -851,12 +859,13 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
|
||||
zeroed = false;
|
||||
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
|
||||
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
|
||||
// then apply an "exp" f(x,k) = (k∙x∙x∙x + x∙(256−k)) / 256
|
||||
// then apply an "exp" f(x,k) = (k*x*x*x + x*(256−k)) / 256
|
||||
altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
|
||||
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
|
||||
altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
|
||||
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
|
||||
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
|
||||
// Require the stick to enter the dead band before they can move height
|
||||
// Vario is not "engaged" when throttleRate == 0
|
||||
zeroed = true;
|
||||
}
|
||||
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
|
||||
|
||||
<!-- Note these enumerated values should be the same as ManualControlSettings -->
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
|
||||
@ -43,31 +43,31 @@
|
||||
units=""
|
||||
type="enum"
|
||||
elements="6"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
|
||||
defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold,Manual"
|
||||
limits="\
|
||||
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
|
||||
|
||||
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user