diff --git a/flight/modules/PathFollower/inc/vtolautotakeofffsm.h b/flight/modules/PathFollower/inc/vtolautotakeofffsm.h
index 3f8badb70..adbc76a38 100644
--- a/flight/modules/PathFollower/inc/vtolautotakeofffsm.h
+++ b/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 @@ protected:
     int32_t runAlways();
 
     void updateVtolAutoTakeoffFSMStatus();
-    void assessAltitude(void);
+    float assessAltitude(void);
 
     void setStateTimeout(int32_t count);
 
diff --git a/flight/modules/PathFollower/inc/vtollandfsm.h b/flight/modules/PathFollower/inc/vtollandfsm.h
index 4c582a00a..5c3fe8e75 100644
--- a/flight/modules/PathFollower/inc/vtollandfsm.h
+++ b/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 @@ protected:
     int32_t runState();
     int32_t runAlways();
     void updateVtolLandFSMStatus();
-    void assessAltitude(void);
+    float assessAltitude(void);
 
     void setStateTimeout(int32_t count);
 
diff --git a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
index 613ed0d42..d4e6240f5 100644
--- a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
+++ b/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
  *
diff --git a/flight/modules/PathFollower/vtolautotakeofffsm.cpp b/flight/modules/PathFollower/vtolautotakeofffsm.cpp
index 1b71a6114..372e01af6 100644
--- a/flight/modules/PathFollower/vtolautotakeofffsm.cpp
+++ b/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 altitudeAboveGround = 0.0f;
-        if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
-            altitudeAboveGround = mAutoTakeoffData->takeOffLocation.Down - positionState.Down;
-        }
-        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = altitudeAboveGround;
-        assessAltitude();
+        float altitudeAboveTakeoff = assessAltitude();
+        mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
     }
 
     // Restart state timer counter
@@ -290,7 +285,7 @@ void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
 }
 
 
-void VtolAutoTakeoffFSM::assessAltitude(void)
+float VtolAutoTakeoffFSM::assessAltitude(void)
 {
     float positionDown;
 
@@ -300,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
diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp
index acfa675a0..236acdc72 100644
--- a/flight/modules/PathFollower/vtollandfsm.cpp
+++ b/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;
 }