Merged in corvusvcorax/librepilot/LP-295_OP-1900_fixedwingautotakeoff_rebasenext (pull request #234)

LP-295 fixedwingautotakeoff

flight/libraries/paths.c

@@ -2,11 +2,14 @@
  ******************************************************************************
  *
  * @file       paths.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ *
  * @brief      Library path manipulation
  *
  * @see        The GNU Public License (GPL) Version 3
  *
+ * @addtogroup LibrePilotLibraries LibrePilot Libraries Navigation
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -61,6 +64,7 @@ void path_progress(PathDesiredData *path, float *cur_point, struct path_status *
 
         break;
     case PATHDESIRED_MODE_GOTOENDPOINT:
+    case PATHDESIRED_MODE_AUTOTAKEOFF: // needed for pos hold at end of takeoff
         return path_endpoint(path, cur_point, status, mode3D);
 
         break;

flight/libraries/plans.c

@@ -1,16 +1,15 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotLibraries OpenPilot Libraries
- * @{
- * @addtogroup Navigation
- * @brief setups RTH/PH and other pathfollower/pathplanner status
- * @{
  *
  * @file       plan.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ *
+ * @brief setups RTH/PH and other pathfollower/pathplanner status
  *
  * @see        The GNU Public License (GPL) Version 3
  *
+ * @addtogroup LibrePilotLibraries LibrePilot Libraries Navigation
  ******************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -43,6 +42,7 @@
 #include <stabilizationbank.h>
 #include <stabilizationdesired.h>
 #include <sin_lookup.h>
+#include <sanitycheck.h>
 #include <statusvtolautotakeoff.h>
 
 #define UPDATE_EXPECTED 0.02f
@@ -170,158 +170,35 @@ void plan_setup_returnToBase()
     PathDesiredSet(&pathDesired);
 }
 
-
+void plan_setup_AutoTakeoff()
-// Vtol AutoTakeoff invocation from flight mode requires the following sequence:
-// 1. Arming must be done whilst in the AutoTakeOff flight mode
-// 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
-// 3. Wait for armed state
-// 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
-// 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
-// 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
-
-static StatusVtolAutoTakeoffControlStateOptions autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
-
-#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
-#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
-static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
 {
+    PathDesiredData pathDesired;
+
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
     PositionStateData positionState;
 
     PositionStateGet(&positionState);
-    float velocity_down;
     float autotakeoff_height;
 
-    FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
     FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
-    autotakeoff_height = fabsf(autotakeoff_height);
+    autotakeoff_height      = fabsf(autotakeoff_height);
-    if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
-        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
-    } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
-        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
-    }
 
+    pathDesired.Start.North = positionState.North;
+    pathDesired.Start.East  = positionState.East;
+    pathDesired.Start.Down  = positionState.Down;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = 0.0f;
 
-    pathDesired->Start.North = positionState.North;
+    pathDesired.End.North = positionState.North;
-    pathDesired->Start.East  = positionState.East;
+    pathDesired.End.East  = positionState.East;
-    pathDesired->Start.Down  = positionState.Down;
+    pathDesired.End.Down  = positionState.Down - autotakeoff_height;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = -velocity_down;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)autotakeoffState;
 
-    pathDesired->End.North = positionState.North;
+    pathDesired.StartingVelocity = 0.0f;
-    pathDesired->End.East  = positionState.East;
+    pathDesired.EndingVelocity   = 0.0f;
-    pathDesired->End.Down  = positionState.Down - autotakeoff_height;
+    pathDesired.Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
-
+    PathDesiredSet(&pathDesired);
-    pathDesired->StartingVelocity = 0.0f;
-    pathDesired->EndingVelocity   = 0.0f;
-    pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
-}
-
-#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT 0.2f
-void plan_setup_AutoTakeoff()
-{
-    autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
-    // We only allow takeoff if the state transition of disarmed to armed occurs
-    // whilst in the autotake flight mode
-    FlightStatusData flightStatus;
-    FlightStatusGet(&flightStatus);
-    StabilizationDesiredData stabiDesired;
-    StabilizationDesiredGet(&stabiDesired);
-
-    // Are we inflight?
-    if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
-        // ok assume already in flight and just enter position hold
-        // if we are not actually inflight this will just be a violent autotakeoff
-        autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
-        plan_setup_positionHold();
-    } else {
-        if (flightStatus.Armed) {
-            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
-            // Note that if this mode was invoked unintentionally whilst in flight, effectively
-            // all inputs get ignored and the vtol continues to fly to its previous
-            // stabi command.
-        }
-        PathDesiredData pathDesired;
-        // re-initialise in setup stage
-        memset(&pathDesired, 0, sizeof(PathDesiredData));
-        plan_setup_AutoTakeoff_helper(&pathDesired);
-        PathDesiredSet(&pathDesired);
-    }
-}
-
-#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
-#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT                  0.1f
-void plan_run_AutoTakeoff()
-{
-    StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState;
-
-    switch (autotakeoffState) {
-    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
-    {
-        FlightStatusData flightStatus;
-        FlightStatusGet(&flightStatus);
-        if (!flightStatus.Armed) {
-            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
-        }
-    }
-    break;
-    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
-    {
-        FlightStatusData flightStatus;
-        FlightStatusGet(&flightStatus);
-        if (flightStatus.Armed) {
-            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
-        }
-    }
-    break;
-    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
-    {
-        ManualControlCommandData cmd;
-        ManualControlCommandGet(&cmd);
-
-        if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
-            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
-        }
-    }
-    break;
-    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
-    {
-        ManualControlCommandData cmd;
-        ManualControlCommandGet(&cmd);
-
-        if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
-            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
-            plan_setup_land();
-        }
-    }
-    break;
-
-    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
-    {
-        FlightStatusData flightStatus;
-        FlightStatusGet(&flightStatus);
-        if (!flightStatus.Armed) {
-            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
-        }
-    }
-    break;
-    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
-    // nothing to do. land has been requested. stay here for forever until mode change.
-    default:
-        break;
-    }
-
-    if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT &&
-        autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
-        if (priorState != autotakeoffState) {
-            PathDesiredData pathDesired;
-            // re-initialise in setup stage
-            memset(&pathDesired, 0, sizeof(PathDesiredData));
-            plan_setup_AutoTakeoff_helper(&pathDesired);
-            PathDesiredSet(&pathDesired);
-        }
-    }
 }
 
 static void plan_setup_land_helper(PathDesiredData *pathDesired)

flight/modules/ManualControl/pathfollowerhandler.c

@@ -1,16 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup ManualControl
- * @brief Interpretes the control input in ManualControlCommand
- * @{
- *
  * @file       pathfollowerhandler.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ *
+ * @brief Interpretes the control input in ManualControlCommand
  *
  * @see        The GNU Public License (GPL) Version 3
  *
+ * @addtogroup LibrePilotModules LibrePilot Modules ManualControl
  ******************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -140,9 +138,6 @@ void pathFollowerHandler(bool newinit)
             plan_run_VelocityRoam();
         }
         break;
-    case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
-        plan_run_AutoTakeoff();
-        break;
     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
         plan_run_AutoCruise();
         break;

flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp

@@ -0,0 +1,280 @@
+/*
+ ******************************************************************************
+ *
+ * @file       FixedWingAutoTakeoffController.cpp
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Fixed wing fly controller implementation
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <pid.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <fixedwingpathfollowersettings.h>
+#include <flightstatus.h>
+#include <pathstatus.h>
+#include <stabilizationdesired.h>
+#include <velocitystate.h>
+#include <positionstate.h>
+#include <attitudestate.h>
+}
+
+// C++ includes
+#include "fixedwingautotakeoffcontroller.h"
+
+// Private constants
+
+// pointer to a singleton instance
+FixedWingAutoTakeoffController *FixedWingAutoTakeoffController::p_inst = 0;
+
+
+// Called when mode first engaged
+void FixedWingAutoTakeoffController::Activate(void)
+{
+    if (!mActive) {
+        setState(FW_AUTOTAKEOFF_STATE_LAUNCH);
+    }
+    FixedWingFlyController::Activate();
+}
+
+/**
+ * fixed wing autopilot
+ * use fixed attitude heading towards destination waypoint
+ */
+void FixedWingAutoTakeoffController::UpdateAutoPilot()
+{
+    if (state < FW_AUTOTAKEOFF_STATE_SIZE) {
+        (this->*runFunctionTable[state])();
+    } else {
+        setState(FW_AUTOTAKEOFF_STATE_LAUNCH);
+    }
+}
+
+/**
+ * getAirspeed helper function
+ */
+float FixedWingAutoTakeoffController::getAirspeed(void)
+{
+    VelocityStateData v;
+    float yaw;
+
+    VelocityStateGet(&v);
+    AttitudeStateYawGet(&yaw);
+
+    // current ground speed projected in forward direction
+    float groundspeedProjection = v.North * cos_lookup_deg(yaw) + v.East * sin_lookup_deg(yaw);
+
+    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
+    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
+    // than airspeed and gps sensors alone
+    return groundspeedProjection + indicatedAirspeedStateBias;
+}
+
+
+/**
+ * setState - state transition including initialization
+ */
+void FixedWingAutoTakeoffController::setState(FixedWingAutoTakeoffControllerState_T setstate)
+{
+    if (state < FW_AUTOTAKEOFF_STATE_SIZE && setstate != state) {
+        state = setstate;
+        (this->*initFunctionTable[state])();
+    }
+}
+
+/**
+ * setAttitude - output function to steer plane
+ */
+void FixedWingAutoTakeoffController::setAttitude(bool unsafe)
+{
+    StabilizationDesiredData stabDesired;
+
+    stabDesired.Roll = 0.0f;
+    stabDesired.Yaw  = initYaw;
+    if (unsafe) {
+        stabDesired.Pitch  = fixedWingSettings->LandingPitch;
+        stabDesired.Thrust = 0.0f;
+    } else {
+        stabDesired.Pitch  = fixedWingSettings->TakeOffPitch;
+        stabDesired.Thrust = fixedWingSettings->ThrustLimit.Max;
+    }
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+
+    StabilizationDesiredSet(&stabDesired);
+    if (unsafe) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+    } else {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    }
+
+    // calculate fractional progress based on altitude
+    float downPos;
+    PositionStateDownGet(&downPos);
+    if (fabsf(pathDesired->End.Down - pathDesired->Start.Down) < 1e-3f) {
+        pathStatus->fractional_progress = 1.0f;
+        pathStatus->error = 0.0f;
+    } else {
+        pathStatus->fractional_progress = (downPos - pathDesired->Start.Down) / (pathDesired->End.Down - pathDesired->Start.Down);
+    }
+    pathStatus->error = fabsf(downPos - pathDesired->End.Down);
+
+    PathStatusSet(pathStatus);
+}
+
+/**
+ * check if situation is unsafe
+ */
+bool FixedWingAutoTakeoffController::isUnsafe(void)
+{
+    bool abort  = false;
+    float speed = getAirspeed();
+
+    if (speed > maxVelocity) {
+        maxVelocity = speed;
+    }
+    // too much total deceleration (crash, insufficient climbing power, ...)
+    if (speed < maxVelocity - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
+        abort = true;
+    }
+    AttitudeStateData attitude;
+    AttitudeStateGet(&attitude);
+    // too much bank angle
+    if (fabsf(attitude.Roll) > fixedWingSettings->SafetyCutoffLimits.RollDeg) {
+        abort = true;
+    }
+    if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->SafetyCutoffLimits.PitchDeg) {
+        abort = true;
+    }
+    float deltayaw = attitude.Yaw - initYaw;
+    if (deltayaw > 180.0f) {
+        deltayaw -= 360.0f;
+    }
+    if (deltayaw < -180.0f) {
+        deltayaw += 360.0f;
+    }
+    if (fabsf(deltayaw) > fixedWingSettings->SafetyCutoffLimits.YawDeg) {
+        abort = true;
+    }
+    return abort;
+}
+
+
+// init inactive does nothing
+void FixedWingAutoTakeoffController::init_inactive(void) {}
+
+// init launch resets private variables to start values
+void FixedWingAutoTakeoffController::init_launch(void)
+{
+    // find out vector direction of *runway* (if any)
+    // and align, otherwise just stay straight ahead
+    pathStatus->path_direction_north = 0.0f;
+    pathStatus->path_direction_east  = 0.0f;
+    pathStatus->path_direction_down  = 0.0f;
+    pathStatus->correction_direction_north = 0.0f;
+    pathStatus->correction_direction_east  = 0.0f;
+    pathStatus->correction_direction_down  = 0.0f;
+    if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
+        fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
+        AttitudeStateYawGet(&initYaw);
+    } else {
+        initYaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
+        if (initYaw < -180.0f) {
+            initYaw += 360.0f;
+        }
+        if (initYaw > 180.0f) {
+            initYaw -= 360.0f;
+        }
+    }
+
+    maxVelocity = getAirspeed();
+}
+
+// init climb does nothing
+void FixedWingAutoTakeoffController::init_climb(void) {}
+
+// init hold does nothing
+void FixedWingAutoTakeoffController::init_hold(void) {}
+
+// init abort does nothing
+void FixedWingAutoTakeoffController::init_abort(void) {}
+
+
+// run inactive does nothing
+// no state transitions
+void FixedWingAutoTakeoffController::run_inactive(void) {}
+
+// run launch tries to takeoff - indicates safe situation with engine power (for hand launch)
+// run launch checks for:
+// 1. min velocity for climb
+void FixedWingAutoTakeoffController::run_launch(void)
+{
+    // state transition
+    if (maxVelocity > fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
+        setState(FW_AUTOTAKEOFF_STATE_CLIMB);
+    }
+
+    setAttitude(isUnsafe());
+}
+
+// run climb climbs with max power
+// run climb checks for:
+// 1. min altitude for hold
+// 2. critical situation for abort (different than launch)
+void FixedWingAutoTakeoffController::run_climb(void)
+{
+    bool unsafe = isUnsafe();
+    float downPos;
+
+    PositionStateDownGet(&downPos);
+
+    if (unsafe) {
+        // state transition 2
+        setState(FW_AUTOTAKEOFF_STATE_ABORT);
+    } else if (downPos < pathDesired->End.Down) {
+        // state transition 1
+        setState(FW_AUTOTAKEOFF_STATE_HOLD);
+    }
+
+    setAttitude(unsafe);
+}
+
+// run hold loiters like in position hold
+// no state transitions (FlyController does exception handling)
+void FixedWingAutoTakeoffController::run_hold(void)
+{
+    // parent controller will do perfect position hold in autotakeoff mode
+    FixedWingFlyController::UpdateAutoPilot();
+}
+
+// run abort descends with wings level, engine off (like land)
+// no state transitions
+void FixedWingAutoTakeoffController::run_abort(void)
+{
+    setAttitude(true);
+}

flight/modules/PathFollower/fixedwingflycontroller.cpp

@@ -2,10 +2,13 @@
  ******************************************************************************
  *
  * @file       FixedWingFlyController.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Fixed wing fly controller implementation
  * @see        The GNU Public License (GPL) Version 3
  *
+ * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
+ *
  *****************************************************************************/
 /*
  * This program is free software; you can redistribute it and/or modify
@@ -26,23 +29,13 @@
 extern "C" {
 #include <openpilot.h>
 
-#include <callbackinfo.h>
-
-#include <math.h>
 #include <pid.h>
-#include <CoordinateConversions.h>
 #include <sin_lookup.h>
 #include <pathdesired.h>
 #include <paths.h>
-#include "plans.h"
-#include <sanitycheck.h>
-
-#include <homelocation.h>
-#include <accelstate.h>
 #include <fixedwingpathfollowersettings.h>
 #include <fixedwingpathfollowerstatus.h>
 #include <flightstatus.h>
-#include <flightmodesettings.h>
 #include <pathstatus.h>
 #include <positionstate.h>
 #include <velocitystate.h>
@@ -50,14 +43,7 @@ extern "C" {
 #include <stabilizationdesired.h>
 #include <airspeedstate.h>
 #include <attitudestate.h>
-#include <takeofflocation.h>
-#include <poilocation.h>
-#include <manualcontrolcommand.h>
 #include <systemsettings.h>
-#include <stabilizationbank.h>
-#include <stabilizationdesired.h>
-#include <vtolselftuningstats.h>
-#include <pathsummary.h>
 }
 
 // C++ includes
@@ -80,6 +66,7 @@ void FixedWingFlyController::Activate(void)
         SettingsUpdated();
         resetGlobals();
         mMode   = pathDesired->Mode;
+        lastAirspeedUpdate = 0;
     }
 }
 
@@ -244,9 +231,11 @@ void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
  */
 uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
 {
-    uint8_t result = 1;
+    uint8_t result   = 1;
+    bool cutThrust   = false;
+    bool hasAirspeed = true;
 
-    const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
+    const float dT   = fixedWingSettings->UpdatePeriod / 1000.0f;
 
     VelocityDesiredData velocityDesired;
     VelocityStateData velocityState;
@@ -259,7 +248,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     float groundspeedProjection;
     float indicatedAirspeedState;
     float indicatedAirspeedDesired;
-    float airspeedError;
+    float airspeedError = 0.0f;
 
     float pitchCommand;
 
@@ -282,56 +271,99 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     AirspeedStateGet(&airspeedState);
     SystemSettingsGet(&systemSettings);
 
+
     /**
      * Compute speed error and course
      */
-    // missing sensors for airspeed-direction we have to assume within
-    // reasonable error that measured airspeed is actually the airspeed
-    // component in forward pointing direction
-    // airspeedVector is normalized
-    airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
-    airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
 
-    // current ground speed projected in forward direction
+    // check for airspeed sensor
-    groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
+    fixedWingPathFollowerStatus.Errors.AirspeedSensor = 0;
-
+    if (fixedWingSettings->UseAirspeedSensor == FIXEDWINGPATHFOLLOWERSETTINGS_USEAIRSPEEDSENSOR_FALSE) {
-    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
+        // fallback algo triggered voluntarily
-    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
+        hasAirspeed = false;
-    // than airspeed and gps sensors alone
+        fixedWingPathFollowerStatus.Errors.AirspeedSensor = 1;
-    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
+    } else if (PIOS_DELAY_GetuSSince(lastAirspeedUpdate) > 1000000) {
-
+        // no airspeed update in one second, assume airspeed sensor failure
-    // fluidMovement is a vector describing the aproximate movement vector of
+        hasAirspeed = false;
-    // the surrounding fluid in 2d space (aka wind vector)
-    fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
-    fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
-
-    // calculate the movement vector we need to fly to reach velocityDesired -
-    // taking fluidMovement into account
-    courseComponent[0] = velocityDesired.North - fluidMovement[0];
-    courseComponent[1] = velocityDesired.East - fluidMovement[1];
-
-    indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
-                                      fixedWingSettings->HorizontalVelMin,
-                                      fixedWingSettings->HorizontalVelMax);
-
-    // if we could fly at arbitrary speeds, we'd just have to move towards the
-    // courseComponent vector as previously calculated and we'd be fine
-    // unfortunately however we are bound by min and max air speed limits, so
-    // we need to recalculate the correct course to meet at least the
-    // velocityDesired vector direction at our current speed
-    // this overwrites courseComponent
-    bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
-
-    // Error condition: wind speed too high, we can't go where we want anymore
-    fixedWingPathFollowerStatus.Errors.Wind = 0;
-    if ((!valid) &&
-        fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
-        fixedWingPathFollowerStatus.Errors.Wind = 1;
         result = 0;
+        fixedWingPathFollowerStatus.Errors.AirspeedSensor = 1;
     }
 
-    // Airspeed error
+
-    airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
+    if (hasAirspeed) {
+        // missing sensors for airspeed-direction we have to assume within
+        // reasonable error that measured airspeed is actually the airspeed
+        // component in forward pointing direction
+        // airspeedVector is normalized
+        airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
+        airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
+
+        // current ground speed projected in forward direction
+        groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
+
+        // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
+        // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
+        // than airspeed and gps sensors alone
+        indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
+
+        // fluidMovement is a vector describing the aproximate movement vector of
+        // the surrounding fluid in 2d space (aka wind vector)
+        fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
+        fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
+
+        // calculate the movement vector we need to fly to reach velocityDesired -
+        // taking fluidMovement into account
+        courseComponent[0] = velocityDesired.North - fluidMovement[0];
+        courseComponent[1] = velocityDesired.East - fluidMovement[1];
+
+        indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
+                                          fixedWingSettings->HorizontalVelMin,
+                                          fixedWingSettings->HorizontalVelMax);
+
+        // if we could fly at arbitrary speeds, we'd just have to move towards the
+        // courseComponent vector as previously calculated and we'd be fine
+        // unfortunately however we are bound by min and max air speed limits, so
+        // we need to recalculate the correct course to meet at least the
+        // velocityDesired vector direction at our current speed
+        // this overwrites courseComponent
+        bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
+
+        // Error condition: wind speed too high, we can't go where we want anymore
+        fixedWingPathFollowerStatus.Errors.Wind = 0;
+        if ((!valid) &&
+            fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
+            fixedWingPathFollowerStatus.Errors.Wind = 1;
+            result = 0;
+        }
+
+        // Airspeed error
+        airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
+
+        // Error condition: plane too slow or too fast
+        fixedWingPathFollowerStatus.Errors.Highspeed = 0;
+        fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
+        if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
+            fixedWingPathFollowerStatus.Errors.Overspeed = 1;
+            result = 0;
+        }
+        if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
+            fixedWingPathFollowerStatus.Errors.Highspeed = 1;
+            result = 0;
+            cutThrust = true;
+        }
+        if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
+            fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
+            result = 0;
+        }
+        if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
+            fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
+            result = 0;
+        }
+        if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
+            cutThrust = true;
+            result    = 0;
+        }
+    }
 
     // Vertical speed error
     descentspeedDesired = boundf(
@@ -340,36 +372,19 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
         fixedWingSettings->VerticalVelMax);
     descentspeedError = descentspeedDesired - velocityState.Down;
 
-    // Error condition: plane too slow or too fast
-    fixedWingPathFollowerStatus.Errors.Highspeed = 0;
-    fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
-    if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
-        fixedWingPathFollowerStatus.Errors.Overspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
-        fixedWingPathFollowerStatus.Errors.Highspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
-        fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
-        fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
-        result = 0;
-    }
-
     /**
      * Compute desired thrust command
      */
 
     // Compute the cross feed from vertical speed to pitch, with saturation
-    float speedErrorToPowerCommandComponent = boundf(
+    float speedErrorToPowerCommandComponent = 0.0f;
-        (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
+    if (hasAirspeed) {
-        -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
+        speedErrorToPowerCommandComponent = boundf(
-        fixedWingSettings->AirspeedToPowerCrossFeed.Max
+            (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-        );
+            -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
+            fixedWingSettings->AirspeedToPowerCrossFeed.Max
+            );
+    }
 
     // Compute final thrust response
     powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
@@ -390,57 +405,84 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
         velocityState.Down > 0.0f && // we ARE going down
         descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError > 0.0f && // we are too slow already
+        airspeedError > 0.0f) { // we are too slow already
-        fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
         fixedWingPathFollowerStatus.Errors.Lowpower = 1;
-        result = 0;
+
+        if (fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
+            result = 0;
+        }
     }
     // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
     fixedWingPathFollowerStatus.Errors.Highpower = 0;
     if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
         velocityState.Down < 0.0f && // we ARE going up
         descentspeedDesired > 0.0f && // we WANT to go down
-        airspeedError < 0.0f && // we are too fast already
+        airspeedError < 0.0f) { // we are too fast already
-        fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
+        // this alarm is often switched off because of false positives, however we still want to cut throttle if it happens
+        cutThrust = true;
         fixedWingPathFollowerStatus.Errors.Highpower = 1;
-        result = 0;
+
+        if (fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
+            result = 0;
+        }
     }
 
     /**
      * Compute desired pitch command
      */
-    // Compute the cross feed from vertical speed to pitch, with saturation
+    if (hasAirspeed) {
-    float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
+        // Compute the cross feed from vertical speed to pitch, with saturation
-                                                        -fixedWingSettings->VerticalToPitchCrossFeed.Max,
+        float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-                                                        fixedWingSettings->VerticalToPitchCrossFeed.Max
+                                                            -fixedWingSettings->VerticalToPitchCrossFeed.Max,
-                                                        );
+                                                            fixedWingSettings->VerticalToPitchCrossFeed.Max
+                                                            );
 
-    // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
+        // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
-    pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
+        pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
 
-    fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
+        fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
-    fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
+        fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
+        fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
 
-    stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
+        stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
-                               fixedWingSettings->PitchLimit.Min,
+                                   fixedWingSettings->PitchLimit.Min,
-                               fixedWingSettings->PitchLimit.Max);
+                                   fixedWingSettings->PitchLimit.Max);
 
-    // Error condition: high speed dive
+        // Error condition: high speed dive
-    fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
+        fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
-    if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
+        if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
-        velocityState.Down > 0.0f && // we ARE going down
+            velocityState.Down > 0.0f && // we ARE going down
-        descentspeedDesired < 0.0f && // we WANT to go up
+            descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError < 0.0f && // we are too fast already
+            airspeedError < 0.0f && // we are too fast already
-        fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
+            fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
+            fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
+            result = 0;
+            cutThrust = true;
+        }
+    } else {
+        // no airspeed sensor means we fly with constant pitch, like for landing pathfollower
+        stabDesired.Pitch = fixedWingSettings->LandingPitch;
+    }
+
+    // Error condition: pitch way out of wack
+    if (fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f &&
+        (attitudeState.Pitch < fixedWingSettings->PitchLimit.Min - fixedWingSettings->SafetyCutoffLimits.PitchDeg ||
+         attitudeState.Pitch > fixedWingSettings->PitchLimit.Max + fixedWingSettings->SafetyCutoffLimits.PitchDeg)) {
         fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
         result = 0;
+        cutThrust = true;
     }
 
+
     /**
      * Compute desired roll command
      */
-    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
+    if (hasAirspeed) {
+        courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
+    } else {
+        // fallback based on effective movement direction when in fallback mode, hope that airspeed > wind velocity, or we will never get home
+        courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North)) - RAD2DEG(atan2f(velocityState.East, velocityState.North));
+    }
 
     if (courseError < -180.0f) {
         courseError += 360.0f;
@@ -473,7 +515,15 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
                               fixedWingSettings->RollLimit.Min,
                               fixedWingSettings->RollLimit.Max);
 
-    // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
+    // Error condition: roll way out of wack
+    fixedWingPathFollowerStatus.Errors.Rollcontrol = 0;
+    if (fixedWingSettings->Safetymargins.Rollcontrol > 0.5f &&
+        (attitudeState.Roll < fixedWingSettings->RollLimit.Min - fixedWingSettings->SafetyCutoffLimits.RollDeg ||
+         attitudeState.Roll > fixedWingSettings->RollLimit.Max + fixedWingSettings->SafetyCutoffLimits.RollDeg)) {
+        fixedWingPathFollowerStatus.Errors.Rollcontrol = 1;
+        result = 0;
+        cutThrust = true;
+    }
 
 
     /**
@@ -482,6 +532,10 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     // TODO implement raw control mode for yaw and base on Accels.Y
     stabDesired.Yaw = 0.0f;
 
+    // safety cutoff condition
+    if (cutThrust) {
+        stabDesired.Thrust = 0.0f;
+    }
 
     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
@@ -615,4 +669,6 @@ void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVO
     // changes to groundspeed to offset the airspeed by the same measurement.
     // This has a side effect that in the absence of any airspeed updates, the
     // pathfollower will fly using groundspeed.
+
+    lastAirspeedUpdate = PIOS_DELAY_GetuS();
 }

flight/modules/PathFollower/fixedwinglandcontroller.cpp

@@ -0,0 +1,154 @@
+/*
+ ******************************************************************************
+ *
+ * @file       FixedWingLandController.cpp
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Fixed wing fly controller implementation
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <pathdesired.h>
+#include <fixedwingpathfollowersettings.h>
+#include <flightstatus.h>
+#include <pathstatus.h>
+#include <stabilizationdesired.h>
+}
+
+// C++ includes
+#include "fixedwinglandcontroller.h"
+
+// Private constants
+
+// pointer to a singleton instance
+FixedWingLandController *FixedWingLandController::p_inst = 0;
+
+FixedWingLandController::FixedWingLandController()
+    : fixedWingSettings(NULL), mActive(false), mMode(0)
+{}
+
+// Called when mode first engaged
+void FixedWingLandController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        resetGlobals();
+        mMode   = pathDesired->Mode;
+    }
+}
+
+uint8_t FixedWingLandController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t FixedWingLandController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void FixedWingLandController::ObjectiveUpdated(void)
+{}
+
+void FixedWingLandController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        resetGlobals();
+    }
+}
+
+
+void FixedWingLandController::SettingsUpdated(void)
+{}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingLandController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
+{
+    PIOS_Assert(ptr_fixedWingSettings);
+
+    fixedWingSettings = ptr_fixedWingSettings;
+
+    resetGlobals();
+
+    return 0;
+}
+
+/**
+ * reset globals, (integrals, accumulated errors and timers)
+ */
+void FixedWingLandController::resetGlobals()
+{
+    pathStatus->path_time = 0.0f;
+    pathStatus->path_direction_north = 0.0f;
+    pathStatus->path_direction_east  = 0.0f;
+    pathStatus->path_direction_down  = 0.0f;
+    pathStatus->correction_direction_north = 0.0f;
+    pathStatus->correction_direction_east  = 0.0f;
+    pathStatus->correction_direction_down  = 0.0f;
+    pathStatus->error = 0.0f;
+    pathStatus->fractional_progress = 0.0f;
+}
+
+/**
+ * fixed wing autopilot
+ * use fixed attitude heading towards destination waypoint
+ */
+void FixedWingLandController::UpdateAutoPilot()
+{
+    StabilizationDesiredData stabDesired;
+
+    stabDesired.Roll   = 0.0f;
+    stabDesired.Pitch  = fixedWingSettings->LandingPitch;
+    stabDesired.Thrust = 0.0f;
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+
+    // find out vector direction of *runway* (if any)
+    // and align, otherwise just stay straight ahead
+    if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
+        fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
+        stabDesired.Yaw = 0.0f;
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+    } else {
+        stabDesired.Yaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
+        if (stabDesired.Yaw < -180.0f) {
+            stabDesired.Yaw += 360.0f;
+        }
+        if (stabDesired.Yaw > 180.0f) {
+            stabDesired.Yaw -= 360.0f;
+        }
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    }
+    StabilizationDesiredSet(&stabDesired);
+    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+
+    PathStatusSet(pathStatus);
+}

flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h

@@ -0,0 +1,98 @@
+/**
+ ******************************************************************************
+ * @addtogroup LibrePilotModules LibrePilot Modules
+ * @{
+ * @addtogroup FixedWing CONTROL interface class
+ * @brief CONTROL interface class for pathfollower fixed wing fly controller
+ * @{
+ *
+ * @file       fixedwingautotakeoffcontroller.h
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for fixed wing fly objectives
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef FIXEDWINGAUTOTAKEOFFCONTROLLER_H
+#define FIXEDWINGAUTOTAKEOFFCONTROLLER_H
+#include "fixedwingflycontroller.h"
+
+// AutoTakeoff state machine
+typedef enum {
+    FW_AUTOTAKEOFF_STATE_INACTIVE = 0,
+    FW_AUTOTAKEOFF_STATE_LAUNCH,
+    FW_AUTOTAKEOFF_STATE_CLIMB,
+    FW_AUTOTAKEOFF_STATE_HOLD,
+    FW_AUTOTAKEOFF_STATE_ABORT,
+    FW_AUTOTAKEOFF_STATE_SIZE
+} FixedWingAutoTakeoffControllerState_T;
+
+class FixedWingAutoTakeoffController : public FixedWingFlyController {
+protected:
+    static FixedWingAutoTakeoffController *p_inst;
+
+public:
+    static FixedWingFlyController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new FixedWingAutoTakeoffController();
+        }
+        return p_inst;
+    }
+    void Activate(void);
+    void UpdateAutoPilot(void);
+
+private:
+    // variables
+    FixedWingAutoTakeoffControllerState_T state;
+    float initYaw;
+    float maxVelocity;
+
+    // functions
+    void setState(FixedWingAutoTakeoffControllerState_T setstate);
+    void setAttitude(bool unsafe);
+    float getAirspeed(void);
+    bool isUnsafe(void);
+    void run_inactive(void);
+    void run_launch(void);
+    void run_climb(void);
+    void run_hold(void);
+    void run_abort(void);
+    void init_inactive(void);
+    void init_launch(void);
+    void init_climb(void);
+    void init_hold(void);
+    void init_abort(void);
+    void(FixedWingAutoTakeoffController::*const runFunctionTable[FW_AUTOTAKEOFF_STATE_SIZE]) (void) = {
+        &FixedWingAutoTakeoffController::run_inactive,
+        &FixedWingAutoTakeoffController::run_launch,
+        &FixedWingAutoTakeoffController::run_climb,
+        &FixedWingAutoTakeoffController::run_hold,
+        &FixedWingAutoTakeoffController::run_abort
+    };
+    void(FixedWingAutoTakeoffController::*const initFunctionTable[FW_AUTOTAKEOFF_STATE_SIZE]) (void) = {
+        &FixedWingAutoTakeoffController::init_inactive,
+        &FixedWingAutoTakeoffController::init_launch,
+        &FixedWingAutoTakeoffController::init_climb,
+        &FixedWingAutoTakeoffController::init_hold,
+        &FixedWingAutoTakeoffController::init_abort
+    };
+};
+
+#endif // FIXEDWINGAUTOTAKEOFFCONTROLLER_H

flight/modules/PathFollower/inc/fixedwingflycontroller.h

@@ -1,13 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
+ * @addtogroup LibrePilotModules LibrePilot Modules
  * @{
  * @addtogroup FixedWing CONTROL interface class
  * @brief CONTROL interface class for pathfollower fixed wing fly controller
  * @{
  *
- * @file       FixedWingCONTROL.h
+ * @file       fixedwingflycontroller.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes CONTROL for fixed wing fly objectives
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -33,7 +34,7 @@
 #include "pathfollowercontrol.h"
 
 class FixedWingFlyController : public PathFollowerControl {
-private:
+protected:
     static FixedWingFlyController *p_inst;
     FixedWingFlyController();
 
@@ -57,24 +58,26 @@ public:
     uint8_t Mode(void);
     void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev);
 
+protected:
+    FixedWingPathFollowerSettingsData *fixedWingSettings;
+
+    uint8_t mActive;
+    uint8_t mMode;
+    // correct speed by measured airspeed
+    float indicatedAirspeedStateBias;
 private:
     void resetGlobals();
     uint8_t updateAutoPilotFixedWing();
     void updatePathVelocity(float kFF, bool limited);
     uint8_t updateFixedDesiredAttitude();
     bool correctCourse(float *C, float *V, float *F, float s);
-
+    int32_t lastAirspeedUpdate;
-    FixedWingPathFollowerSettingsData *fixedWingSettings;
-    uint8_t mActive;
-    uint8_t mMode;
 
     struct pid PIDposH[2];
     struct pid PIDposV;
     struct pid PIDcourse;
     struct pid PIDspeed;
     struct pid PIDpower;
-    // correct speed by measured airspeed
-    float indicatedAirspeedStateBias;
 };
 
 #endif // FIXEDWINGFLYCONTROLLER_H

flight/modules/PathFollower/inc/fixedwinglandcontroller.h

@@ -0,0 +1,67 @@
+/**
+ ******************************************************************************
+ * @addtogroup LibrePilotModules LibrePilot Modules
+ * @{
+ * @addtogroup FixedWing CONTROL interface class
+ * @brief CONTROL interface class for pathfollower fixed wing fly controller
+ * @{
+ *
+ * @file       fixedwinglandcontroller.h
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for fixed wing fly objectives
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef FIXEDWINGLANDCONTROLLER_H
+#define FIXEDWINGLANDCONTROLLER_H
+#include "pathfollowercontrol.h"
+
+class FixedWingLandController : public PathFollowerControl {
+private:
+    static FixedWingLandController *p_inst;
+    FixedWingLandController();
+
+
+public:
+    static FixedWingLandController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new FixedWingLandController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void resetGlobals();
+    FixedWingPathFollowerSettingsData *fixedWingSettings;
+    uint8_t mActive;
+    uint8_t mMode;
+};
+
+#endif // FIXEDWINGLANDCONTROLLER_H

flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h

@@ -1,13 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
+ * @addtogroup LibrePilotModules LibrePilot Modules
  * @{
  * @addtogroup PathFollower CONTROL interface class
  * @brief vtol land controller class
  * @{
  *
  * @file       vtollandcontroller.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes CONTROL for landing sequence
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -70,6 +71,8 @@ private:
     PIDControlDown controlDown;
     PIDControlNE controlNE;
     uint8_t mActive;
+    uint8_t mOverride;
+    StatusVtolAutoTakeoffControlStateOptions autotakeoffState;
 };
 
 #endif // VTOLAUTOTAKEOFFCONTROLLER_H

flight/modules/PathFollower/inc/vtolautotakeofffsm.h

@@ -1,13 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
+ * @addtogroup LibrePilotModules LibrePilot Modules
  * @{
  * @addtogroup PathFollower FSM
  * @brief Executes landing sequence via an FSM
  * @{
  *
  * @file       vtollandfsm.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes FSM for landing sequence
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -47,7 +48,7 @@ typedef enum {
     AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
     AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
     AUTOTAKEOFF_STATE_DISARMED, // Disarmed
-    AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to hold
+    AUTOTAKEOFF_STATE_ABORT, // Abort on error triggers fallback to land
     AUTOTAKEOFF_STATE_SIZE
 } PathFollowerFSM_AutoTakeoffState_T;
 

flight/modules/PathFollower/inc/vtollandcontroller.h

@@ -1,13 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
+ * @addtogroup LibrePilotModules LibrePilot Modules
  * @{
  * @addtogroup PathFollower CONTROL interface class
  * @brief vtol land controller class
  * @{
  *
  * @file       vtollandcontroller.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes CONTROL for landing sequence
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -71,6 +72,7 @@ private:
     PIDControlDown controlDown;
     PIDControlNE controlNE;
     uint8_t mActive;
+    uint8_t mOverride;
 };
 
 #endif // VTOLLANDCONTROLLER_H

flight/modules/PathFollower/pathfollower.cpp

@@ -2,12 +2,14 @@
  ******************************************************************************
  *
  * @file       pathfollower.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
  * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
  * of @ref ManualControlCommand is Auto.
  *
  * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  *
  *****************************************************************************/
 /*
@@ -93,6 +95,8 @@ extern "C" {
 #include "vtolbrakecontroller.h"
 #include "vtolflycontroller.h"
 #include "fixedwingflycontroller.h"
+#include "fixedwingautotakeoffcontroller.h"
+#include "fixedwinglandcontroller.h"
 #include "grounddrivecontroller.h"
 
 // Private constants
@@ -220,6 +224,8 @@ void pathFollowerInitializeControllersForFrameType()
     case FRAME_TYPE_FIXED_WING:
         if (!fixedwing_initialised) {
             FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
+            FixedWingAutoTakeoffController::instance()->Initialize(&fixedWingPathFollowerSettings);
+            FixedWingLandController::instance()->Initialize(&fixedWingPathFollowerSettings);
             fixedwing_initialised = 1;
         }
         break;
@@ -289,6 +295,14 @@ static void pathFollowerSetActiveController(void)
                 activeController = FixedWingFlyController::instance();
                 activeController->Activate();
                 break;
+            case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
+                activeController = FixedWingLandController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_AUTOTAKEOFF:
+                activeController = FixedWingAutoTakeoffController::instance();
+                activeController->Activate();
+                break;
             default:
                 activeController = 0;
                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
@@ -451,6 +465,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
+    FixedWingAutoTakeoffController::instance()->AirspeedStateUpdatedCb(ev);
 }
 
 

flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

@@ -2,9 +2,11 @@
  ******************************************************************************
  *
  * @file       vtollandcontroller.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Vtol landing controller loop
  * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  *
  *****************************************************************************/
 /*
@@ -57,6 +59,7 @@ extern "C" {
 #include <stabilizationbank.h>
 #include <stabilizationdesired.h>
 #include <vtolselftuningstats.h>
+#include <statusvtolautotakeoff.h>
 #include <pathsummary.h>
 }
 
@@ -67,6 +70,11 @@ extern "C" {
 #include "pidcontroldown.h"
 
 // Private constants
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN             2.0f
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX             50.0f
+#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT         0.2f
+#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
+#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT                  0.1f
 
 // pointer to a singleton instance
 VtolAutoTakeoffController *VtolAutoTakeoffController::p_inst = 0;
@@ -79,11 +87,34 @@ VtolAutoTakeoffController::VtolAutoTakeoffController()
 void VtolAutoTakeoffController::Activate(void)
 {
     if (!mActive) {
-        mActive = true;
+        mActive   = true;
+        mOverride = true;
         SettingsUpdated();
         fsm->Activate();
         controlDown.Activate();
         controlNE.Activate();
+        autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+        // We only allow takeoff if the state transition of disarmed to armed occurs
+        // whilst in the autotake flight mode
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        StabilizationDesiredData stabiDesired;
+        StabilizationDesiredGet(&stabiDesired);
+
+        if (flightStatus.Armed) {
+            // Are we inflight?
+            if (stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT || flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+                // ok assume already in flight and just enter position hold
+                // if we are not actually inflight this will just be a violent autotakeoff
+                autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
+            } else {
+                autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
+                // Note that if this mode was invoked unintentionally whilst in flight, effectively
+                // all inputs get ignored and the vtol continues to fly to its previous
+                // stabi command.
+            }
+        }
+        fsm->setControlState(autotakeoffState);
     }
 }
 
@@ -100,15 +131,40 @@ uint8_t VtolAutoTakeoffController::Mode(void)
 // Objective updated in pathdesired, e.g. same flight mode but new target velocity
 void VtolAutoTakeoffController::ObjectiveUpdated(void)
 {
-    // Set the objective's target velocity
+    if (mOverride) {
+        // override pathDesired from PathPlanner with current position,
+        // as we deliberately don't care about the location of the waypoints on the map
+        float velocity_down;
+        float autotakeoff_height;
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
+        FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
+        autotakeoff_height = fabsf(autotakeoff_height);
+        if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
+            autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
+        } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
+            autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
+        }
+        controlDown.UpdateVelocitySetpoint(velocity_down);
+        controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
+        controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
 
-    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
+
-    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
+        controlDown.UpdatePositionSetpoint(positionState.Down - autotakeoff_height);
-                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
+        mOverride = false; // further updates always come from ManualControl and will control horizontal position
-    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+    } else {
-    controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
+        // Set the objective's target velocity
-    fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
+
+        controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
+        controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
+                                         pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
+        controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+        controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
+    }
 }
+
+// Controller deactivated
 void VtolAutoTakeoffController::Deactivate(void)
 {
     if (mActive) {
@@ -224,6 +280,16 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
 
     controlNE.GetNECommand(&northCommand, &eastCommand);
     stabDesired.Thrust = controlDown.GetDownCommand();
+    switch (autotakeoffState) {
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
+        stabDesired.Thrust = 0.0f;
+        break;
+    default:
+        break;
+    }
 
     float angle_radians = DEG2RAD(attitudeState.Yaw);
     float cos_angle     = cosf(angle_radians);
@@ -251,6 +317,78 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
 
 void VtolAutoTakeoffController::UpdateAutoPilot()
 {
+    // state machine updates:
+    // Vtol AutoTakeoff invocation from flight mode requires the following sequence:
+    // 1. Arming must be done whilst in the AutoTakeOff flight mode
+    // 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
+    // 3. Wait for armed state
+    // 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
+    // 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
+    // 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
+
+    switch (autotakeoffState) {
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (!flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+            fsm->setControlState(autotakeoffState);
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
+            fsm->setControlState(autotakeoffState);
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
+    {
+        ManualControlCommandData cmd;
+        ManualControlCommandGet(&cmd);
+
+        if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
+            fsm->setControlState(autotakeoffState);
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
+    {
+        ManualControlCommandData cmd;
+        ManualControlCommandGet(&cmd);
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+
+        // we do not do a takeoff abort in pathplanner mode
+        if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE &&
+            cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
+            fsm->setControlState(autotakeoffState);
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (!flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+            fsm->setControlState(autotakeoffState);
+        }
+    }
+    break;
+    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
+    // nothing to do. land has been requested. stay here for forever until mode change.
+    default:
+        break;
+    }
+
     fsm->Update();
 
     UpdateVelocityDesired();

flight/modules/PathFollower/vtollandcontroller.cpp

@@ -2,9 +2,11 @@
  ******************************************************************************
  *
  * @file       vtollandcontroller.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Vtol landing controller loop
  * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  *
  *****************************************************************************/
 /*
@@ -76,7 +78,8 @@ VtolLandController::VtolLandController()
 void VtolLandController::Activate(void)
 {
     if (!mActive) {
-        mActive = true;
+        mActive   = true;
+        mOverride = true;
         SettingsUpdated();
         fsm->Activate();
         controlDown.Activate();
@@ -97,11 +100,24 @@ uint8_t VtolLandController::Mode(void)
 // Objective updated in pathdesired, e.g. same flight mode but new target velocity
 void VtolLandController::ObjectiveUpdated(void)
 {
-    // Set the objective's target velocity
+    if (mOverride) {
-    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
+        // override pathDesired from PathPLanner with current position,
-    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+        // as we deliberately don' not care about the location of the waypoints on the map
-                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+        float velocity_down;
-    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        FlightModeSettingsLandingVelocityGet(&velocity_down);
+        controlDown.UpdateVelocitySetpoint(velocity_down);
+        controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
+        controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
+        mOverride = false; // further updates always come from ManualControl and will control horizontal position
+    } else {
+        // Set the objective's target velocity
+        controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
+        controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+                                         pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+        controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+    }
 }
 void VtolLandController::Deactivate(void)
 {

flight/modules/PathPlanner/pathplanner.c

@@ -1,13 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
+ * @addtogroup LibrePilotModules LibrePilot Modules
  * @{
  * @addtogroup PathPlanner Path Planner Module
  * @brief Executes a series of waypoints
  * @{
  *
  * @file       pathplanner.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes a series of waypoints
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -48,8 +49,6 @@
 #include "plans.h"
 #include <sanitycheck.h>
 #include <vtolpathfollowersettings.h>
-#include <statusvtolautotakeoff.h>
-#include <statusvtolland.h>
 #include <manualcontrolcommand.h>
 
 // Private constants
@@ -80,9 +79,6 @@ static uint8_t conditionPointingTowardsNext();
 static uint8_t conditionPythonScript();
 static uint8_t conditionImmediate();
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
-static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
-static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
-static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position);
 
 
 // Private variables
@@ -132,8 +128,6 @@ int32_t PathPlannerInitialize()
     VelocityStateInitialize();
     WaypointInitialize();
     WaypointActiveInitialize();
-    StatusVtolAutoTakeoffInitialize();
-    StatusVtolLandInitialize();
 
     pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
     pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
@@ -236,17 +230,17 @@ static void pathPlannerTask()
         return;
     }
 
-
     // the transition from pathplanner to another flightmode back to pathplanner
     // triggers a reset back to 0 index in the waypoint list
     if (pathplanner_active == false) {
-        pathplanner_active   = true;
+        pathplanner_active = true;
 
-        // This triggers callback to update variable
+        FlightModeSettingsFlightModeChangeRestartsPathPlanOptions restart;
-        waypointActive.Index = 0;
+        FlightModeSettingsFlightModeChangeRestartsPathPlanGet(&restart);
-        WaypointActiveSet(&waypointActive);
+        if (restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE) {
-
+            setWaypoint(0);
-        return;
+            return;
+        }
     }
 
     WaypointInstGet(waypointActive.Index, &waypoint);
@@ -265,21 +259,6 @@ static void pathPlannerTask()
         return;
     }
 
-    // check start conditions
-    // autotakeoff requires midpoint thrust if we are in a pending takeoff situation
-    if (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF) {
-        pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
-        if ((uint8_t)pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE) {
-            ManualControlCommandData cmd;
-            ManualControlCommandGet(&cmd);
-            if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
-                pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
-                PathDesiredSet(&pathDesired);
-            }
-            return;
-        }
-    }
-
 
     // check if condition has been met
     endCondition = pathConditionCheck();
@@ -331,22 +310,45 @@ void updatePathDesired()
     WaypointActiveGet(&waypointActive);
     WaypointInstGet(waypointActive.Index, &waypoint);
 
-    // Capture if current mode is takeoff
-    bool autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
-
     PathActionInstGet(waypoint.Action, &pathAction);
 
     PathDesiredData pathDesired;
-    switch (pathAction.Mode) {
+
-    case PATHACTION_MODE_AUTOTAKEOFF:
+    pathDesired.End.North = waypoint.Position.North;
-        planner_setup_pathdesired_takeoff(&pathDesired);
+    pathDesired.End.East  = waypoint.Position.East;
-        break;
+    pathDesired.End.Down  = waypoint.Position.Down;
-    case PATHACTION_MODE_LAND:
+    pathDesired.EndingVelocity    = waypoint.Velocity;
-        planner_setup_pathdesired_land(&pathDesired);
+    pathDesired.Mode = pathAction.Mode;
-        break;
+    pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
-    default:
+    pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
-        planner_setup_pathdesired(&pathDesired, autotakeoff);
+    pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
-        break;
+    pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
+    pathDesired.UID = waypointActive.Index;
+
+
+    if (waypointActive.Index == 0) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
+
+        /*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
+           pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
+           pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
+        // note: if certain flightmodes need to override Start, End or mode parameters, that should happen within
+        // the pathfollower as needed. This also holds if Start is replaced by current position for takeoff and landing
+        pathDesired.Start.North = positionState.North;
+        pathDesired.Start.East  = positionState.East;
+        pathDesired.Start.Down  = positionState.Down;
+        pathDesired.StartingVelocity = pathDesired.EndingVelocity;
+    } else {
+        // Get previous waypoint as start point
+        WaypointData waypointPrev;
+        WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
+
+        pathDesired.Start.North = waypointPrev.Position.North;
+        pathDesired.Start.East  = waypointPrev.Position.East;
+        pathDesired.Start.Down  = waypointPrev.Position.Down;
+        pathDesired.StartingVelocity = waypointPrev.Velocity;
     }
 
     PathDesiredSet(&pathDesired);
@@ -437,112 +439,6 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 }
 
-// Standard setup of a pathDesired command from the waypoint path plan
-static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position)
-{
-    pathDesired->End.North = waypoint.Position.North;
-    pathDesired->End.East  = waypoint.Position.East;
-    pathDesired->End.Down  = waypoint.Position.Down;
-    pathDesired->EndingVelocity    = waypoint.Velocity;
-    pathDesired->Mode = pathAction.Mode;
-    pathDesired->ModeParameters[0] = pathAction.ModeParameters[0];
-    pathDesired->ModeParameters[1] = pathAction.ModeParameters[1];
-    pathDesired->ModeParameters[2] = pathAction.ModeParameters[2];
-    pathDesired->ModeParameters[3] = pathAction.ModeParameters[3];
-    pathDesired->UID = waypointActive.Index;
-
-
-    if (waypointActive.Index == 0 || overwrite_start_position) {
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-        // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
-
-        /*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
-           pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
-           pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
-        // note takeoff relies on the start being the current location as it merely ascends and using
-        // the start as assumption current NE location
-        pathDesired->Start.North = positionState.North;
-        pathDesired->Start.East  = positionState.East;
-        pathDesired->Start.Down  = positionState.Down;
-        pathDesired->StartingVelocity = pathDesired->EndingVelocity;
-    } else {
-        // Get previous waypoint as start point
-        WaypointData waypointPrev;
-        WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
-
-        pathDesired->Start.North = waypointPrev.Position.North;
-        pathDesired->Start.East  = waypointPrev.Position.East;
-        pathDesired->Start.Down  = waypointPrev.Position.Down;
-        pathDesired->StartingVelocity = waypointPrev.Velocity;
-    }
-}
-
-#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
-#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
-static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
-{
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-    float velocity_down;
-    float autotakeoff_height;
-    FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
-    FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
-    autotakeoff_height = fabsf(autotakeoff_height);
-    if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
-        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
-    } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
-        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
-    }
-
-
-    pathDesired->Start.North = positionState.North;
-    pathDesired->Start.East  = positionState.East;
-    pathDesired->Start.Down  = positionState.Down;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = -velocity_down;
-    // initially halt takeoff.
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
-
-    pathDesired->End.North = positionState.North;
-    pathDesired->End.East  = positionState.East;
-    pathDesired->End.Down  = positionState.Down - autotakeoff_height;
-
-    pathDesired->StartingVelocity = 0.0f;
-    pathDesired->EndingVelocity   = 0.0f;
-    pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
-
-    pathDesired->UID  = waypointActive.Index;
-}
-
-static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
-{
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-    float velocity_down;
-
-    FlightModeSettingsLandingVelocityGet(&velocity_down);
-
-    pathDesired->Start.North = positionState.North;
-    pathDesired->Start.East  = positionState.East;
-    pathDesired->Start.Down  = positionState.Down;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST]  = 0.0f;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]  = velocity_down;
-
-    pathDesired->End.North = positionState.North;
-    pathDesired->End.East  = positionState.East;
-    pathDesired->End.Down  = positionState.Down;
-
-    pathDesired->StartingVelocity = 0.0f;
-    pathDesired->EndingVelocity   = 0.0f;
-    pathDesired->Mode = PATHDESIRED_MODE_LAND;
-    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
-}
-
 
 // helper function to go to a specific waypoint
 static void setWaypoint(uint16_t num)

flight/modules/System/systemmod.c

@@ -465,9 +465,11 @@ static void callbackSchedulerForEachCallback(int16_t callback_id, const struct p
         return;
     }
     // delayed callback scheduler reports callback stack overflows as remaininng: -1
+#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
     if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
         stackOverflow = STACKOVERFLOW_WARNING;
     }
+#endif
     // By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
     // of the CallbackInfoXXXXElem enums
     PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
@@ -681,6 +683,7 @@ void vApplicationIdleHook(void)
 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 {
+#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
     stackOverflow = STACKOVERFLOW_CRITICAL;
 #if DEBUG_STACK_OVERFLOW
     static volatile bool wait_here = true;
@@ -689,6 +692,7 @@ void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
     }
     wait_here = true;
 #endif
+#endif
 }
 
 /**

ground/gcs/src/plugins/hitl/fgsimulator.cpp

@@ -2,7 +2,8 @@
  ******************************************************************************
  *
  * @file       flightgearbridge.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
  * @addtogroup HITLPlugin HITL Plugin
@@ -269,7 +270,7 @@ void FGSimulator::processUpdate(const QByteArray & inp)
     // Get pressure (kpa)
     float pressure     = fields[20].toFloat() * INHG2KPA;
     // Get VelocityState Down (m/s)
-    float velocityStateDown  = -fields[21].toFloat() * FPS2CMPS * 1e-2f;
+    float velocityStateDown  = fields[21].toFloat() * FPS2CMPS * 1e-2f;
     // Get VelocityState East (m/s)
     float velocityStateEast  = fields[22].toFloat() * FPS2CMPS * 1e-2f;
     // Get VelocityState Down (m/s)

shared/uavobjectdefinition/fixedwingpathfollowersettings.xml

@@ -3,54 +3,43 @@
         <description>Settings for the @ref FixedWingPathFollowerModule</description>
 
         <!-- these coefficients control desired movement in airspace -->
-        <field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="20"/>
+        <field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="20" description="Maximum speed the autopilot will try to achieve, usually for long distances" />
-                <!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
+        <field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="10" description="Minimum speed the autopilot will try to fly, for example when loitering" />
-        <field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
+        <field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10" description="maximum allowed climb or sink rate in guided flight" />
-                <!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
-        <field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10"/>
-                <!-- maximum allowed climb or sink rate in guided flight-->
 
-        <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
+        <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0" description="how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response" />
-                <!-- how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response -->
+        <field name="ReverseCourseOverlap" units="deg" type="float" elements="1" defaultvalue="20.0" description="how big the overlapping area behind the plane is, where, if the desired course lies in, the plane will not change bank angle while turning (to prevent oscillation)" />
-        <field name="ReverseCourseOverlap" units="deg" type="float" elements="1" defaultvalue="20.0"/>
-		<!-- how big the overlapping area behind the plane is, where, if the desired course lies behind, the current bank angle will determine if the plane goes left or right -->
 
-        <field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.2"/>
+        <field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.2" description="proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations" />
-                <!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
+        <field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4" description="proportional coefficient for desired vertical speed in relation to altitude displacement - reduce if plane is oscillating vertically, increase if altitude is not held reliably" />
-        <field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
+
-                <!-- proportional coefficient for desired vertical speed in relation to altitude displacement-->
+	<field name="UseAirspeedSensor" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" description="allows to ignore the airspeed sensor, set to false if you have none, which will trigger fallback algorithm without indicating a fault" />
 
         <!-- these coefficients control actual control outputs -->
-        <field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
+        <field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2,0,0" description="coefficients for desired bank angle in relation to ground bearing error - this controls heading" />
-                <!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading -->
 
-        <field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="2.5, .25, 10"/>
+        <field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,.25,10" description="coefficients for desired pitch in relation to speed error" />
-                <!-- coefficients for desired pitch
+        <field name="VerticalToPitchCrossFeed" units="deg / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="5,10" description="coefficients for adjusting desired pitch in relation to vertical speed error" />
-                     in relation to speed error IASerror -->
+        <field name="AirspeedToPowerCrossFeed" units="1 / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="0.2,1" description="proportional coefficient for adjusting vertical speed error for power calculation in relation to airspeed error" />
-        <field name="VerticalToPitchCrossFeed" units="deg / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
+        <field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.05,0.5" description="proportional coefficient for desired thrust in relation to vertical speed error (absolute but including crossfeed)" />
-                <!-- coefficients for adjusting desired pitch
-                     in relation to "vertical speed error -->
-        <field name="AirspeedToPowerCrossFeed" units="1 / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="0.2, 1"/>
-                <!-- proportional coefficient for adjusting vertical speed error for power calculation
-                     in relation to airspeed error IASerror -->
-        <field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.05,0.5"/>
-                <!-- proportional coefficient for desired thrust
-                     in relation to vertical speed error (absolute but including crossfeed) -->
 
         <!-- output limits -->
-        <field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-45,0,45" />
+        <field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-45,0,45" description="maximum allowed bank angles in navigates flight" />
-                <!-- maximum allowed bank angles in navigates flight -->
+        <field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" description="maximum allowed pitch angles and setpoint for neutral pitch" />
-        <field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" />
+        <field name="ThrustLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" description="minimum and maximum allowed thrust and setpoint for cruise speed" />
-                <!-- maximum allowed pitch angles and setpoint for neutral pitch -->
+	<field name="Safetymargins" units="" type="float"
-        <field name="ThrustLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
+                elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol"
-                <!-- minimum and maximum allowed thrust and setpoint for cruise speed -->
+                defaultvalue="1,1.0,0.5,1.5,1.0,1,0,1,1"
-        <field name="Safetymargins" units="" type="float"
+		description="Wind: degrees of crabbing allowed -- Speeds: percentage (1.0=100%) of the limit to be over/under -- Power &amp; Control: flag to turn on/off 0.0=off 1.0 = on" />
-                elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Pitchcontrol"
+        <field name="SafetyCutoffLimits" units="" type="float"
-                defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" />
+		elementnames="RollDeg,PitchDeg,YawDeg,MaxDecelerationDeltaMPS" defaultvalue="25.0,25.0,25.0,4.0"
-                <!-- Wind: degrees of crabbing allowed
+		description="maximum margins from attempted attitude allowed during takeoff -- In flight Roll/Pitch are added to RollLimit/PitchLimit as max limits - throttle is cut when beyond if roll &gt; RollLimit.Max + TakeOffLimits.RollDeg the same happens when MaxDecelerationDeltaMPS below Lowspeed limit if speed &lt; Safetymargins.Lowspeed*HorizontalVelMin - TakeOffLimits.MaxDecelerationDeltaMPS" />
-                     Speeds: percentage (1.0=100%) of the limit to be over/onder
+
-                     Power & Control: flag to turn on/off 0.0 =off 1.0 = on -->
+
-        <field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
+	<field name="TakeOffPitch" units="deg" type="float" elements="1" defaultvalue="25.0" description="pitch angle in autotakeoff mode" />
+	<field name="LandingPitch" units="deg" type="float" elements="1" defaultvalue="7.5" description="pitch angle in autoland mode" />
+
+        <field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100" description="update period of pathfollower" />
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml

@@ -4,7 +4,7 @@
 	<field name="Error" units="" type="float" elementnames="Course,Speed,Power" />
 	<field name="ErrorInt" units="" type="float" elementnames="Course,Speed,Power" />
 	<field name="Command" units="" type="float" elementnames="Course,Speed,Power" />
-	<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" />
+	<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol,AirspeedSensor" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="500"/>

shared/uavobjectdefinition/flightmodesettings.xml

@@ -94,6 +94,7 @@
         <field name="AutoTakeOffHeight" units="m" type="float" elements="1" defaultvalue="2.5" description="height in meters above arming altitude to climb to during autotakeoff"/>
         <field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
 	<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
+	<field name="FlightModeChangeRestartsPathPlan" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" description="wether a path plan should continue when interrupted by flight mode change (False), or restart from waypoint 0 (True)"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>