Merged in Oblivium/librepilot/LP-598_Autotakeoff_Corrections (pull request #512)

LP-598 Autotakeoff Corrections

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Jan NIJS <dr.oblivium@gmail.com>

Author: dr-oblivium

flight/modules/PathFollower/inc/vtolautotakeofffsm.h

@@ -3,13 +3,13 @@
  * @addtogroup LibrePilotModules LibrePilot Modules
  * @{
  * @addtogroup PathFollower FSM
- * @brief Executes landing sequence via an FSM
+ * @brief Executes auto takeoff sequence via an FSM
  * @{
  *
- * @file       vtollandfsm.h
- * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ * @file       vtolautotakeofffsm.h
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Executes FSM for landing sequence
+ * @brief      Executes FSM for auto takeoff sequence
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -144,7 +144,7 @@ class VtolAutoTakeoffFSM : public PathFollowerFSM {
     int32_t runAlways();
 
     void updateVtolAutoTakeoffFSMStatus();
-    void assessAltitude(void);
+    float assessAltitude(void);
 
     void setStateTimeout(int32_t count);
 

flight/modules/PathFollower/inc/vtollandfsm.h

@@ -7,7 +7,8 @@
  * @{
  *
  * @file       vtollandfsm.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes FSM for landing sequence
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -133,7 +134,7 @@ class VtolLandFSM : public PathFollowerFSM {
     int32_t runState();
     int32_t runAlways();
     void updateVtolLandFSMStatus();
-    void assessAltitude(void);
+    float assessAltitude(void);
 
     void setStateTimeout(int32_t count);
 

flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

@@ -1,10 +1,10 @@
 /*
  ******************************************************************************
  *
- * @file       vtollandcontroller.cpp
- * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ * @file       vtolautotakeoffcontroller.cpp
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Vtol landing controller loop
+ * @brief      Vtol auto takeoff controller loop
  * @see        The GNU Public License (GPL) Version 3
  * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  *
@@ -132,19 +132,19 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
     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_velocity;
         float autotakeoff_height;
         PositionStateData positionState;
         PositionStateGet(&positionState);
-        FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
+        FlightModeSettingsAutoTakeOffVelocityGet(&autotakeoff_velocity);
         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);
+        controlDown.UpdateVelocitySetpoint(-autotakeoff_velocity);
         controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
         controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
 
@@ -320,9 +320,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
     // 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.
+    // 4. Once the user increases the throttle position to above 30%, then and only then initiate auto-takeoff.
+    // 5. Whilst the throttle is < 30% before takeoff, all stick inputs are being ignored.
+    // 6. If during the autotakeoff sequence, at any stage, the throttle stick position reduces to less than 10%, landing is initiated.
 
     switch (autotakeoffState) {
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:

flight/modules/PathFollower/vtolautotakeofffsm.cpp

@@ -2,7 +2,8 @@
  ******************************************************************************
  *
  * @file       vtolautotakeofffsm.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      This autotakeoff state machine is a helper state machine to the
  *             VtolAutoTakeoffController.
  * @see        The GNU Public License (GPL) Version 3
@@ -60,11 +61,11 @@ extern "C" {
 
 
 // Private constants
-#define TIMER_COUNT_PER_SECOND         (1000 / vtolPathFollowerSettings->UpdatePeriod)
-#define TIMEOUT_SLOWSTART              (2 * TIMER_COUNT_PER_SECOND)
-#define TIMEOUT_THRUSTUP               (1 * TIMER_COUNT_PER_SECOND)
-#define TIMEOUT_THRUSTDOWN             (5 * TIMER_COUNT_PER_SECOND)
-#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
+#define TIMER_COUNT_PER_SECOND      (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define TIMEOUT_SLOWSTART           (2 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTUP            (1 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTDOWN          (5 * TIMER_COUNT_PER_SECOND)
+#define AUTOTAKEOFF_SLOWDOWN_HEIGHT -5.0f
 
 VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
     [AUTOTAKEOFF_STATE_INACTIVE]   = { .setup = &VtolAutoTakeoffFSM::setup_inactive,   .run = 0                                   },
@@ -246,14 +247,8 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St
     mAutoTakeoffData->currentState = newState;
 
     if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-        float takeOffDown = 0.0f;
-        if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
-            takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
-        }
-        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
-        assessAltitude();
+        float altitudeAboveTakeoff = assessAltitude();
+        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
     }
 
     // Restart state timer counter
@@ -266,6 +261,7 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St
         (this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
     }
 
+    // Update StatusVtolAutoTakeoff UAVObject with new status
     updateVtolAutoTakeoffFSMStatus();
 }
 
@@ -276,6 +272,7 @@ void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
     mAutoTakeoffData->stateTimeoutCount = count;
 }
 
+// Update StatusVtolAutoTakeoff UAVObject
 void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
 {
     mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
@@ -288,7 +285,7 @@ void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
 }
 
 
-void VtolAutoTakeoffFSM::assessAltitude(void)
+float VtolAutoTakeoffFSM::assessAltitude(void)
 {
     float positionDown;
 
@@ -298,11 +295,13 @@ void VtolAutoTakeoffFSM::assessAltitude(void)
         takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
     }
     float positionDownRelativeToTakeoff = positionDown - takeOffDown;
-    if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
+    if (positionDownRelativeToTakeoff < AUTOTAKEOFF_SLOWDOWN_HEIGHT) {
         mAutoTakeoffData->flLowAltitude = false;
     } else {
         mAutoTakeoffData->flLowAltitude = true;
     }
+    // Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff
+    return -positionDownRelativeToTakeoff;
 }
 
 // Action the required state from plans.c
@@ -347,10 +346,10 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
     // Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
     // 1. Already armed
     // 2. Not in flight. This was checked in plans.c
-    // 3. User has placed throttle position to more than 50% to allow autotakeoff
+    // 3. User has placed throttle position to more than 30% to allow autotakeoff
 
     // If pathplanner, we need additional checks
-    // E.g. if inflight, this mode is just positon hol
+    // E.g. if inflight, this mode is just position hold
     StabilizationDesiredData stabDesired;
 
     StabilizationDesiredGet(&stabDesired);
@@ -407,15 +406,13 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout
     }
 }
 
-// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
+// STATE: THRUSTUP spools up motors to vtol min over 1 second for effect.
 // PID loops may be cumulating I terms but that problem needs to be solved
 #define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
 void VtolAutoTakeoffFSM::setup_thrustup(void)
 {
     setStateTimeout(TIMEOUT_THRUSTUP);
     mAutoTakeoffData->flZeroStabiHorizontal = false;
-    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
     mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
     mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
     mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;

flight/modules/PathFollower/vtollandfsm.cpp

@@ -2,7 +2,8 @@
  ******************************************************************************
  *
  * @file       vtollandfsm.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      This landing state machine is a helper state machine to the
  *             VtolLandController.
  * @see        The GNU Public License (GPL) Version 3
@@ -275,14 +276,8 @@ void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandSt
     mLandData->currentState = newState;
 
     if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-        float takeOffDown = 0.0f;
-        if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
-            takeOffDown = mLandData->takeOffLocation.Down;
-        }
-        mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
-        assessAltitude();
+        float altitudeAboveTakeoff = assessAltitude();
+        mLandData->fsmLandStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
     }
 
     // Restart state timer counter
@@ -332,7 +327,7 @@ float VtolLandFSM::BoundVelocityDown(float velocity_down)
     }
 }
 
-void VtolLandFSM::assessAltitude(void)
+float VtolLandFSM::assessAltitude(void)
 {
     float positionDown;
 
@@ -347,6 +342,8 @@ void VtolLandFSM::assessAltitude(void)
     } else {
         mLandData->flLowAltitude = true;
     }
+    // Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff
+    return -positionDownRelativeToTakeoff;
 }