diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h
index 8c5f55858..cc32cf442 100644
--- a/flight/libraries/inc/CoordinateConversions.h
+++ b/flight/libraries/inc/CoordinateConversions.h
@@ -75,5 +75,27 @@ void quat_inverse(float q[4]);
 void quat_copy(const float q[4], float qnew[4]);
 void quat_mult(const float q1[4], const float q2[4], float qout[4]);
 void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
+/**
+ * matrix_mult_3x3f - perform a multiplication between two 3x3 float matrices
+ * result = a*b
+ * @param a
+ * @param b
+ * @param result
+ */
+inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
+{
+    result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
+    result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
+    result[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2];
+
+    result[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2];
+    result[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2];
+    result[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2];
+
+    result[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2];
+    result[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2];
+    result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
+}
+
 
 #endif // COORDINATECONVERSIONS_H_
diff --git a/flight/libraries/inc/plans.h b/flight/libraries/inc/plans.h
new file mode 100644
index 000000000..207567b3a
--- /dev/null
+++ b/flight/libraries/inc/plans.h
@@ -0,0 +1,94 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotLibraries OpenPilot Libraries
+ * @{
+ * @addtogroup Navigation
+ * @brief setups RTH/PH and other pathfollower/pathplanner status
+ * @{
+ *
+ * @file       plan.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 PLANS_H_
+#define PLANS_H_
+#include <pios_math.h>
+
+/** \page standard Plans
+   How to use this library
+   \li Call plan_initialize() to init all needed struct and uavos at startup.<br>
+   It may be safely called more than once.<br>
+
+   \li Functions called plan_setup_* needs to be called once, every time the requested function is engaged.<br>
+
+   \li Functions called plan_run_* are to be periodically called while the requested mode is engaged.<br>
+ */
+
+/**
+ * @brief initialize UAVOs and structs used by this library
+ */
+void plan_initialize();
+
+/**
+ * @brief setup pathplanner/pathfollower for positionhold
+ */
+void plan_setup_positionHold();
+
+/**
+ * @brief setup pathplanner/pathfollower for return to base
+ */
+void plan_setup_returnToBase();
+
+/**
+ * @brief setup pathplanner/pathfollower for land
+ */
+void plan_setup_land();
+
+/**
+ * @brief execute land
+ */
+void plan_run_land();
+
+/**
+ * @brief setup pathfollower for positionvario
+ */
+void plan_setup_PositionVarioFPV();
+void plan_setup_PositionVarioLOS();
+void plan_setup_PositionVarioNSEW();
+
+/**
+ * @brief run for positionvario
+ */
+void plan_run_PositionVarioFPV();
+void plan_run_PositionVarioLOS();
+void plan_run_PositionVarioNSEW();
+
+/**
+ * @brief setup pathplanner/pathfollower for AutoCruise
+ */
+void plan_setup_AutoCruise();
+
+/**
+ * @brief execute AutoCruise
+ */
+void plan_run_AutoCruise();
+
+#endif /* PLANS_H_ */
diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c
new file mode 100644
index 000000000..9055419fa
--- /dev/null
+++ b/flight/libraries/plans.c
@@ -0,0 +1,465 @@
+/**
+ ******************************************************************************
+ * @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.
+ *
+ * @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
+ */
+
+#include <plans.h>
+#include <openpilot.h>
+#include <attitudesettings.h>
+#include <takeofflocation.h>
+#include <pathdesired.h>
+#include <positionstate.h>
+#include <flightmodesettings.h>
+#include <manualcontrolcommand.h>
+#include <attitudestate.h>
+#include <sin_lookup.h>
+
+#define UPDATE_EXPECTED 0.02f
+#define UPDATE_MIN      1.0e-6f
+#define UPDATE_MAX      1.0f
+#define UPDATE_ALPHA    1.0e-2f
+
+/**
+ * @brief initialize UAVOs and structs used by this library
+ */
+void plan_initialize()
+{
+    TakeOffLocationInitialize();
+    PositionStateInitialize();
+    PathDesiredInitialize();
+    FlightModeSettingsInitialize();
+    AttitudeStateInitialize();
+    ManualControlCommandInitialize();
+}
+
+/**
+ * @brief setup pathplanner/pathfollower for positionhold
+ */
+void plan_setup_positionHold()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+    float startingVelocity;
+    FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
+
+    pathDesired.End.North        = positionState.North;
+    pathDesired.End.East         = positionState.East;
+    pathDesired.End.Down         = positionState.Down;
+    pathDesired.Start.North      = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
+    pathDesired.Start.East       = positionState.East;
+    pathDesired.Start.Down       = positionState.Down;
+    pathDesired.StartingVelocity = startingVelocity;
+    pathDesired.EndingVelocity   = 0.0f;
+    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+}
+
+/**
+ * @brief setup pathplanner/pathfollower for return to base
+ */
+void plan_setup_returnToBase()
+{
+    // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
+    float positionStateDown;
+
+    PositionStateDownGet(&positionStateDown);
+
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    TakeOffLocationData takeoffLocation;
+    TakeOffLocationGet(&takeoffLocation);
+
+    // TODO: right now VTOLPF does fly straight to destination altitude.
+    // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
+
+    float destDown;
+    FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
+    destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+    float startingVelocity;
+    FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
+
+    pathDesired.End.North        = takeoffLocation.North;
+    pathDesired.End.East         = takeoffLocation.East;
+    pathDesired.End.Down         = destDown;
+
+    pathDesired.Start.North      = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
+    pathDesired.Start.East       = takeoffLocation.East;
+    pathDesired.Start.Down       = destDown;
+
+    pathDesired.StartingVelocity = startingVelocity;
+    pathDesired.EndingVelocity   = 0.0f;
+    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+}
+
+void plan_setup_land()
+{
+    plan_setup_positionHold();
+}
+
+/**
+ * @brief execute land
+ */
+void plan_run_land()
+{
+    PathDesiredEndData pathDesiredEnd;
+
+    PathDesiredEndGet(&pathDesiredEnd);
+
+    PositionStateDownGet(&pathDesiredEnd.Down);
+    pathDesiredEnd.Down += 5;
+
+    PathDesiredEndSet(&pathDesiredEnd);
+}
+
+
+/**
+ * @brief positionvario functionality
+ */
+static bool vario_hold = true;
+static float hold_position[3];
+
+static void plan_setup_PositionVario()
+{
+    vario_hold = true;
+    plan_setup_positionHold();
+}
+
+void plan_setup_PositionVarioFPV()
+{
+    plan_setup_PositionVario();
+}
+
+void plan_setup_PositionVarioLOS()
+{
+    plan_setup_PositionVario();
+}
+
+void plan_setup_PositionVarioNSEW()
+{
+    plan_setup_PositionVario();
+}
+
+
+#define DEADBAND 0.1f
+static bool normalizeDeadband(float controlVector[4])
+{
+    bool moving = false;
+
+    // roll, pitch, yaw between -1 and +1
+    // thrust between 0 and 1 mapped to -1 to +1
+    controlVector[3] = (2.0f * controlVector[3]) - 1.0f;
+    int t;
+
+    for (t = 0; t < 4; t++) {
+        if (controlVector[t] < -DEADBAND) {
+            moving = true;
+            controlVector[t] += DEADBAND;
+        } else if (controlVector[t] > DEADBAND) {
+            moving = true;
+            controlVector[t] -= DEADBAND;
+        } else {
+            controlVector[t] = 0.0f;
+        }
+        // deadband has been cut out, scale value back to [-1,+1]
+        controlVector[t] *= (1.0f / (1.0f - DEADBAND));
+        controlVector[t]  = boundf(controlVector[t], -1.0f, 1.0f);
+    }
+
+    return moving;
+}
+
+typedef enum { FPV, LOS, NSEW } vario_type;
+
+static void getVector(float controlVector[4], vario_type type)
+{
+    FlightModeSettingsPositionHoldOffsetData offset;
+
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+
+    // scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive
+    controlVector[3] *= offset.Vertical / offset.Horizontal;
+
+    float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]);
+
+    if (length <= 1e-9f) {
+        length = 1.0f; // should never happen as getVector is not called if control within deadband
+    }
+    {
+        float direction[3] = {
+            controlVector[1] / length, // pitch is north
+            controlVector[0] / length, // roll is east
+            controlVector[3] / length // thrust is down
+        };
+        controlVector[0] = direction[0];
+        controlVector[1] = direction[1];
+        controlVector[2] = direction[2];
+    }
+    controlVector[3] = length * offset.Horizontal;
+
+    // rotate north and east - rotation angle based on type
+    float angle;
+    switch (type) {
+    case NSEW:
+        angle = 0.0f;
+        // NSEW no rotation takes place
+        break;
+    case FPV:
+        // local rotation, using current yaw
+        AttitudeStateYawGet(&angle);
+        break;
+    case LOS:
+        // determine location based on vector from takeoff to current location
+    {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        TakeOffLocationData takeoffLocation;
+        TakeOffLocationGet(&takeoffLocation);
+        angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North));
+    }
+    break;
+    }
+    // rotate horizontally by angle
+    {
+        float rotated[2] = {
+            controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle),
+            controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle)
+        };
+        controlVector[0] = rotated[0];
+        controlVector[1] = rotated[1];
+    }
+}
+
+
+static void plan_run_PositionVario(vario_type type)
+{
+    float controlVector[4];
+    PathDesiredData pathDesired;
+
+    PathDesiredGet(&pathDesired);
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+
+
+    ManualControlCommandRollGet(&controlVector[0]);
+    ManualControlCommandPitchGet(&controlVector[1]);
+    ManualControlCommandYawGet(&controlVector[2]);
+    ManualControlCommandThrustGet(&controlVector[3]);
+
+    // check if movement is desired
+    if (normalizeDeadband(controlVector) == false) {
+        // no movement desired, re-enter positionHold at current start-position
+        if (!vario_hold) {
+            vario_hold = true;
+
+            // new hold position is the position that was previously the start position
+            pathDesired.End.North   = hold_position[0];
+            pathDesired.End.East    = hold_position[1];
+            pathDesired.End.Down    = hold_position[2];
+            // while the new start position has the same offset as in position hold
+            pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
+            pathDesired.Start.East  = pathDesired.End.East;
+            pathDesired.Start.Down  = pathDesired.End.Down;
+            PathDesiredSet(&pathDesired);
+        }
+    } else {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+
+        // flip pitch to have pitch down (away) point north
+        controlVector[1] = -controlVector[1];
+        getVector(controlVector, type);
+
+        // layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4}
+        if (vario_hold) {
+            // start position is the position that was previously the hold position
+            vario_hold = false;
+            hold_position[0] = pathDesired.End.North;
+            hold_position[1] = pathDesired.End.East;
+            hold_position[2] = pathDesired.End.Down;
+        } else {
+            // start position is advanced according to movement - in the direction of ControlVector only
+            // projection using scalar product
+            float kp = (positionState.North - hold_position[0]) * controlVector[0]
+                       + (positionState.East - hold_position[1]) * controlVector[1]
+                       + (positionState.Down - hold_position[2]) * -controlVector[2];
+            if (kp > 0.0f) {
+                hold_position[0] += kp * controlVector[0];
+                hold_position[1] += kp * controlVector[1];
+                hold_position[2] += kp * -controlVector[2];
+            }
+        }
+        // new destination position is advanced based on controlVector
+        pathDesired.End.North   = hold_position[0] + controlVector[0] * controlVector[3];
+        pathDesired.End.East    = hold_position[1] + controlVector[1] * controlVector[3];
+        pathDesired.End.Down    = hold_position[2] - controlVector[2] * controlVector[3];
+        // the new start position has the same offset as in position hold
+        pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
+        pathDesired.Start.East  = pathDesired.End.East;
+        pathDesired.Start.Down  = pathDesired.End.Down;
+        PathDesiredSet(&pathDesired);
+    }
+}
+void plan_run_PositionVarioFPV()
+{
+    plan_run_PositionVario(FPV);
+}
+
+void plan_run_PositionVarioLOS()
+{
+    plan_run_PositionVario(LOS);
+}
+
+void plan_run_PositionVarioNSEW()
+{
+    plan_run_PositionVario(NSEW);
+}
+
+
+/**
+ * @brief setup pathplanner/pathfollower for AutoCruise
+ */
+static PiOSDeltatimeConfig actimeval;
+void plan_setup_AutoCruise()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+    float startingVelocity;
+    FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
+
+    // initialization is flight in direction of the nose.
+    // the velocity is not relevant, as it will be reset by the run function even during first call
+    float angle;
+    AttitudeStateYawGet(&angle);
+    float vector[2] = {
+        cos_lookup_deg(angle),
+        sin_lookup_deg(angle)
+    };
+    hold_position[0]             = positionState.North;
+    hold_position[1]             = positionState.East;
+    hold_position[2]             = positionState.Down;
+    pathDesired.End.North        = hold_position[0] + vector[0];
+    pathDesired.End.East         = hold_position[1] + vector[1];
+    pathDesired.End.Down         = hold_position[2];
+    // start position has the same offset as in position hold
+    pathDesired.Start.North      = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
+    pathDesired.Start.East       = pathDesired.End.East;
+    pathDesired.Start.Down       = pathDesired.End.Down;
+    pathDesired.StartingVelocity = startingVelocity;
+    pathDesired.EndingVelocity   = 0.0f;
+    pathDesired.Mode             = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+
+    // re-iniztializing deltatime is valid and also good practice here since
+    // getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode.
+    PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+}
+
+/**
+ * @brief execute autocruise
+ */
+void plan_run_AutoCruise()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+
+    float controlVector[4];
+    ManualControlCommandRollGet(&controlVector[0]);
+    ManualControlCommandPitchGet(&controlVector[1]);
+    ManualControlCommandYawGet(&controlVector[2]);
+    controlVector[3] = 0.5f; // dummy, thrust is normalized separately
+    normalizeDeadband(controlVector); // return value ignored
+    ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
+    controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction
+
+    // normalize old desired movement vector
+    float vector[3] = { pathDesired.End.North - hold_position[0],
+                        pathDesired.End.East - hold_position[1],
+                        pathDesired.End.Down - hold_position[2] };
+    float length    = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
+    if (length < 1e-9f) {
+        length = 1.0f; // should not happen since initialized properly in setup()
+    }
+    vector[0] /= length;
+    vector[1] /= length;
+    vector[2] /= length;
+
+    // start position is advanced according to actual movement - in the direction of desired vector only
+    // projection using scalar product
+    float kp = (positionState.North - hold_position[0]) * vector[0]
+               + (positionState.East - hold_position[1]) * vector[1]
+               + (positionState.Down - hold_position[2]) * vector[2];
+    if (kp > 0.0f) {
+        hold_position[0] += kp * vector[0];
+        hold_position[1] += kp * vector[1];
+        hold_position[2] += kp * vector[2];
+    }
+
+    // new angle is equal to old angle plus offset depending on yaw input and time
+    // (controlVector is normalized with a deadband, change is zero within deadband)
+    float angle = RAD2DEG(atan2f(vector[1], vector[0]));
+    float dT    = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
+    angle    += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings
+
+    // resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
+    vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
+    vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
+    vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];
+
+    pathDesired.End.North   = hold_position[0] + vector[0];
+    pathDesired.End.East    = hold_position[1] + vector[1];
+    pathDesired.End.Down    = hold_position[2] + vector[2];
+    // start position has the same offset as in position hold
+    pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
+    pathDesired.Start.East  = pathDesired.End.East;
+    pathDesired.Start.Down  = pathDesired.End.Down;
+    PathDesiredSet(&pathDesired);
+}
diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c
index 03ab299df..270543f24 100644
--- a/flight/libraries/sanitycheck.c
+++ b/flight/libraries/sanitycheck.c
@@ -37,8 +37,13 @@
 #include <flightmodesettings.h>
 #include <systemsettings.h>
 #include <systemalarms.h>
+#include <revosettings.h>
 #include <taskinfo.h>
 
+// a number of useful macros
+#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_ERROR))
+
+
 /****************************
 * Current checks:
 * 1. If a flight mode switch allows autotune and autotune module not running
@@ -46,7 +51,7 @@
 ****************************/
 
 // ! Check a stabilization mode switch position for safety
-static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
+static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
 
 /**
  * Run a preflight check over the hardware configuration
@@ -61,8 +66,27 @@ int32_t configuration_check()
     const struct pios_board_info *bdinfo = &pios_board_info_blob;
     bool coptercontrol     = bdinfo->board_type == 0x04;
 
+    // Classify navigation capability
+#ifdef REVOLUTION
+    RevoSettingsInitialize();
+    uint8_t revoFusion;
+    RevoSettingsFusionAlgorithmGet(&revoFusion);
+    bool navCapableFusion;
+    switch (revoFusion) {
+    case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
+    case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
+        navCapableFusion = true;
+        break;
+    default:
+        navCapableFusion = false;
+    }
+#else
+    const bool navCapableFusion = false;
+#endif
+
+
     // Classify airframe type
-    bool multirotor = true;
+    bool multirotor;
     uint8_t airframe_type;
 
     SystemSettingsAirframeTypeGet(&airframe_type);
@@ -93,82 +117,54 @@ int32_t configuration_check()
     for (uint32_t i = 0; i < num_modes; i++) {
         switch (modes[i]) {
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
-            if (multirotor) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
+            ADDSEVERITY(!multirotor);
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor, coptercontrol) : severity;
+            ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor, coptercontrol) : severity;
+            ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor, coptercontrol) : severity;
+            ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(4, multirotor, coptercontrol) : severity;
+            ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(5, multirotor, coptercontrol) : severity;
+            ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(6, multirotor, coptercontrol) : severity;
+            ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
-            if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
-            break;
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
-                // Revo supports Position Hold
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
-            break;
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
-                // Revo supports AutoLand Mode
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
-            break;
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
-                // Revo supports POI Mode
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
+            ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE));
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            } else {
-                // Revo supports PathPlanner and that must be OK or we are not sane
-                // PathPlan alarm is uninitialized if not running
-                // PathPlan alarm is warning or error if the flightplan is invalid
-                SystemAlarmsAlarmData alarms;
-                SystemAlarmsAlarmGet(&alarms);
-                if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
-                    severity = SYSTEMALARMS_ALARM_ERROR;
-                }
-            }
-            break;
+        {
+            // Revo supports PathPlanner and that must be OK or we are not sane
+            // PathPlan alarm is uninitialized if not running
+            // PathPlan alarm is warning or error if the flightplan is invalid
+            SystemAlarmsAlarmData alarms;
+            SystemAlarmsAlarmGet(&alarms);
+            ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
+        }
+        // intentionally no break as this also needs pathfollower
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOFPV:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOLOS:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIONSEW:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
-                // Revo supports ReturnToBase
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
+            ADDSEVERITY(!coptercontrol);
+            ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER));
+            ADDSEVERITY(navCapableFusion);
             break;
         default:
             // Uncovered modes are automatically an error
-            severity = SYSTEMALARMS_ALARM_ERROR;
+            ADDSEVERITY(false);
         }
         // mark the first encountered erroneous setting in status and substatus
         if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
@@ -191,9 +187,9 @@ int32_t configuration_check()
  * Checks the stabiliation settings for a paritcular mode and makes
  * sure it is appropriate for the airframe
  * @param[in] index Which stabilization mode to check
- * @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
+ * @returns true or false
  */
-static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
+static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
 {
     uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 
@@ -218,7 +214,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
         FlightModeSettingsStabilization6SettingsArrayGet(modes);
         break;
     default:
-        return SYSTEMALARMS_ALARM_ERROR;
+        return false;
     }
 
     // For multirotors verify that roll/pitch/yaw are not set to "none"
@@ -226,7 +222,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
     if (multirotor) {
         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
-                return SYSTEMALARMS_ALARM_ERROR;
+                return false;
             }
         }
     }
@@ -234,26 +230,26 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
     // coptercontrol cannot do altitude holding
     if (coptercontrol) {
         if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
-            || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
+            || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
             ) {
-            return SYSTEMALARMS_ALARM_ERROR;
+            return false;
         }
     }
 
     // check that thrust modes are only set to thrust axis
     for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
         if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
-            || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
+            || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
             ) {
-            return SYSTEMALARMS_ALARM_ERROR;
+            return false;
         }
     }
     if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
-          || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
+          || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
           )) {
-        return SYSTEMALARMS_ALARM_ERROR;
+        return false;
     }
 
     // Warning: This assumes that certain conditions in the XML file are met.  That
@@ -261,5 +257,5 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
     // (this is checked at compile time by static constraint manualcontrol.h)
 
-    return SYSTEMALARMS_ALARM_OK;
+    return true;
 }
diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
index d381f27ff..417665b6c 100644
--- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
+++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
@@ -63,6 +63,7 @@
 #include "taskinfo.h"
 #include <pios_struct_helper.h>
 
+#include "sin_lookup.h"
 #include "paths.h"
 #include "CoordinateConversions.h"
 
@@ -85,6 +86,7 @@ static void updatePathVelocity();
 static uint8_t updateFixedDesiredAttitude();
 static void updateFixedAttitude();
 static void airspeedStateUpdatedCb(UAVObjEvent *ev);
+static bool correctCourse(float *C, float *V, float *F, float s);
 
 /**
  * Initialise the module, called on startup
@@ -125,17 +127,17 @@ int32_t FixedWingPathFollowerInitialize()
 }
 MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart);
 
-static float northVelIntegral = 0;
-static float eastVelIntegral  = 0;
-static float downVelIntegral  = 0;
+static float northVelIntegral = 0.0f;
+static float eastVelIntegral  = 0.0f;
+static float downVelIntegral  = 0.0f;
 
-static float courseIntegral   = 0;
-static float speedIntegral    = 0;
-static float powerIntegral    = 0;
-static float airspeedErrorInt = 0;
+static float courseIntegral   = 0.0f;
+static float speedIntegral    = 0.0f;
+static float powerIntegral    = 0.0f;
+static float airspeedErrorInt = 0.0f;
 
 // correct speed by measured airspeed
-static float indicatedAirspeedStateBias = 0;
+static float indicatedAirspeedStateBias = 0.0f;
 
 /**
  * Module thread, should not return.
@@ -225,12 +227,12 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
             }
         } else {
             // Be cleaner and get rid of global variables
-            northVelIntegral = 0;
-            eastVelIntegral  = 0;
-            downVelIntegral  = 0;
-            courseIntegral   = 0;
-            speedIntegral    = 0;
-            powerIntegral    = 0;
+            northVelIntegral = 0.0f;
+            eastVelIntegral  = 0.0f;
+            downVelIntegral  = 0.0f;
+            courseIntegral   = 0.0f;
+            speedIntegral    = 0.0f;
+            powerIntegral    = 0.0f;
         }
         PathStatusSet(&pathStatus);
     }
@@ -276,14 +278,14 @@ static void updatePathVelocity()
     case PATHDESIRED_MODE_DRIVEVECTOR:
     default:
         groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
-                      boundf(progress.fractional_progress, 0, 1);
+                      boundf(progress.fractional_progress, 0.0f, 1.0f);
         altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
-                           boundf(progress.fractional_progress, 0, 1);
+                           boundf(progress.fractional_progress, 0.0f, 1.0f);
         break;
     }
     // make sure groundspeed is not zero
-    if (groundspeed < 1e-2f) {
-        groundspeed = 1e-2f;
+    if (groundspeed < 1e-6f) {
+        groundspeed = 1e-6f;
     }
 
     // calculate velocity - can be zero if waypoints are too close
@@ -302,24 +304,12 @@ static void updatePathVelocity()
     // in this case the plane effectively needs to be turned around
     // indicators:
     // difference between correction_direction and velocitystate >90 degrees and
-    // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from eerything )
+    // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from everything )
     // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
-    float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North));
-    float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North));
-    if (angle1 < -180.0f) {
-        angle1 += 360.0f;
-    }
-    if (angle1 > 180.0f) {
-        angle1 -= 360.0f;
-    }
-    if (angle2 < -180.0f) {
-        angle2 += 360.0f;
-    }
-    if (angle2 > 180.0f) {
-        angle2 -= 360.0f;
-    }
-    if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) {
-        error_speed = 0;
+    if ( // calculating angles < 90 degrees through dot products
+        ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) &&
+        ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) {
+        error_speed = 0.0f;
     }
 
     // calculate correction - can also be zero if correction vector is 0 or no error present
@@ -328,14 +318,16 @@ static void updatePathVelocity()
 
     // scale to correct length
     float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
-    velocityDesired.North *= groundspeed / l;
-    velocityDesired.East  *= groundspeed / l;
+    if (l > 1e-9f) {
+        velocityDesired.North *= groundspeed / l;
+        velocityDesired.East  *= groundspeed / l;
+    }
 
     float downError = altitudeSetpoint - positionState.Down;
-    velocityDesired.Down   = downError * fixedwingpathfollowerSettings.VerticalPosP;
+    velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
 
     // update pathstatus
-    pathStatus.error = progress.error;
+    pathStatus.error     = progress.error;
     pathStatus.fractional_progress = progress.fractional_progress;
 
     VelocityDesiredSet(&velocityDesired);
@@ -384,8 +376,7 @@ static uint8_t updateFixedDesiredAttitude()
     AirspeedStateData airspeedState;
     SystemSettingsData systemSettings;
 
-    float groundspeedState;
-    float groundspeedDesired;
+    float groundspeedProjection;
     float indicatedAirspeedState;
     float indicatedAirspeedDesired;
     float airspeedError;
@@ -396,10 +387,9 @@ static uint8_t updateFixedDesiredAttitude()
     float descentspeedError;
     float powerCommand;
 
-    float bearing;
-    float heading;
-    float headingError;
-    float course;
+    float airspeedVector[2];
+    float fluidMovement[2];
+    float courseComponent[2];
     float courseError;
     float courseCommand;
 
@@ -413,24 +403,54 @@ static uint8_t updateFixedDesiredAttitude()
     AirspeedStateGet(&airspeedState);
     SystemSettingsGet(&systemSettings);
 
-
     /**
-     * Compute speed error (required for thrust and pitch)
+     * 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
-    groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
-    // note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
-    // but thanks to accelerometers,  groundspeed reacts faster to changes in direction
+    // 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   = groundspeedState + indicatedAirspeedStateBias;
+    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
 
-    // Desired ground speed
-    groundspeedDesired       = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
-    indicatedAirspeedDesired = boundf(groundspeedDesired + 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]),
                                       fixedwingpathfollowerSettings.HorizontalVelMin,
                                       fixedwingpathfollowerSettings.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) &&
+        fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
+        fixedwingpathfollowerStatus.Errors.Wind = 1;
+        result = 0;
+    }
+
     // Airspeed error
     airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
 
@@ -461,23 +481,18 @@ static uint8_t updateFixedDesiredAttitude()
         result = 0;
     }
 
-    if (indicatedAirspeedState < 1e-6f) {
-        // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
-        // also we cannot handle planes flying backwards, lets just wait until the nose drops
-        fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
-        return 0;
-    }
-
     /**
      * Compute desired thrust command
      */
     // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
-    if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
+    if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) {
         powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
                                -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
                                fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
                                ) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
-    } else { powerIntegral = 0; }
+    } else {
+        powerIntegral = 0.0f;
+    }
 
     // Compute the cross feed from vertical speed to pitch, with saturation
     float speedErrorToPowerCommandComponent = boundf(
@@ -504,9 +519,9 @@ static uint8_t updateFixedDesiredAttitude()
     // Error condition: plane cannot hold altitude at current speed.
     fixedwingpathfollowerStatus.Errors.Lowpower = 0;
     if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
-        velocityState.Down > 0 && // we ARE going down
-        descentspeedDesired < 0 && // we WANT to go up
-        airspeedError > 0 && // we are too slow already
+        velocityState.Down > 0.0f && // we ARE going down
+        descentspeedDesired < 0.0f && // we WANT to go up
+        airspeedError > 0.0f && // we are too slow already
         fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
         fixedwingpathfollowerStatus.Errors.Lowpower = 1;
         result = 0;
@@ -514,9 +529,9 @@ static uint8_t updateFixedDesiredAttitude()
     // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
     fixedwingpathfollowerStatus.Errors.Highpower = 0;
     if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
-        velocityState.Down < 0 && // we ARE going up
-        descentspeedDesired > 0 && // we WANT to go down
-        airspeedError < 0 && // we are too fast already
+        velocityState.Down < 0.0f && // we ARE going up
+        descentspeedDesired > 0.0f && // we WANT to go down
+        airspeedError < 0.0f && // we are too fast already
         fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
         fixedwingpathfollowerStatus.Errors.Highpower = 1;
         result = 0;
@@ -526,7 +541,6 @@ static uint8_t updateFixedDesiredAttitude()
     /**
      * Compute desired pitch command
      */
-
     if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
         // Integrate with saturation
         airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
@@ -556,48 +570,18 @@ static uint8_t updateFixedDesiredAttitude()
     // Error condition: high speed dive
     fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
     if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
-        velocityState.Down > 0 && // we ARE going down
-        descentspeedDesired < 0 && // we WANT to go up
-        airspeedError < 0 && // we are too fast already
+        velocityState.Down > 0.0f && // we ARE going down
+        descentspeedDesired < 0.0f && // we WANT to go up
+        airspeedError < 0.0f && // we are too fast already
         fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
         fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1;
         result = 0;
     }
 
-    /**
-     * Calculate where we are heading and why (wind issues)
-     */
-    bearing = attitudeState.Yaw;
-    heading = RAD2DEG(atan2f(velocityState.East, velocityState.North));
-    headingError = heading - bearing;
-    if (headingError < -180.0f) {
-        headingError += 360.0f;
-    }
-    if (headingError > 180.0f) {
-        headingError -= 360.0f;
-    }
-    // Error condition: wind speed is higher than airspeed. We are forced backwards!
-    fixedwingpathfollowerStatus.Errors.Wind = 0;
-    if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind ||
-         headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) &&
-        fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
-        // we are flying backwards
-        fixedwingpathfollowerStatus.Errors.Wind = 1;
-        result = 0;
-    }
-
     /**
      * Compute desired roll command
      */
-    if (groundspeedDesired > 1e-6f) {
-        course = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North));
-
-        courseError = course - heading;
-    } else {
-        // if we are not supposed to move, run in a circle
-        courseError = -90.0f;
-        result = 0;
-    }
+    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
 
     if (courseError < -180.0f) {
         courseError += 360.0f;
@@ -606,6 +590,19 @@ static uint8_t updateFixedDesiredAttitude()
         courseError -= 360.0f;
     }
 
+    // overlap calculation. Theres a dead zone behind the craft where the
+    // counter-yawing of some craft while rolling could render a desired right
+    // turn into a desired left turn. Making the turn direction based on
+    // current roll angle keeps the plane committed to a direction once chosen
+    if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll > 0.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll < 0.0f) {
+        courseError -= 360.0f;
+    }
+
     courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
                             -fixedwingpathfollowerSettings.CoursePI.ILimit,
                             fixedwingpathfollowerSettings.CoursePI.ILimit);
@@ -628,7 +625,7 @@ static uint8_t updateFixedDesiredAttitude()
      * Compute desired yaw command
      */
     // TODO implement raw control mode for yaw and base on Accels.Y
-    stabDesired.Yaw = 0;
+    stabDesired.Yaw = 0.0f;
 
 
     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
@@ -656,10 +653,116 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 
     AirspeedStateGet(&airspeedState);
     VelocityStateGet(&velocityState);
-    float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
+    float airspeedVector[2];
+    float yaw;
+    AttitudeStateYawGet(&yaw);
+    airspeedVector[0] = cos_lookup_deg(yaw);
+    airspeedVector[1] = sin_lookup_deg(yaw);
+    // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
+    float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
 
-
-    indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed;
-    // note - we do fly by Indicated Airspeed (== calibrated airspeed)
-    // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
+    indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
+    // note - we do fly by Indicated Airspeed (== calibrated airspeed) however
+    // since airspeed is updated less often than groundspeed, we use sudden
+    // 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.
+}
+
+
+/**
+ * Function to calculate course vector C based on airspeed s, fluid movement F
+ * and desired movement vector V
+ * parameters in: V,F,s
+ * parameters out: C
+ * returns true if a valid solution could be found for V,F,s, false if not
+ * C will be set to a best effort attempt either way
+ */
+static bool correctCourse(float *C, float *V, float *F, float s)
+{
+    // Approach:
+    // Let Sc be a circle around origin marking possible movement vectors
+    // of the craft with airspeed s (all possible options for C)
+    // Let Vl be a line through the origin along movement vector V where fr any
+    // point v on line Vl v = k * (V / |V|) = k' * V
+    // Let Wl be a line parallel to Vl where for any point v on line Vl exists
+    // a point w on WL with w = v - F
+    // Then any intersection between circle Sc and line Wl represents course
+    // vector which would result in a movement vector
+    // V' = k * ( V / |V|) = k' * V
+    // If there is no intersection point, S is insufficient to compensate
+    // for F and we can only try to fly in direction of V (thus having wind drift
+    // but at least making progress orthogonal to wind)
+
+    s = fabsf(s);
+    float f = sqrtf(F[0] * F[0] + F[1] * F[1]);
+
+    // normalize Cn=V/|V|, |V| must be >0
+    float v = sqrtf(V[0] * V[0] + V[1] * V[1]);
+    if (v < 1e-6f) {
+        // if |V|=0, we aren't supposed to move, turn into the wind
+        // (this allows hovering)
+        C[0] = -F[0];
+        C[1] = -F[1];
+        // if desired airspeed matches fluidmovement a hover is actually
+        // intended so return true
+        return fabsf(f - s) < 1e-3f;
+    }
+    float Vn[2] = { V[0] / v, V[1] / v };
+
+    // project F on V
+    float fp    = F[0] * Vn[0] + F[1] * Vn[1];
+
+    // find component Fo of F that is orthogonal to V
+    // (which is exactly the distance between Vl and Wl)
+    float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
+    float fo2   = Fo[0] * Fo[0] + Fo[1] * Fo[1];
+
+    // find k where k * Vn = C - Fo
+    // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
+    // so k^2 + fo^2 = s^2 (since |Vn|=1)
+    float k2 = s * s - fo2;
+    if (k2 <= -1e-3f) {
+        // there is no solution, we will be drifted off either way
+        // fallback: fly stupidly in direction of V and hope for the best
+        C[0] = V[0];
+        C[1] = V[1];
+        return false;
+    } else if (k2 <= 1e-3f) {
+        // there is exactly one solution: -Fo
+        C[0] = -Fo[0];
+        C[1] = -Fo[1];
+        return true;
+    }
+    // we have two possible solutions k positive and k negative as there are
+    // two intersection points between Wl and Sc
+    // which one is better? two criteria:
+    // 1. we MUST move in the right direction, if any k leads to -v its invalid
+    // 2. we should minimize the speed error
+    float k     = sqrt(k2);
+    float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
+    float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
+    // project C+F on Vn to find signed resulting movement vector length
+    float vp1   = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
+    float vp2   = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
+    if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
+        // in this case the angle between course and resulting movement vector
+        // is greater than 90 degrees - so we actually fly backwards
+        C[0] = C1[0];
+        C[1] = C1[1];
+        return true;
+    }
+    C[0] = C2[0];
+    C[1] = C2[1];
+    if (vp2 >= 0.0f) {
+        // in this case the angle between course and movement vector is less than
+        // 90 degrees, but we do move in the right direction
+        return true;
+    } else {
+        // in this case we actually get driven in the opposite direction of V
+        // with both solutions for C
+        // this might be reached in headwind stronger than maximum allowed
+        // airspeed.
+        return false;
+    }
 }
diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h
index 0d5c00d72..e3a1d6405 100644
--- a/flight/modules/ManualControl/inc/manualcontrol.h
+++ b/flight/modules/ManualControl/inc/manualcontrol.h
@@ -75,6 +75,18 @@ void pathFollowerHandler(bool newinit);
  */
 void pathPlannerHandler(bool newinit);
 
+/**
+ * @brief Handler to setup takeofflocation on arming. it is set up during Arming
+ * @input: NONE:
+ * @output: NONE
+ */
+void takeOffLocationHandler();
+
+/**
+ * @brief Initialize TakeoffLocationHanlder
+ */
+void takeOffLocationHandlerInit();
+
 /*
  * These are assumptions we make in the flight code about the order of settings and their consistency between
  * objects.  Please keep this synchronized to the UAVObjects
@@ -91,7 +103,7 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
         1 \
     )
@@ -108,7 +120,7 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
         1 \
     )
@@ -125,7 +137,7 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
         1 \
     )
@@ -142,7 +154,7 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
         1 \
     )
@@ -159,7 +171,7 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
         1 \
     )
@@ -176,7 +188,7 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
         1 \
     )
diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c
index 1bd29329f..64167f978 100644
--- a/flight/modules/ManualControl/manualcontrol.c
+++ b/flight/modules/ManualControl/manualcontrol.c
@@ -134,6 +134,9 @@ int32_t ManualControlStart()
     // Make sure unarmed on power up
     armHandler(true);
 
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    takeOffLocationHandlerInit();
+#endif
     // Start main task
     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 
@@ -167,7 +170,9 @@ static void manualControlTask(void)
 {
     // Process Arming
     armHandler(false);
-
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    takeOffLocationHandler();
+#endif
     // Process flight mode
     FlightStatusData flightStatus;
 
@@ -200,9 +205,13 @@ static void manualControlTask(void)
         break;
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV:
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS:
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW:
     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
     case FLIGHTSTATUS_FLIGHTMODE_LAND:
     case FLIGHTSTATUS_FLIGHTMODE_POI:
+    case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
         handler = &handler_PATHFOLLOWER;
         break;
     case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c
index 0f9bfe902..a0ba7d1e6 100644
--- a/flight/modules/ManualControl/pathfollowerhandler.c
+++ b/flight/modules/ManualControl/pathfollowerhandler.c
@@ -35,8 +35,9 @@
 #include <positionstate.h>
 #include <flightmodesettings.h>
 
-
 #if defined(REVOLUTION)
+#include <plans.h>
+
 // Private constants
 
 // Private types
@@ -51,69 +52,63 @@
 void pathFollowerHandler(bool newinit)
 {
     if (newinit) {
-        PathDesiredInitialize();
-        PositionStateInitialize();
+        plan_initialize();
     }
 
-    FlightStatusData flightStatus;
-    FlightStatusGet(&flightStatus);
+    uint8_t flightMode;
+    FlightStatusFlightModeGet(&flightMode);
 
     if (newinit) {
         // After not being in this mode for a while init at current height
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-        FlightModeSettingsData settings;
-        FlightModeSettingsGet(&settings);
-        PathDesiredData pathDesired;
-        PathDesiredGet(&pathDesired);
-        switch (flightStatus.FlightMode) {
+        switch (flightMode) {
         case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-            // Simple Return To Base mode - keep altitude the same, fly to home position
-
-
-            pathDesired.Start.North      = 0;
-            pathDesired.Start.East       = 0;
-            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.End.North        = 0;
-            pathDesired.End.East         = 0;
-            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            plan_setup_returnToBase();
             break;
-        default:
 
-            pathDesired.Start.North      = positionState.North;
-            pathDesired.Start.East       = positionState.East;
-            pathDesired.Start.Down       = positionState.Down;
-            pathDesired.End.North        = positionState.North;
-            pathDesired.End.East         = positionState.East;
-            pathDesired.End.Down         = positionState.Down;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-            /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
-               } else {
-               PathDesiredData pathDesired;
-               PathDesiredGet(&pathDesired);
-               pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
-               pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
-               pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-               PathDesiredSet(&pathDesired);
-             */
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+            plan_setup_positionHold();
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV:
+            plan_setup_PositionVarioFPV();
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS:
+            plan_setup_PositionVarioLOS();
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW:
+            plan_setup_PositionVarioNSEW();
+            break;
+
+        case FLIGHTSTATUS_FLIGHTMODE_LAND:
+            plan_setup_land();
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
+            plan_setup_AutoCruise();
+            break;
+
+        default:
+            plan_setup_positionHold();
             break;
         }
-        PathDesiredSet(&pathDesired);
     }
 
-    // special handling of autoland - behaves like positon hold but with slow altitude decrease
-    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
-        PositionStateData positionState;
-        PositionStateGet(&positionState);
-        PathDesiredData pathDesired;
-        PathDesiredGet(&pathDesired);
-        pathDesired.End.Down = positionState.Down + 5;
-        PathDesiredSet(&pathDesired);
+    switch (flightMode) {
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV:
+        plan_run_PositionVarioFPV();
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS:
+        plan_run_PositionVarioLOS();
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW:
+        plan_run_PositionVarioNSEW();
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
+        plan_run_land();
+        break;
+    case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
+        plan_run_AutoCruise();
+        break;
+    default:
+        break;
     }
 }
 
diff --git a/flight/modules/ManualControl/takeofflocationhandler.c b/flight/modules/ManualControl/takeofflocationhandler.c
new file mode 100644
index 000000000..42a10777d
--- /dev/null
+++ b/flight/modules/ManualControl/takeofflocationhandler.c
@@ -0,0 +1,125 @@
+/**
+ ******************************************************************************
+ *
+ * @file       takeofflocationhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      handles TakeOffLocation
+ *             --
+ * @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
+ */
+#include "inc/manualcontrol.h"
+#include <stdint.h>
+#include <flightstatus.h>
+#include <takeofflocation.h>
+#include <positionstate.h>
+
+// Private constants
+
+// Private types
+
+// Private variables
+static bool locationSet;
+// Private functions
+static void SetTakeOffLocation();
+
+void takeOffLocationHandlerInit()
+{
+    TakeOffLocationInitialize();
+    // check whether there is a preset/valid takeoff location
+    uint8_t mode;
+    uint8_t status;
+    TakeOffLocationModeGet(&mode);
+    TakeOffLocationStatusGet(&status);
+    // preset with invalid location will actually behave like FirstTakeoff
+    if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
+        locationSet = true;
+    } else {
+        locationSet = false;
+        status = TAKEOFFLOCATION_STATUS_INVALID;
+        TakeOffLocationStatusSet(&status);
+    }
+}
+/**
+ * Handles TakeOffPosition location setup
+ * @param newinit
+ */
+void takeOffLocationHandler()
+{
+    uint8_t armed;
+    uint8_t status;
+
+    FlightStatusArmedGet(&armed);
+
+    // Location already acquired/preset
+    if (armed == FLIGHTSTATUS_ARMED_ARMED && locationSet) {
+        return;
+    }
+
+    TakeOffLocationStatusGet(&status);
+
+    switch (armed) {
+    case FLIGHTSTATUS_ARMED_ARMING:
+    case FLIGHTSTATUS_ARMED_ARMED:
+        if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
+            uint8_t mode;
+            TakeOffLocationModeGet(&mode);
+
+            if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
+                SetTakeOffLocation();
+            } else {
+                locationSet = true;
+            }
+        }
+        break;
+
+    case FLIGHTSTATUS_ARMED_DISARMED:
+        // unset if location is to be acquired at each arming
+        if (locationSet) {
+            uint8_t mode;
+            TakeOffLocationModeGet(&mode);
+            if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
+                locationSet = false;
+                status = TAKEOFFLOCATION_STATUS_INVALID;
+                TakeOffLocationStatusSet(&status);
+            }
+        }
+        break;
+    }
+}
+
+/**
+ * Retrieve TakeOffLocation from current PositionStatus
+ */
+void SetTakeOffLocation()
+{
+    TakeOffLocationData takeOffLocation;
+
+    TakeOffLocationGet(&takeOffLocation);
+
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+
+    takeOffLocation.North  = positionState.North;
+    takeOffLocation.East   = positionState.East;
+    takeOffLocation.Down   = positionState.Down;
+    takeOffLocation.Status = TAKEOFFLOCATION_STATUS_VALID;
+
+    TakeOffLocationSet(&takeOffLocation);
+    locationSet = true;
+}
diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c
index 8cc50c4aa..c25d3a0b7 100644
--- a/flight/modules/PathPlanner/pathplanner.c
+++ b/flight/modules/PathPlanner/pathplanner.c
@@ -45,6 +45,7 @@
 #include "flightmodesettings.h"
 #include <pios_struct_helper.h>
 #include "paths.h"
+#include "plans.h"
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -89,6 +90,7 @@ static bool pathplanner_active = false;
  */
 int32_t PathPlannerStart()
 {
+    plan_initialize();
     // when the active waypoint changes, update pathDesired
     WaypointConnectCallback(commandUpdated);
     WaypointActiveConnectCallback(commandUpdated);
@@ -169,21 +171,7 @@ static void pathPlannerTask()
         if (!failsafeRTHset) {
             failsafeRTHset = 1;
             // copy pasta: same calculation as in manualcontrol, set return to home coordinates
-            PositionStateData positionState;
-            PositionStateGet(&positionState);
-            FlightModeSettingsData settings;
-            FlightModeSettingsGet(&settings);
-
-            pathDesired.Start.North      = 0;
-            pathDesired.Start.East       = 0;
-            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.End.North        = 0;
-            pathDesired.End.East         = 0;
-            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-            PathDesiredSet(&pathDesired);
+            plan_setup_positionHold();
         }
         AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
 
diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c
index c1c76ee8f..bd35945fb 100644
--- a/flight/modules/Sensors/sensors.c
+++ b/flight/modules/Sensors/sensors.c
@@ -62,6 +62,7 @@
 #include <CoordinateConversions.h>
 
 #include <pios_board_info.h>
+#include <pios_struct_helper.h>
 
 // Private constants
 #define STACK_SIZE_BYTES 1000
@@ -474,35 +475,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
     RPY2Quaternion(rpy, rotationQuat);
     Quaternion2R(rotationQuat, R);
 
-    mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
-                          R[1][0] * cal.mag_transform.r0c1 +
-                          R[2][0] * cal.mag_transform.r0c2;
-    mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
-                          R[1][1] * cal.mag_transform.r0c1 +
-                          R[2][1] * cal.mag_transform.r0c2;
-    mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
-                          R[1][2] * cal.mag_transform.r0c1 +
-                          R[2][2] * cal.mag_transform.r0c2;
-
-    mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
-                          R[1][0] * cal.mag_transform.r1c1 +
-                          R[2][0] * cal.mag_transform.r1c2;
-    mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
-                          R[1][1] * cal.mag_transform.r1c1 +
-                          R[2][1] * cal.mag_transform.r1c2;
-    mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
-                          R[1][2] * cal.mag_transform.r1c1 +
-                          R[2][2] * cal.mag_transform.r1c2;
-
-    mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
-                          R[1][0] * cal.mag_transform.r2c1 +
-                          R[2][0] * cal.mag_transform.r2c2;
-    mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
-                          R[1][1] * cal.mag_transform.r2c1 +
-                          R[2][1] * cal.mag_transform.r2c2;
-    mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
-                          R[1][2] * cal.mag_transform.r2c1 +
-                          R[2][2] * cal.mag_transform.r2c2;
+    matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
 }
 /**
  * @}
diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c
index d80c00185..b5ffb4d35 100644
--- a/flight/modules/Stabilization/outerloop.c
+++ b/flight/modules/Stabilization/outerloop.c
@@ -247,7 +247,7 @@ static void stabilizationOuterloopTask()
             case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
                 rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
                 break;
-            case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY:
+            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
                 rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
                 break;
 #endif /* REVOLUTION */
diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c
index 15b8c3caf..b9ff0541a 100644
--- a/flight/modules/Stabilization/stabilization.c
+++ b/flight/modules/Stabilization/stabilization.c
@@ -174,8 +174,8 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
             cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
             cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
             break;
-        case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY:
-            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY;
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
             cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
             break;
         case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile
index 8b14a385e..7472d176a 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/Makefile
+++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile
@@ -82,6 +82,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+	SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 
diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
index 44c43d080..3905af42f 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
+++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
@@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
 UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile
index 88b8dd6b6..8c3337a1a 100644
--- a/flight/targets/boards/revolution/firmware/Makefile
+++ b/flight/targets/boards/revolution/firmware/Makefile
@@ -84,6 +84,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+    SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 
diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc
index 44c43d080..3905af42f 100644
--- a/flight/targets/boards/revolution/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc
@@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
 UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile
index affe5a570..39d2d7f3b 100644
--- a/flight/targets/boards/revoproto/firmware/Makefile
+++ b/flight/targets/boards/revoproto/firmware/Makefile
@@ -83,6 +83,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+    SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 
diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
index 44c43d080..3905af42f 100644
--- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
@@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
 UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile
index 8d77fd4f1..9eaa00725 100644
--- a/flight/targets/boards/simposix/firmware/Makefile
+++ b/flight/targets/boards/simposix/firmware/Makefile
@@ -94,6 +94,7 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
 SRC += $(FLIGHTLIB)/insgps13state.c
 SRC += $(FLIGHTLIB)/paths.c
+SRC += $(FLIGHTLIB)/plans.c
 SRC += $(FLIGHTLIB)/sanitycheck.c
 
 SRC += $(MATHLIB)/sin_lookup.c
diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc
index 724adc0a6..907ff6833 100644
--- a/flight/targets/boards/simposix/firmware/UAVObjects.inc
+++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc
@@ -111,6 +111,7 @@ UAVOBJSRCFILENAMES += revosettings
 UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += ekfconfiguration
 UAVOBJSRCFILENAMES += ekfstatevariance
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml
index bf4e7ccbc..88032dc41 100644
--- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml
+++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml
@@ -2582,7 +2582,7 @@
             <type>splitter</type>
           </side1>
           <splitterOrientation>2</splitterOrientation>
-          <splitterSizes>@Variant(AAAACQAAAAIAAAACAAAB7wAAAAIAAAEf)</splitterSizes>
+          <splitterSizes>@Variant(AAAACQAAAAIAAAACAAABqQAAAAIAAADm)</splitterSizes>
           <type>splitter</type>
         </side0>
         <side1>
@@ -2759,7 +2759,7 @@
               <type>uavGadget</type>
             </side1>
             <splitterOrientation>1</splitterOrientation>
-            <splitterSizes>@Variant(AAAACQAAAAIAAAACAAABQgAAAAIAAAGM)</splitterSizes>
+            <splitterSizes>@Variant(AAAACQAAAAIAAAACAAAAxwAAAAIAAAFi)</splitterSizes>
             <type>splitter</type>
           </side0>
           <side1>
diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
index 884c78717..2a65f1202 100644
--- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
+++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
@@ -10,7 +10,7 @@
    xmlns:xlink="http://www.w3.org/1999/xlink"
    xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
    xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
-   width="94.2584"
+   width="113.03149"
    height="79.57505"
    id="svg3604"
    version="1.1"
@@ -27,10 +27,10 @@
      borderopacity="1.0"
      inkscape:pageopacity="0.0"
      inkscape:pageshadow="2"
-     inkscape:zoom="29.851822"
-     inkscape:cx="82.604436"
-     inkscape:cy="54.252252"
-     inkscape:current-layer="background"
+     inkscape:zoom="5.2771064"
+     inkscape:cx="50.100274"
+     inkscape:cy="37.563212"
+     inkscape:current-layer="layer36"
      id="namedview3608"
      showgrid="true"
      inkscape:window-width="1366"
@@ -40,7 +40,8 @@
      inkscape:window-maximized="1"
      showguides="true"
      inkscape:guide-bbox="true"
-     inkscape:snap-global="false">
+     inkscape:snap-global="false"
+     units="mm">
     <sodipodi:guide
        id="guide3857"
        position="68.372091,-63.708224"
@@ -59,10 +60,14 @@
        orientation="0,1" />
     <inkscape:grid
        type="xygrid"
-       id="grid4540" />
+       id="grid4540"
+       empspacing="5"
+       visible="true"
+       enabled="true"
+       snapvisiblegridlinesonly="true" />
     <sodipodi:guide
        orientation="0,1"
-       position="86.912617,26.949779"
+       position="78.262587,27.098184"
        id="guide4221" />
   </sodipodi:namedview>
   <defs
@@ -693,419 +698,391 @@
      inkscape:groupmode="layer"
      inkscape:label="Background">
     <rect
-       ry="1.6285406"
-       y="344.53958"
-       x="497.92484"
-       height="79.056633"
-       width="95.066467"
+       ry="1.6267107"
+       y="344.58401"
+       x="497.96927"
+       height="78.967804"
+       width="112.71937"
        id="Background"
-       style="fill:#483737;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+       style="fill:#918a6f;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.09500301;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+    <rect
+       style="fill:#333333;fill-opacity:1;stroke:none;display:inline"
+       id="rect1234-5-1"
+       width="40.113979"
+       height="13.206578"
+       x="569.19208"
+       y="380.45892"
+       ry="0" />
+    <rect
+       style="fill:#333333;fill-opacity:1;stroke:none;display:inline"
+       id="rect1234-9-3"
+       width="62.110886"
+       height="13.206578"
+       x="547.19519"
+       y="394.70282"
+       ry="0" />
+    <rect
+       style="fill:#323232;fill-opacity:1;stroke:none;display:inline"
+       id="rect4358-0"
+       width="109.66098"
+       height="13.853112"
+       x="499.64511"
+       y="345.93436"
+       ry="0" />
+    <rect
+       style="fill:#333333;fill-opacity:1;stroke:none;display:inline"
+       id="rect1234-5"
+       width="68.523476"
+       height="13.206578"
+       x="499.64508"
+       y="380.45892"
+       ry="0" />
+    <rect
+       style="fill:#333333;fill-opacity:1;stroke:none;display:inline"
+       id="rect1234-9"
+       width="46.00037"
+       height="13.206578"
+       x="499.64508"
+       y="394.70282"
+       ry="0" />
+    <rect
+       style="fill:#323232;fill-opacity:1;stroke:none;display:inline"
+       id="rect4358-0-7"
+       width="109.64501"
+       height="13.853112"
+       x="499.66107"
+       y="360.58862"
+       ry="0" />
     <rect
        style="fill:#241c1c;fill-opacity:1;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
        id="rect4550-8-7-82"
        width="10.470912"
        height="8.5405388"
        x="547.97754"
-       y="411.38937"
+       y="411.27975"
        ry="1" />
     <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none"
+       style="fill:#333333;fill-opacity:1;stroke:none"
        id="rect1234"
-       width="63.478992"
-       height="12.219843"
-       x="527.70123"
-       y="409.64447"
+       width="109.5812"
+       height="13.206578"
+       x="499.72488"
+       y="408.94675"
        ry="0" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none"
-       id="Receiver"
-       width="19.129522"
-       height="10.639811"
-       x="504.99179"
-       y="387.5629"
-       ry="0.24124773"
-       inkscape:label="#rect4550" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect4358-7"
-       width="37.799622"
-       height="13.255064"
-       x="553.24451"
-       y="395.2457"
-       ry="0" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect4358-4"
-       width="63.468098"
-       height="12.839675"
-       x="527.57617"
-       y="381.38202"
-       ry="0" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect4358-0"
-       width="91.066727"
-       height="13.183137"
-       x="499.96637"
-       y="346.05209"
-       ry="0" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect4358-0-9"
-       width="91.019348"
-       height="13.183137"
-       x="500.06686"
-       y="360.27713"
-       ry="0" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect4358-7-4"
-       width="24.646557"
-       height="13.221018"
-       x="527.58087"
-       y="395.29047"
-       ry="0" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="CPUOverload"
-       width="10.470912"
-       height="8.5405388"
-       x="532.78839"
-       y="411.4689"
-       ry="1"
+       width="19.142458"
+       height="9.4961557"
+       x="588.72668"
+       y="410.80194"
+       ry="1.111892"
        inkscape:label="#rect4550-8" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="OutOfMemory"
-       width="10.470912"
-       height="8.5405388"
-       x="544.5553"
-       y="411.4689"
-       ry="1"
-       inkscape:label="#rect4550-8-7" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="StackOverflow"
-       width="10.470912"
-       height="8.5405388"
-       x="556.32214"
-       y="411.4689"
-       ry="1"
-       inkscape:label="#rect4550-8-1" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="EventSystem"
-       width="10.470912"
-       height="8.5405388"
-       x="568.08905"
-       y="411.4689"
-       ry="1"
-       inkscape:label="#rect4550-8-15" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="I2C"
-       width="10.470912"
-       height="8.5405388"
-       x="579.8559"
-       y="411.4689"
-       ry="1"
-       inkscape:label="#rect4550-8-2" />
     <text
        xml:space="preserve"
        style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="521.74384"
-       y="379.36826"
+       x="529.24725"
+       y="379.43469"
        id="text5334"
        sodipodi:linespacing="125%"><tspan
          sodipodi:role="line"
          id="tspan4263"
-         x="521.74384"
-         y="379.36826">SYSTEM HEALTH</tspan></text>
+         x="529.24725"
+         y="379.43469">SYSTEM HEALTH</tspan></text>
     <text
        xml:space="preserve"
        style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-426.01138"
-       y="440.49539"
+       x="-426.10474"
+       y="440.35391"
        id="text4641-5-2"
        sodipodi:linespacing="125%"
        transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
          sodipodi:role="line"
-         id="tspan5401"
-         x="-426.01138"
-         y="440.49539">Sensors</tspan></text>
+         id="tspan5393"
+         x="-426.10474"
+         y="440.35391">SENSR</tspan></text>
     <text
        xml:space="preserve"
        style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-406.95456"
-       y="440.47641"
+       x="-408.18991"
+       y="440.35474"
        id="text4641-5-2-5"
        sodipodi:linespacing="125%"
        transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
          sodipodi:role="line"
-         id="tspan5424"
-         x="-406.95456"
-         y="440.47641">Auto</tspan></text>
+         id="tspan5375"
+         x="-408.18991"
+         y="440.35474">AUTO</tspan></text>
     <text
        xml:space="preserve"
        style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-447.34424"
-       y="464.44702"
+       x="-463.11954"
+       y="481.81909"
        id="text4641-5-2-1"
        sodipodi:linespacing="125%"
        transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
          sodipodi:role="line"
-         id="tspan5435"
-         x="-447.34424"
-         y="464.44702">Misc.</tspan></text>
+         id="tspan5383"
+         x="-463.11954"
+         y="481.81909">MISC</tspan></text>
     <text
        xml:space="preserve"
        style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-462.94199"
-       y="464.46683"
-       id="text4641-5-2-11"
-       sodipodi:linespacing="125%"
-       transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
-         sodipodi:role="line"
-         id="tspan5446"
-         x="-462.94199"
-         y="464.46683">Link</tspan></text>
-    <text
-       xml:space="preserve"
-       style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-464.57455"
-       y="486.72632"
+       x="-462.91629"
+       y="440.35474"
        id="text4641-5-2-2"
        sodipodi:linespacing="125%"
        transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
          sodipodi:role="line"
-         id="tspan5469"
-         x="-464.57455"
-         y="486.72632">Power</tspan></text>
+         id="tspan5387"
+         x="-462.91629"
+         y="440.35474">PWR</tspan></text>
     <text
        xml:space="preserve"
        style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-478.51532"
-       y="464.46683"
+       x="-478.508"
+       y="440.35391"
        id="text4641-5-2-6"
        sodipodi:linespacing="125%"
        transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
          sodipodi:role="line"
-         id="tspan4340"
-         x="-478.51532"
-         y="464.46683">Syst.</tspan></text>
+         id="tspan5389"
+         x="-478.508"
+         y="440.35391">SYS</tspan></text>
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="BootFault"
-       width="13.310432"
-       height="10.308098"
-       x="546.92853"
-       y="382.58969"
-       ry="1"
-       inkscape:label="#rect4550-8-1-4-21-8" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="rect4550-8-1-4-21-9"
-       width="13.310432"
-       height="10.308098"
-       x="561.6792"
-       y="382.58969"
-       ry="1" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="rect41234"
-       width="13.310432"
-       height="10.308098"
-       x="576.42981"
-       y="382.52267"
-       ry="1.3167187"
-       inkscape:label="#rect4550-8-1-4-21-2" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="FlightTime"
-       width="15.447426"
-       height="10.265867"
-       x="574.46124"
-       y="396.60678"
-       ry="1"
+       width="18.060333"
+       height="10.19887"
+       x="525.18445"
+       y="396.2067"
+       ry="0.99347377"
        inkscape:label="#rect4550-8-1-4-21-2-9" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Telemetry"
-       width="18.966711"
-       height="10.202584"
-       x="532.13593"
-       y="396.63843"
-       ry="1"
+       width="33.123543"
+       height="9.9680929"
+       x="574.63525"
+       y="382.0614"
+       ry="0.97701645"
        inkscape:label="#rect4550-8-1-4-21-5" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Attitude"
-       width="18.966711"
-       height="10.202584"
-       x="505.26666"
-       y="347.495"
-       ry="1"
+       width="23.782648"
+       height="10.03509"
+       x="504.74219"
+       y="347.8266"
+       ry="0.98358321"
        inkscape:label="#rect4550-8-1-4-21-5-4" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Stabilization"
-       width="18.966711"
-       height="10.202584"
-       x="527.08551"
-       y="347.495"
-       ry="1"
+       width="23.804651"
+       height="10.068589"
+       x="530.97595"
+       y="347.79312"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-3" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Guidance"
-       width="18.966711"
-       height="10.202584"
-       x="548.90442"
-       y="347.495"
-       ry="1"
+       width="23.83815"
+       height="10.068589"
+       x="557.11407"
+       y="347.8266"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-1" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="PathPlan"
-       width="18.966711"
-       height="10.202584"
-       x="570.72327"
-       y="347.495"
-       ry="1"
-       inkscape:label="#rect4550-8-1-4-21-5-2" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="GPS"
-       width="18.966711"
-       height="10.202584"
-       x="505.26666"
-       y="361.65591"
-       ry="1"
+       width="23.788395"
+       height="10.102088"
+       x="504.7757"
+       y="362.44739"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Sensors"
-       width="18.966711"
-       height="10.202584"
-       x="527.08551"
-       y="361.65591"
-       ry="1"
+       width="23.8104"
+       height="10.102088"
+       x="530.96442"
+       y="362.44739"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-41" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Airspeed"
-       width="18.966711"
-       height="10.202584"
-       x="548.90442"
-       y="361.65591"
-       ry="1"
+       width="23.751532"
+       height="10.102088"
+       x="557.14178"
+       y="362.48087"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-13" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Magnetometer"
-       width="18.966711"
-       height="10.202584"
-       x="570.72327"
-       y="361.65591"
-       ry="1"
+       width="23.793158"
+       height="10.102088"
+       x="584.00055"
+       y="362.44739"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-8" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="Battery"
-       width="15.447426"
-       height="10.265867"
-       x="557.75928"
-       y="396.60678"
-       ry="1"
+       width="18.0655"
+       height="10.232368"
+       x="504.83118"
+       y="396.17319"
+       ry="0.99673688"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
        id="SystemConfiguration"
-       width="13.310432"
-       height="10.308098"
-       x="532.17792"
-       y="382.58969"
-       ry="1"
+       width="23.271717"
+       height="10.107105"
+       x="552.43744"
+       y="396.21909"
+       ry="0.98050147"
        inkscape:label="#rect4550-8-1-4-21" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect1234-1"
-       width="26.147932"
-       height="27.124466"
-       x="500.06686"
-       y="381.38202"
-       ry="0" />
     <text
        xml:space="preserve"
        style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
-       x="-453.88235"
-       y="440.49704"
+       x="-444.87003"
+       y="440.35474"
        id="text4641-5-2-11-0"
        sodipodi:linespacing="125%"
        transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
          sodipodi:role="line"
          id="tspan3467"
-         x="-453.88235"
-         y="440.49704">I/O</tspan></text>
+         x="-444.87003"
+         y="440.35474">I/O</tspan></text>
     <rect
        style="fill:#6c5353;fill-opacity:1;stroke:#6c5d53;stroke-width:0.66968507;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
        id="rect3510"
        width="1.94293"
        height="7.3697343"
-       x="12.260558"
-       y="54.383957"
-       transform="translate(497.66563,344.28037)" />
+       x="531.36542"
+       y="383.37735" />
     <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="Receiver-OK-2"
-       width="19.129522"
-       height="10.639811"
-       x="505.12875"
-       y="382.42383"
-       ry="1.0787175"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
+       id="Receiver"
+       width="23.149378"
+       height="10.137329"
+       x="504.79376"
+       y="381.97681"
+       ry="1.0277734"
        inkscape:label="#rect4550"
-       rx="1.1054602" />
-    <rect
-       style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline"
-       id="rect1234-1-1"
-       width="26.147932"
-       height="12.217504"
-       x="500.06686"
-       y="409.64682"
-       ry="0" />
-    <rect
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="rect"
-       width="19.129522"
-       height="10.639811"
-       x="504.98425"
-       y="410.36182"
-       ry="1.0787175"
-       inkscape:label="#rect4550"
-       rx="1.1054602" />
+       rx="1.3377603" />
     <rect
        inkscape:label="#rect4550-8-1-4-21-5-33"
        ry="0.98606986"
-       y="396.92505"
-       x="505.28799"
+       y="382.03198"
+       x="530.21112"
        height="10.060461"
-       width="18.729839"
+       width="23.084681"
        id="Actuator"
-       style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline" />
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline" />
+    <rect
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
+       id="OutOfMemory"
+       width="19.07546"
+       height="9.5296545"
+       x="525.68011"
+       y="410.76846"
+       ry="1.1158142"
+       inkscape:label="#rect4550-8" />
+    <rect
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
+       id="StackOverflow"
+       width="19.07546"
+       height="9.4961557"
+       x="546.75079"
+       y="410.80194"
+       ry="1.111892"
+       inkscape:label="#rect4550-8" />
+    <rect
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
+       id="EventSystem"
+       width="19.07546"
+       height="9.5296545"
+       x="567.6875"
+       y="410.76846"
+       ry="1.1158142"
+       inkscape:label="#rect4550-8" />
+    <rect
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
+       id="BootFault"
+       width="19.108959"
+       height="9.5296545"
+       x="504.70978"
+       y="410.73495"
+       ry="1.1158142"
+       inkscape:label="#rect4550-8" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-33"
+       ry="1.0047177"
+       y="396.14725"
+       x="578.25714"
+       height="10.250717"
+       width="13.477478"
+       id="rect-1"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline" />
+    <rect
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline"
+       id="PathPlan"
+       width="23.883144"
+       height="10.135587"
+       x="583.93506"
+       y="347.75961"
+       ry="0.9934333"
+       inkscape:label="#rect4550-8-1-4-21-5-1" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-33"
+       ry="1.008266"
+       y="381.91876"
+       x="555.55225"
+       height="10.286918"
+       width="11.346323"
+       id="I2C"
+       style="fill:#28170b;stroke:none;display:inline" />
+    <text
+       xml:space="preserve"
+       style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold"
+       x="-446.64218"
+       y="501.22174"
+       id="text4641-5-2-1-5"
+       sodipodi:linespacing="125%"
+       transform="matrix(0,-0.87458357,1.1434013,0,0,0)"><tspan
+         sodipodi:role="line"
+         id="tspan5472"
+         x="-446.64218"
+         y="501.22174">LINK</tspan></text>
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-33"
+       ry="1.0080011"
+       y="396.14725"
+       x="594.28265"
+       height="10.284216"
+       width="13.544476"
+       id="rect-2"
+       style="fill:#28170b;fill-opacity:1;stroke:none;display:inline" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer1"
      inkscape:label="GPS-OK"
-     style="display:inline">
+     style="display:none">
     <rect
-       transform="translate(-497.66563,-344.28037)"
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="GPS-OK"
-       width="18.966711"
+       width="23.804651"
        height="10.202584"
-       x="505.26666"
-       y="361.65591"
+       x="7.0741663"
+       y="18.133535"
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
@@ -1113,15 +1090,15 @@
      inkscape:groupmode="layer"
      id="layer11"
      inkscape:label="Airspeed-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Airspeed-OK"
-       width="18.966711"
-       height="10.202584"
-       x="51.238789"
-       y="17.375544"
-       ry="1"
+       width="23.790537"
+       height="10.102088"
+       x="59.881477"
+       y="18.179514"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
@@ -1132,70 +1109,70 @@
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Magnetometer-OK"
-       width="18.966711"
-       height="10.202584"
-       x="73.057671"
-       y="17.391815"
-       ry="1"
+       width="23.824036"
+       height="10.135587"
+       x="86.323196"
+       y="18.162287"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer10"
      inkscape:label="Sensors-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Sensors-OK"
-       width="18.966711"
-       height="10.202584"
-       x="29.41988"
-       y="17.375544"
-       ry="1"
+       width="23.857534"
+       height="10.135587"
+       x="33.506733"
+       y="18.146015"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer9"
      inkscape:label="PathPlan-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="PathPlan-OK"
-       width="18.966711"
-       height="10.202584"
-       x="73.05764"
-       y="3.2146251"
-       ry="1"
+       width="23.857534"
+       height="10.102088"
+       x="86.289665"
+       y="3.5161142"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer8"
      inkscape:label="Guidance-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Guidance-OK"
-       width="18.966711"
-       height="10.202584"
-       x="51.238789"
-       y="3.2146251"
-       ry="1"
+       width="23.824036"
+       height="10.135587"
+       x="59.847977"
+       y="3.5161142"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer7"
      inkscape:label="Stabilization-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Stabilization-OK"
-       width="18.966711"
+       width="23.824036"
        height="10.202584"
-       x="29.41988"
-       y="3.2146251"
+       x="33.506733"
+       y="3.4826155"
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
@@ -1203,88 +1180,88 @@
      inkscape:groupmode="layer"
      id="layer6"
      inkscape:label="Attitude-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Attitude-OK"
-       width="18.966711"
-       height="10.202584"
-       x="7.6010327"
-       y="3.2146251"
-       ry="1"
+       width="23.824036"
+       height="10.135587"
+       x="7.0985508"
+       y="3.4826155"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer12"
      inkscape:label="SystemConfiguration-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="SystemConfiguration-OK"
-       width="13.310432"
-       height="10.308098"
-       x="34.512287"
-       y="38.309322"
-       ry="1"
+       width="23.259575"
+       height="10.107105"
+       x="54.779057"
+       y="51.943333"
+       ry="0.98050147"
        inkscape:label="#rect4550-8-1-4-21-1" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer13"
      inkscape:label="BootFault-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="BootFault-OK"
-       width="13.310432"
-       height="10.308098"
-       x="49.262897"
-       y="38.309322"
-       ry="1" />
+       width="19.072226"
+       height="9.5041265"
+       x="7.0544181"
+       y="66.481804"
+       ry="0.92200589" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer15"
      inkscape:label="Battery-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Battery-OK"
-       width="15.447426"
-       height="10.265867"
-       x="60.093647"
-       y="52.326412"
-       ry="1"
+       width="18.026833"
+       height="10.165371"
+       x="7.1983929"
+       y="51.953167"
+       ry="0.99021065"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer14"
      inkscape:label="Telemetry-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Telemetry-OK"
-       width="18.966711"
-       height="10.202584"
-       x="34.470295"
-       y="52.358059"
-       ry="1"
+       width="33.036205"
+       height="10.03509"
+       x="77.013763"
+       y="37.752586"
+       ry="0.98358321"
        inkscape:label="#rect4550-8-1-4-21-5" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer16"
      inkscape:label="FlightTime-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="FlightTime-OK"
-       width="15.447426"
+       width="18.060331"
        height="10.265867"
-       x="76.795616"
-       y="52.326412"
+       x="27.518892"
+       y="51.890926"
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
   </g>
@@ -1292,123 +1269,122 @@
      inkscape:groupmode="layer"
      id="layer21"
      inkscape:label="I2C-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="I2C-OK"
-       width="10.470912"
-       height="8.5405388"
-       x="82.190269"
-       y="67.18853"
-       ry="1"
+       width="11.341881"
+       height="10.315975"
+       x="57.870148"
+       y="37.609093"
+       ry="1.2078834"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer20"
      inkscape:label="EventSystem-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="EventSystem-OK"
-       width="10.470912"
-       height="8.5405388"
-       x="70.423416"
-       y="67.18853"
-       ry="1"
+       width="19.046602"
+       height="9.5455027"
+       x="70.088425"
+       y="66.485054"
+       ry="1.1176698"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer19"
      inkscape:label="StackOverflow-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="StackOverflow-OK"
-       width="10.470912"
-       height="8.5405388"
-       x="58.656513"
-       y="67.18853"
-       ry="1"
+       width="19.046602"
+       height="9.5790014"
+       x="49.089085"
+       y="66.42955"
+       ry="1.1215922"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer18"
      inkscape:label="OutOfMemory-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="OutOfMemory-OK"
-       width="10.470912"
-       height="8.5405388"
-       x="46.889668"
-       y="67.18853"
-       ry="1"
+       width="19.080101"
+       height="9.4785051"
+       x="28.080856"
+       y="66.493179"
+       ry="1.1098251"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer17"
      inkscape:label="CPUOverload-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="CPUOverload-OK"
-       width="10.470912"
-       height="8.5405388"
-       x="35.122761"
-       y="67.18853"
-       ry="1"
+       width="19.147099"
+       height="9.4785051"
+       x="91.072304"
+       y="66.503593"
+       ry="1.1098251"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer22"
      inkscape:label="Receiver-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Receiver-OK"
-       width="18.963711"
-       height="10.260815"
-       x="7.6226416"
-       y="38.370422"
-       ry="0.99888694"
+       width="23.184559"
+       height="10.160318"
+       x="7.0866609"
+       y="37.700447"
+       ry="0.98910367"
        inkscape:label="#rect4550"
-       rx="0.88121402" />
+       rx="1.0773503" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer23"
      inkscape:label="Actuator-OK"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
        id="Actuator-OK"
-       width="18.905607"
+       width="23.26045"
        height="10.044919"
-       x="7.6230011"
-       y="52.617489"
+       x="32.445606"
+       y="37.744026"
        ry="1.112498"
        inkscape:label="#rect4550"
-       rx="0.86747229" />
+       rx="1.0672916" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5041"
      inkscape:label="GPS-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
-       transform="translate(-497.66563,-344.28037)"
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="GPS-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="505.26666"
-       y="361.65591"
-       ry="1"
+       width="23.790537"
+       height="10.068589"
+       x="7.1320496"
+       y="18.213015"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
@@ -1419,11 +1395,11 @@
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Airspeed-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="51.238789"
-       y="17.375544"
-       ry="1"
+       width="23.824036"
+       height="10.068589"
+       x="59.881477"
+       y="18.213013"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
@@ -1434,252 +1410,252 @@
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Magnetometer-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="73.091171"
-       y="17.425316"
-       ry="1"
+       width="23.790537"
+       height="10.135587"
+       x="86.323196"
+       y="18.162289"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5047"
      inkscape:label="Sensors-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Sensors-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="29.41988"
-       y="17.375544"
-       ry="1"
+       width="23.824036"
+       height="10.068589"
+       x="33.506733"
+       y="18.213015"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5050"
      inkscape:label="PathPlan-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="PathPlan-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="73.05764"
-       y="3.2146251"
-       ry="1"
+       width="23.857534"
+       height="10.135587"
+       x="86.261238"
+       y="3.503633"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5053"
      inkscape:label="Guidance-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Guidance-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="51.238789"
-       y="3.2146251"
-       ry="1"
+       width="23.857534"
+       height="10.068589"
+       x="59.845669"
+       y="3.5428796"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5056"
      inkscape:label="Stabilization-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Stabilization-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="29.41988"
-       y="3.2146251"
-       ry="1"
+       width="23.824036"
+       height="10.169086"
+       x="33.448105"
+       y="3.4873767"
+       ry="0.99671662"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5059"
      inkscape:label="Attitude-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Attitude-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="7.6010327"
-       y="3.2146251"
-       ry="1"
+       width="23.790537"
+       height="10.102088"
+       x="7.0985508"
+       y="3.5161142"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5062"
      inkscape:label="SystemConfiguration-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="SystemConfiguration-Warning"
-       width="13.310432"
-       height="10.308098"
-       x="34.512287"
-       y="38.309322"
-       ry="1"
+       width="23.326572"
+       height="10.140604"
+       x="54.759811"
+       y="51.958912"
+       ry="0.98375124"
        inkscape:label="#rect4550-8-1-4-21-1" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5065"
      inkscape:label="BootFault-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="BootFault-Warning"
-       width="13.310432"
-       height="10.308098"
-       x="49.262897"
-       y="38.309322"
-       ry="1" />
+       width="19.113802"
+       height="9.550107"
+       x="7.0522652"
+       y="66.473434"
+       ry="0.92646646" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5069"
      inkscape:label="Battery-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Battery-Warning"
-       width="15.447426"
-       height="10.265867"
-       x="60.093647"
-       y="52.326412"
-       ry="1"
+       width="17.993334"
+       height="10.165371"
+       x="7.2150578"
+       y="51.908169"
+       ry="0.99021065"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5072"
      inkscape:label="Telemetry-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Telemetry-Warning"
-       width="18.966711"
-       height="10.202584"
-       x="34.470295"
-       y="52.358059"
-       ry="1"
+       width="33.069702"
+       height="9.9680929"
+       x="77.01255"
+       y="37.783974"
+       ry="0.97701645"
        inkscape:label="#rect4550-8-1-4-21-5" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5075"
      inkscape:label="FlightTime-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="FlightTime-Warning"
-       width="15.447426"
-       height="10.265867"
-       x="76.795616"
-       y="52.326412"
-       ry="1"
+       width="18.060331"
+       height="10.165371"
+       x="27.54425"
+       y="51.941666"
+       ry="0.99021065"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5078"
      inkscape:label="I2C-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="I2C-Warning"
-       width="10.470912"
-       height="8.5405388"
-       x="82.190269"
-       y="67.18853"
-       ry="1"
+       width="11.37538"
+       height="10.315975"
+       x="57.870148"
+       y="37.609093"
+       ry="1.2078834"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5081"
      inkscape:label="EventSystem-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="EventSystem-Warning"
-       width="10.470912"
-       height="8.5405388"
-       x="70.423416"
-       y="67.18853"
-       ry="1"
+       width="19.069374"
+       height="9.5117149"
+       x="70.091797"
+       y="66.525291"
+       ry="1.1137137"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5084"
      inkscape:label="StackOverflow-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="StackOverflow-Warning"
-       width="10.470912"
-       height="8.5405388"
-       x="58.656513"
-       y="67.18853"
-       ry="1"
+       width="19.116749"
+       height="9.4406528"
+       x="49.086876"
+       y="66.548973"
+       ry="1.1053932"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5087"
      inkscape:label="OutOfMemory-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="OutOfMemory-Warning"
-       width="10.470912"
-       height="8.5405388"
-       x="46.889668"
-       y="67.18853"
-       ry="1"
+       width="19.116749"
+       height="9.5354023"
+       x="28.058323"
+       y="66.477913"
+       ry="1.1164871"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5090"
      inkscape:label="CPUOverload-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="CPUOverload-Warning"
-       width="10.470912"
-       height="8.5405388"
-       x="35.122761"
-       y="67.18853"
-       ry="1"
+       width="19.140436"
+       height="9.5354023"
+       x="91.068588"
+       y="66.494171"
+       ry="1.1164871"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5093"
      inkscape:label="Receiver-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Receiver-Warning"
-       width="18.821587"
-       height="10.21344"
-       x="7.7214546"
-       y="38.403919"
-       ry="0.79517722"
+       width="23.142931"
+       height="10.112944"
+       x="7.1854739"
+       y="37.700443"
+       ry="0.78735298"
        inkscape:label="#rect4550"
-       rx="0.8158862" />
+       rx="1.0032097" />
   </g>
   <g
      inkscape:groupmode="layer"
@@ -1689,28 +1665,27 @@
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Actuator-Warning"
-       width="18.91176"
-       height="10.037488"
-       x="7.6274734"
-       y="52.645531"
-       ry="0.92773163"
+       width="23.233105"
+       height="10.104486"
+       x="32.485184"
+       y="37.706322"
+       ry="0.93392402"
        inkscape:label="#rect4550"
-       rx="0.9812547" />
+       rx="1.2054718" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5417"
      inkscape:label="GPS-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
-       transform="translate(-497.66563,-344.28037)"
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="GPS-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="505.26666"
-       y="361.65591"
-       ry="1"
+       width="23.857534"
+       height="10.135587"
+       x="7.065052"
+       y="18.146017"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
@@ -1721,11 +1696,11 @@
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Airspeed-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="51.238789"
-       y="17.375544"
-       ry="1"
+       width="23.824036"
+       height="10.102088"
+       x="59.904945"
+       y="18.177544"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
@@ -1736,55 +1711,55 @@
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Magnetometer-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="73.02417"
-       y="17.391815"
-       ry="1"
+       width="23.824036"
+       height="10.102088"
+       x="86.290001"
+       y="18.17181"
+       ry="0.99014992"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5423"
      inkscape:label="Sensors-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Sensors-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="29.41988"
-       y="17.375544"
-       ry="1"
+       width="23.824036"
+       height="10.135587"
+       x="33.488335"
+       y="18.155539"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5426"
      inkscape:label="PathPlan-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="PathPlan-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="73.05764"
-       y="3.2146251"
-       ry="1"
+       width="23.824036"
+       height="10.068589"
+       x="86.2995"
+       y="3.5313845"
+       ry="0.98686653"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5429"
      inkscape:label="Guidance-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Guidance-Critical"
-       width="18.966711"
+       width="23.857534"
        height="10.202584"
-       x="51.238789"
-       y="3.2146251"
+       x="59.858967"
+       y="3.4538779"
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
@@ -1792,73 +1767,73 @@
      inkscape:groupmode="layer"
      id="g5432"
      inkscape:label="Stabilization-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Stabilization-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="29.41988"
-       y="3.2146251"
-       ry="1"
+       width="23.824036"
+       height="10.135587"
+       x="33.504589"
+       y="3.4826155"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5435"
      inkscape:label="Attitude-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Attitude-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="7.6010327"
-       y="3.2146251"
-       ry="1"
+       width="23.790537"
+       height="10.135587"
+       x="7.1320496"
+       y="3.5161142"
+       ry="0.9934333"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5438"
      inkscape:label="SystemConfiguration-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="SystemConfiguration-Critical"
-       width="13.310432"
-       height="10.308098"
-       x="34.512287"
-       y="38.309322"
-       ry="1"
+       width="23.226076"
+       height="10.107105"
+       x="54.812557"
+       y="51.97683"
+       ry="0.98050147"
        inkscape:label="#rect4550-8-1-4-21-1" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5441"
      inkscape:label="BootFault-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="BootFault-Critical"
-       width="13.310432"
-       height="10.308098"
-       x="49.262897"
-       y="38.309322"
-       ry="1" />
+       width="19.139223"
+       height="9.5711241"
+       x="7.0209193"
+       y="66.448311"
+       ry="0.92850536" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5445"
      inkscape:label="Battery-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Battery-Critical"
-       width="15.447426"
+       width="18.060331"
        height="10.265867"
-       x="60.093647"
-       y="52.326412"
+       x="7.165554"
+       y="51.890926"
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
   </g>
@@ -1866,29 +1841,29 @@
      inkscape:groupmode="layer"
      id="g5448"
      inkscape:label="Telemetry-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Telemetry-Critical"
-       width="18.966711"
-       height="10.202584"
-       x="34.470295"
-       y="52.358059"
-       ry="1"
+       width="33.069702"
+       height="10.001592"
+       x="76.980263"
+       y="37.752586"
+       ry="0.98029983"
        inkscape:label="#rect4550-8-1-4-21-5" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5451"
      inkscape:label="FlightTime-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="FlightTime-Critical"
-       width="15.447426"
+       width="17.993334"
        height="10.265867"
-       x="76.795616"
-       y="52.326412"
+       x="27.552391"
+       y="51.85743"
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-2-9-8" />
   </g>
@@ -1896,92 +1871,92 @@
      inkscape:groupmode="layer"
      id="g5454"
      inkscape:label="I2C-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="I2C-Critical"
-       width="10.470912"
-       height="8.5405388"
-       x="82.190269"
-       y="67.18853"
-       ry="1"
+       width="11.37538"
+       height="10.315975"
+       x="57.870144"
+       y="37.609093"
+       ry="1.2078834"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5457"
      inkscape:label="EventSystem-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="EventSystem-Critical"
-       width="10.470912"
-       height="8.5405388"
-       x="70.423416"
-       y="67.18853"
-       ry="1"
+       width="19.046602"
+       height="9.5120039"
+       x="70.088425"
+       y="66.518555"
+       ry="1.1137475"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5460"
      inkscape:label="StackOverflow-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="StackOverflow-Critical"
-       width="10.470912"
-       height="8.5405388"
-       x="58.656513"
-       y="67.18853"
-       ry="1"
+       width="19.1136"
+       height="9.5455027"
+       x="49.075859"
+       y="66.485054"
+       ry="1.1176698"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5463"
      inkscape:label="OutOfMemory-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="OutOfMemory-Critical"
-       width="10.470912"
-       height="8.5405388"
-       x="46.889668"
-       y="67.18853"
-       ry="1"
+       width="19.1136"
+       height="9.5455027"
+       x="28.063345"
+       y="66.485054"
+       ry="1.1176698"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5466"
      inkscape:label="CPUOverload-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="CPUOverload-Critical"
-       width="10.470912"
-       height="8.5405388"
-       x="35.122761"
-       y="67.18853"
-       ry="1"
+       width="19.1136"
+       height="9.5120039"
+       x="91.065742"
+       y="66.485054"
+       ry="1.1137475"
        inkscape:label="#rect4550-8" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="g5469"
      inkscape:label="Receiver-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Receiver-Critical"
-       width="18.940025"
-       height="10.213441"
-       x="7.6267047"
-       y="38.417793"
-       ry="1.1877477"
+       width="23.194372"
+       height="10.146443"
+       x="7.090724"
+       y="37.714317"
+       ry="1.1799564"
        inkscape:label="#rect4550"
-       rx="1.2082177" />
+       rx="1.4796101" />
   </g>
   <g
      inkscape:groupmode="layer"
@@ -1991,23 +1966,23 @@
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Actuator-Critical"
-       width="18.874826"
-       height="10.081664"
-       x="7.663065"
-       y="52.603611"
-       ry="0.77868479"
+       width="23.22967"
+       height="10.148662"
+       x="32.519169"
+       y="37.663151"
+       ry="0.78385955"
        inkscape:label="#rect4550"
-       rx="0.79499185" />
+       rx="0.97841418" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer5"
      inkscape:label="GPS-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="GPS-Error"
        inkscape:label="#g3406"
-       transform="matrix(1.0377771,0,0,1,-0.27728602,0)">
+       transform="matrix(1.303815,0,0,1.0006407,-2.9181954,0.76728593)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9"
@@ -2024,10 +1999,11 @@
      inkscape:groupmode="layer"
      id="layer39"
      inkscape:label="PathPlan-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="PathPlan-Error"
-       inkscape:label="#g3394">
+       inkscape:label="#g3394"
+       transform="matrix(1.3060972,0,0,1.0001794,-9.286657,0.26650193)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-34"
@@ -2044,10 +2020,11 @@
      inkscape:groupmode="layer"
      id="layer38"
      inkscape:label="Guidance-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Guidance-Error"
-       inkscape:label="#g3390">
+       inkscape:label="#g3390"
+       transform="matrix(1.3027754,0,0,1.0133326,-6.8947082,0.12363438)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-3"
@@ -2064,10 +2041,11 @@
      inkscape:groupmode="layer"
      id="layer37"
      inkscape:label="Stabilization-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Stabilization-Error"
-       inkscape:label="#g3386">
+       inkscape:label="#g3386"
+       transform="matrix(1.2992362,0,0,1.0066736,-4.7769775,0.17889491)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-7"
@@ -2084,11 +2062,11 @@
      inkscape:groupmode="layer"
      id="layer3"
      inkscape:label="Airspeed-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Airspeed-Error"
        inkscape:label="#g3398"
-       transform="matrix(1.0309004,0,0,1,-1.5754528,0)">
+       transform="matrix(1.2972911,0,0,1.0167477,-6.6181285,0.34411746)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-79"
@@ -2110,7 +2088,7 @@
        style="display:inline"
        id="Magnetometer-Error"
        inkscape:label="#g3398"
-       transform="matrix(1.0309004,0,0,1,20.190366,0.03602308)">
+       transform="matrix(1.299095,0,0,1.0167223,19.677703,0.38071462)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-79-9"
@@ -2127,10 +2105,11 @@
      inkscape:groupmode="layer"
      id="layer2"
      inkscape:label="Sensors-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Sensors-Error"
-       inkscape:label="#g3402">
+       inkscape:label="#g3402"
+       transform="matrix(1.2955541,0,0,1.0099811,-4.6286669,0.49590947)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-86"
@@ -2147,33 +2126,33 @@
      inkscape:groupmode="layer"
      id="layer33"
      inkscape:label="Telemetry-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Telemetry-Error"
-       inkscape:label="#g3422"
-       transform="matrix(1.0343732,0,0,1,13.305779,0)">
+       transform="matrix(1.011145,0,0,1,-1.2370971,0)"
+       inkscape:label="#g4252">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-86-4"
-         d="M 20.794895,53.039633 39.08945,61.825066"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.1966573;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 77.702794,38.300164 32.588486,8.785433"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-5-4"
-         d="M 20.83573,61.847333 39.066764,53.07372"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19377422;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 77.775534,47.107864 110.25087,38.334251"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer35"
      inkscape:label="Receiver-Error"
-     style="display:inline">
+     style="display:none">
     <g
        style="display:inline"
        id="Receiver-Error"
        inkscape:label="#g3406"
-       transform="matrix(1.0487144,0,0,1,-0.57430778,21.097426)">
+       transform="matrix(1.2670269,0,0,1.0033347,-2.5810186,20.235292)">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-2"
@@ -2190,231 +2169,229 @@
      inkscape:groupmode="layer"
      id="layer34"
      inkscape:label="Actuator-Error"
-     style="display:inline">
+     style="display:none">
     <g
-       style="display:inline"
        id="Actuator-Error"
-       inkscape:label="#g3406"
-       transform="matrix(1.0377771,0,0,1,-0.32069392,35.147296)">
+       transform="matrix(1,0,0,0.99018386,0,0.46977719)"
+       inkscape:label="#g4238">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-1"
-         d="M 7.9615204,18.056455 26.210275,26.874464"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19737208;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 32.903038,38.329601 23.045017,8.819383"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-4"
-         d="M 7.9364397,26.848957 26.252147,18.055286"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19791019;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 32.871366,47.123473 56.000932,38.328432"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    </g>
+    <g
+       id="I2C-Error"
+       transform="matrix(0.9476726,0,0,0.98093768,3.3912844,0.88018434)"
+       inkscape:label="#g4530">
+      <path
+         inkscape:connector-curvature="0"
+         id="path6233-9-9-1-7"
+         d="m 57.85366,38.087518 11.979058,9.144934"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+      <path
+         inkscape:connector-curvature="0"
+         id="path6233-9-9-4-4-4"
+         d="M 57.837196,47.205999 69.860204,38.086306"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer32"
      inkscape:label="Battery-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Battery-Error"
-       inkscape:label="#g3426"
-       transform="translate(14,0)">
+       transform="matrix(0.99487041,0,0,0.96829624,0.0347729,1.7751095)"
+       inkscape:label="#g4456">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-86-0"
-         d="m 46.502131,52.910887 14.610607,9.14342"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.09097731;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 7.5498082,52.475403 18.0821878,9.14342"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-5-6"
-         d="M 46.442675,61.953487 61.145921,52.934252"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08697307;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 7.476225,61.518003 25.673064,52.498768"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer31"
      inkscape:label="FlightTime-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="FlightTime-Error"
-       inkscape:label="#g3430"
-       transform="translate(14,0)">
+       transform="matrix(0.99485498,0,0,0.97451479,0.20647751,1.4858328)"
+       inkscape:label="#g4450">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-86-0-3"
-         d="m 63.233236,52.875846 14.63044,9.079506"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08789504;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 27.859968,52.440362 45.94248,61.519868"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-5-6-7"
-         d="m 63.192544,61.969763 14.647987,-9.06903"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08791935;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 27.809675,61.534279 18.104198,-9.06903"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer30"
      inkscape:label="SystemConfiguration-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="SystemConfiguration-Error"
-       inkscape:label="#g3414"
-       transform="translate(14,0)">
+       transform="matrix(1.0165533,0,0,0.96183155,-1.1730886,2.2079995)"
+       inkscape:label="#g4438">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-86-6"
-         d="m 20.93643,38.821072 12.493301,9.22163"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01262045;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 55.345762,52.390536 22.824505,9.161262"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36421418;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-5-1"
-         d="M 20.937064,48.026792 33.498002,38.872801"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01214695;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 55.34692,61.535992 78.294994,52.441927"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36357629;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer29"
      inkscape:label="BootFault-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="BootFault-Error"
-       inkscape:label="#g3418"
-       transform="translate(14,0)">
+       transform="matrix(0.99024567,0,0,0.95640592,0.16475989,3.1867166)"
+       inkscape:label="#g4462">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-86-6-8"
-         d="m 35.705761,38.803039 12.45607,9.277316"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01467943;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 7.3536543,66.894221 26.479682,75.463174"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-5-1-9"
-         d="M 35.66424,48.062824 48.169858,38.865363"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01231074;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 7.2898997,75.446981 26.492007,66.951786"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer24"
      inkscape:label="Attitude-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="Attitude-Error"
-       inkscape:label="#g3382"
-       transform="matrix(1.0376867,0,0,1,-0.27616141,0)">
+       inkscape:label="#g4230">
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-8"
-         d="M 7.9267385,3.9562181 26.225281,12.757016"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19783366;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 7.431639,4.1245053 31.174293,12.957929"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36696184;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-9-4-9"
-         d="M 7.9328239,12.666843 26.286235,3.9527152"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19370687;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 7.4395349,12.867421 31.253381,4.1209894"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36225235;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="g3109"
      inkscape:label="OutOfMemory-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="OutOfMemory-Error"
-       inkscape:label="#g3442"
-       transform="translate(14,0)">
+       transform="matrix(0.99672458,0,0,0.95325152,0.02398426,3.264316)"
+       inkscape:label="#g4468">
       <path
          inkscape:connector-curvature="0"
          id="path6233"
-         d="M 33.20487,75.243494 43.009818,67.67709"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80505109;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 28.467187,75.543139 47.482776,66.983913"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9"
-         d="m 33.214298,67.65316 9.826498,7.608655"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81616998;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 28.485471,66.956843 19.057384,8.607021"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer28"
      inkscape:label="I2C-Error"
-     style="display:inline">
-    <g
-       id="I2C-Error"
-       inkscape:label="#g3454">
-      <path
-         inkscape:connector-curvature="0"
-         id="path6233-09"
-         d="m 68.473832,75.266428 9.874503,-7.54121"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80655557;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-      <path
-         inkscape:connector-curvature="0"
-         id="path6233-9-0"
-         d="m 68.55267,67.640417 9.780922,7.610454"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81437129;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    </g>
-  </g>
+     style="display:none" />
   <g
      inkscape:groupmode="layer"
      id="layer27"
      inkscape:label="EventSystem-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="EventSystem-Error"
-       inkscape:label="#g3450"
-       transform="translate(14,0)">
+       transform="matrix(0.99998704,0,0,0.94992153,-0.13295657,3.5178711)"
+       inkscape:label="#g4480">
       <path
          inkscape:connector-curvature="0"
          id="path6233-8"
-         d="m 56.728611,75.196387 9.852857,-7.519565"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80451399;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 70.524713,75.503806 89.549784,66.963359"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-6"
-         d="m 56.761123,67.664132 9.82824,7.574867"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81442791;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="m 70.587491,66.948946 18.977538,8.603257"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer26"
      inkscape:label="StackOverflow-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="StackOverflow-Error"
-       inkscape:label="#g3446"
-       transform="translate(14,0)">
+       transform="matrix(1.0098947,0,0,0.9498994,-0.68444657,3.4889072)"
+       inkscape:label="#g4474">
       <path
          inkscape:connector-curvature="0"
          id="path6233-0"
-         d="m 45.020244,75.290833 9.74566,-7.613709"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80511868;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 49.577304,75.608168 68.400595,67.02704"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-2"
-         d="m 45.015735,67.662945 9.78324,7.565398"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81205326;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 49.568595,67.011059 68.46447,75.537737"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer25"
      inkscape:label="CPUOverload-Error"
-     style="display:inline">
+     style="display:none">
     <g
        id="CPUOverload-Error"
-       inkscape:label="#g3438"
-       transform="translate(14,0)">
+       transform="matrix(1,0,0,0.94690781,0,3.7499736)"
+       inkscape:label="#g4486">
       <path
          inkscape:connector-curvature="0"
          id="path6233-2"
-         d="m 21.425298,75.227504 9.844871,-7.557269"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.8062014;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 91.332053,75.577785 110.51399,66.934663"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
       <path
          inkscape:connector-curvature="0"
          id="path6233-9-8"
-         d="M 21.451681,67.664449 31.2556,75.243937"
-         style="fill:#ff0000;stroke:#ff0000;stroke-width:0.8136676;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         d="M 91.383458,66.928045 110.4856,75.596579"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
   <g
@@ -2424,524 +2401,495 @@
      inkscape:groupmode="layer">
     <rect
        inkscape:label="#rect3054"
-       style="fill:none;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
+       style="fill:none;stroke:#000000;stroke-width:1.09511685;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
        id="rect4795"
-       width="95.066467"
-       height="79.056633"
-       x="0.25920519"
-       y="0.25921404"
-       ry="1" />
+       width="112.74295"
+       height="78.96769"
+       x="0.30367571"
+       y="0.30368456"
+       ry="0.99887496" />
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="67.606667"
-       y="8.3444462"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="78.238228"
+       y="9.280242"
        id="text5330-7"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
          id="tspan4289"
-         x="67.606667"
-         y="8.3444462">PATH</tspan></text>
+         x="78.238228"
+         y="9.280242">PATH</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="93.860985"
-       y="8.3444462"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="108.69147"
+       y="9.2802429"
        id="text5330-7-2"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
          id="tspan4291"
-         x="93.860985"
-         y="8.3444462">PLAN</tspan></text>
+         x="108.69147"
+         y="9.2802429">PLAN</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="10.759087"
-       y="8.3200321"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="15.664986"
+       y="9.2802429"
        id="text5330-7-7"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan3458"
-         x="10.759087"
-         y="8.3200321">ATTITUDE</tspan></text>
+         id="tspan5813"
+         x="15.664986"
+         y="9.2802429">ATTI</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="41.377663"
-       y="8.3434696"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="46.510185"
+       y="9.2787771"
        id="text5330-7-3"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
          id="tspan4287"
-         x="41.377663"
-         y="8.3434696">STAB</tspan></text>
+         x="46.510185"
+         y="9.2787771">STAB</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="16.329399"
-       y="20.115072"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="16.553135"
+       y="21.460487"
        id="text5330-7-7-9"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
          id="tspan5866"
-         x="16.329399"
-         y="20.115072">GPS</tspan></text>
+         x="16.553135"
+         y="21.460487">GPS</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="37.037819"
-       y="20.115072"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="41.87249"
+       y="21.460487"
        id="text5330-7-7-6"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan4319"
-         x="37.037819"
-         y="20.115072">SENSORS</tspan></text>
+         id="tspan5858"
+         x="41.87249"
+         y="21.460487">SENSOR</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="62.913303"
-       y="20.115072"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="75.206001"
+       y="21.460487"
        id="text5330-7-7-0"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan4321"
-         x="62.913303"
-         y="20.115072">AIRSPEED</tspan></text>
+         id="tspan5860"
+         x="75.206001"
+         y="21.460487">AIRSPD</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="94.605125"
-       y="20.116049"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="110.314"
+       y="21.461952"
        id="text5330-7-7-8"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
          id="tspan3835"
-         x="94.605125"
-         y="20.116049">MAG</tspan></text>
+         x="110.314"
+         y="21.461952">MAG</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="47.086681"
-       y="58.514309"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="118.93848"
+       y="59.436008"
        id="text5330-0"
        sodipodi:linespacing="125%"
        transform="scale(0.8038279,1.2440474)"><tspan
          sodipodi:role="line"
          id="tspan5951"
-         x="47.086681"
-         y="58.514309">CPU</tspan></text>
+         x="118.93848"
+         y="59.436008">CPU</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="89.03878"
-       y="58.514309"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="88.885544"
+       y="59.436008"
        id="text5330-0-4"
        sodipodi:linespacing="125%"
        transform="scale(0.8038279,1.2440474)"><tspan
          sodipodi:role="line"
-         id="tspan4311"
-         x="89.03878"
-         y="58.514309">EVENT</tspan></text>
+         id="tspan5907"
+         x="88.885544"
+         y="59.436008">EVENT</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="60.922539"
-       y="58.514309"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="39.798244"
+       y="59.436008"
        id="text5330-0-6"
        sodipodi:linespacing="125%"
        transform="scale(0.8038279,1.2440474)"><tspan
          sodipodi:role="line"
-         id="tspan3462"
-         x="60.922539"
-         y="58.514309">MEM.</tspan></text>
+         id="tspan3865"
+         x="39.798244"
+         y="59.436008">MEM</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="74.263214"
-       y="58.513577"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="62.485939"
+       y="59.434544"
        id="text5330-0-0"
        sodipodi:linespacing="125%"
        transform="scale(0.8038279,1.2440474)"><tspan
          sodipodi:role="line"
-         id="tspan4309"
-         x="74.263214"
-         y="58.513577">STACK</tspan></text>
+         id="tspan5905"
+         x="62.485939"
+         y="59.434544">STACK</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="106.74649"
-       y="58.476372"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="74.377625"
+       y="36.536701"
        id="text5330-0-00"
        sodipodi:linespacing="125%"
        transform="scale(0.8038279,1.2440474)"><tspan
          sodipodi:role="line"
          id="tspan5947"
-         x="106.74649"
-         y="58.476372">I2C</tspan></text>
+         x="74.377625"
+         y="36.536701">I2C</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="43.342724"
-       y="37.561607"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="68.291183"
+       y="49.551472"
        id="textconf"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"
        inkscape:label="#text5330-7-7-9-1"><tspan
          sodipodi:role="line"
-         id="tspan4317"
-         x="43.342724"
-         y="37.561607">CONF.</tspan></text>
+         id="tspan5978"
+         x="68.291183"
+         y="49.551472">CONFIG</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="61.386131"
-       y="37.561607"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="11.171983"
+       y="61.392075"
        id="text5858"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"
        inkscape:label="#text5330-7-7-9-8"><tspan
          sodipodi:role="line"
-         id="tspan4313"
-         x="61.386131"
-         y="37.561607">BOOT</tspan></text>
+         id="tspan5864"
+         x="11.171983"
+         y="61.392075">BOOT</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="45.628952"
-       y="49.196098"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="94.109077"
+       y="37.710892"
        id="text5330-7-7-9-8-3"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan4315"
-         x="45.628952"
-         y="49.196098">TELEM.</tspan></text>
+         id="tspan5862"
+         x="94.109077"
+         y="37.710892">TELEMETRY</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="75.682053"
-       y="49.196095"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="11.343655"
+       y="49.551472"
        id="text5330-7-7-9-8-4"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan4303"
-         x="75.682053"
-         y="49.196095">BATT.</tspan></text>
+         id="tspan4077"
+         x="11.343655"
+         y="49.551472">BATT</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="96.951797"
-       y="49.196095"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="36.921585"
+       y="49.551472"
        id="text5330-7-7-9-8-0"
        sodipodi:linespacing="125%"
-       transform="scale(0.83127393,1.2029729)"><tspan
+       transform="scale(0.83127393,1.2029729)"
+       inkscape:transform-center-x="-3.0319646"
+       inkscape:transform-center-y="-0.94748894"><tspan
          sodipodi:role="line"
          id="tspan4305"
-         x="96.951797"
-         y="49.196095">TIME</tspan></text>
-    <rect
-       style="fill:none;stroke:#ffffff;stroke-width:0.53149605;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
-       id="Actuator-7"
-       width="11.450287"
-       height="44.10326"
-       x="71.210968"
-       y="84.344215"
-       ry="1"
-       inkscape:label="#rect4550-4" />
+         x="36.921585"
+         y="49.551472">TIME</tspan></text>
     <rect
        inkscape:label="#rect4550-8"
-       ry="1"
-       y="67.18853"
-       x="35.122761"
-       height="8.5405388"
-       width="10.470912"
+       ry="1.1183628"
+       y="66.493958"
+       x="7.053793"
+       height="9.5514212"
+       width="19.134621"
        id="rect4780"
-       style="fill:none;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     <rect
-       inkscape:label="#rect4550-8-7"
-       ry="1"
-       y="67.18853"
-       x="46.889668"
-       height="8.5405388"
-       width="10.470912"
-       id="rect4782"
-       style="fill:none;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1"
-       ry="1"
-       y="67.18853"
-       x="58.656513"
-       height="8.5405388"
-       width="10.470912"
-       id="rect4784"
-       style="fill:none;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-15"
-       ry="1"
-       y="67.18853"
-       x="70.423416"
-       height="8.5405388"
-       width="10.470912"
-       id="rect4786"
-       style="fill:none;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-2"
-       ry="1"
-       y="67.18853"
-       x="82.190269"
-       height="8.5405388"
-       width="10.470912"
-       id="rect4788"
-       style="fill:none;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-8"
-       ry="1"
-       y="38.309322"
-       x="49.262897"
-       height="10.308098"
-       width="13.310432"
-       id="rect4790"
-       style="fill:none;stroke:#ffffff;stroke-width:0.56247556;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       ry="1"
-       y="38.309322"
-       x="64.013565"
-       height="10.308098"
-       width="13.310432"
-       id="rect4792"
-       style="fill:none;stroke:#ffffff;stroke-width:0.56247556;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       ry="1"
-       y="38.242306"
-       x="78.764183"
-       height="10.308098"
-       width="13.310432"
-       id="rect4794"
-       style="fill:none;stroke:#ffffff;stroke-width:0.56247556;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-2-9"
-       ry="1"
-       y="52.326412"
-       x="76.795616"
-       height="10.265867"
-       width="15.447426"
-       id="rect4796"
-       style="fill:none;stroke:#ffffff;stroke-width:0.60470587;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-5"
-       ry="1"
-       y="52.358059"
-       x="34.470295"
-       height="10.202584"
-       width="18.966711"
-       id="rect4798"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
        inkscape:label="#rect4550-8-1-4-21-5-4"
-       ry="1"
-       y="347.495"
-       x="505.26666"
-       height="10.202584"
-       width="18.966711"
+       ry="0.99234092"
+       y="3.5183218"
+       x="7.1022706"
+       height="10.124442"
+       width="23.84639"
        id="rect4800"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
-       inkscape:label="#rect4550-8-1-4-21-5-3"
-       ry="1"
-       y="347.495"
-       x="527.08551"
-       height="10.202584"
-       width="18.966711"
-       id="rect4802"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
-       inkscape:label="#rect4550-8-1-4-21-5-1"
-       ry="1"
-       y="347.495"
-       x="548.90442"
-       height="10.202584"
-       width="18.966711"
-       id="rect4804"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
-       inkscape:label="#rect4550-8-1-4-21-5-2"
-       ry="1"
-       y="347.495"
-       x="570.72327"
-       height="10.202584"
-       width="18.966711"
-       id="rect4806"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-5-33"
-       ry="1"
-       y="361.65591"
-       x="505.26666"
-       height="10.202584"
-       width="18.966711"
-       id="rect4808"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
-       transform="translate(-497.66563,-344.28037)" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
-       inkscape:label="#rect4550-8-1-4-21-5-41"
-       ry="1"
-       y="361.65591"
-       x="527.08551"
-       height="10.202584"
-       width="18.966711"
-       id="rect4810"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
-       inkscape:label="#rect4550-8-1-4-21-5-13"
-       ry="1"
-       y="361.65591"
-       x="548.90442"
-       height="10.202584"
-       width="18.966711"
-       id="rect4812"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       transform="translate(-497.66563,-344.28037)"
-       ry="1"
-       y="361.65591"
-       x="570.72327"
-       height="10.202584"
-       width="18.966711"
-       id="rect4814"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-2-9-8"
-       ry="1"
-       y="52.326412"
-       x="60.093647"
-       height="10.265867"
-       width="15.447426"
-       id="rect4816"
-       style="fill:none;stroke:#ffffff;stroke-width:0.60470587;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       ry="1"
-       y="38.309322"
-       x="34.512287"
-       height="10.308098"
-       width="13.310432"
-       id="rect4818"
-       style="fill:none;stroke:#ffffff;stroke-width:0.56247556;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-4"
-       ry="1"
-       y="84.344215"
-       x="71.210968"
-       height="44.10326"
-       width="11.450287"
-       id="rect4841"
-       style="fill:none;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-5-33"
-       ry="1"
-       y="38.420193"
-       x="7.6010327"
-       height="10.202584"
-       width="18.966711"
-       id="rect4808-4"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="14.453424"
-       y="37.585503"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="13.357648"
+       y="37.674271"
        id="text5330-7-7-9-8"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan3410"
-         x="14.453424"
-         y="37.585503">INPUT</tspan></text>
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-5-33"
-       ry="1"
-       y="52.358059"
-       x="7.6010327"
-       height="10.202584"
-       width="18.966711"
-       id="rect4808-4-4"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
-    <rect
-       inkscape:label="#rect4550-8-1-4-21-5-33"
-       ry="1"
-       y="66.372726"
-       x="7.6010327"
-       height="10.202584"
-       width="18.966711"
-       id="rect4808-4-4-7"
-       style="fill:none;stroke:#ffffff;stroke-width:0.66798913;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+         id="tspan5909"
+         x="13.357648"
+         y="37.674271">INPUT</tspan></text>
     <text
        xml:space="preserve"
-       style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="12.280572"
-       y="49.196102"
+       style="font-size:6px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
+       x="40.654816"
+       y="37.710892"
        id="text5330-7-7-9-8-1"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan3412"
-         x="12.280572"
-         y="49.196102">OUTPUT</tspan></text>
+         id="tspan5911"
+         x="40.654816"
+         y="37.710892">OUTPUT</tspan></text>
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="0.98703158"
+       y="51.95937"
+       x="54.784607"
+       height="10.132735"
+       width="23.300135"
+       id="rect4796-2-7"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="0.99532819"
+       y="51.916786"
+       x="7.1737313"
+       height="10.217907"
+       width="18.079369"
+       id="rect4796-2-79"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="0.99532819"
+       y="51.916786"
+       x="27.540998"
+       height="10.217907"
+       width="18.079369"
+       id="rect4796-2-3"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="0.98714912"
+       y="37.71487"
+       x="32.498486"
+       height="10.133942"
+       width="23.221214"
+       id="rect4796-2-79-9"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="1.0086432"
+       y="37.604546"
+       x="57.889503"
+       height="10.354595"
+       width="11.382303"
+       id="rect4796-2-79-8"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="0.97385728"
+       y="37.783096"
+       x="77.012535"
+       height="9.9974899"
+       width="33.066151"
+       id="rect4796-2-79-8-6"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="3.5183218"
+       x="33.500103"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-2"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="3.5183218"
+       x="59.897934"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-4"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="3.5183218"
+       x="86.295769"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-8"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="18.172588"
+       x="7.1022706"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-6"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="18.172588"
+       x="33.500103"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-5"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="18.172588"
+       x="59.897934"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-0"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-5-4"
+       ry="0.99234092"
+       y="18.172588"
+       x="86.295769"
+       height="10.124442"
+       width="23.84639"
+       id="rect4800-9"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8"
+       ry="1.1183628"
+       y="66.493958"
+       x="28.054348"
+       height="9.5514212"
+       width="19.134621"
+       id="rect4780-0"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8"
+       ry="1.1183628"
+       y="66.493958"
+       x="49.054901"
+       height="9.5514212"
+       width="19.134621"
+       id="rect4780-06"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8"
+       ry="1.1183628"
+       y="66.493958"
+       x="70.05545"
+       height="9.5514212"
+       width="19.134621"
+       id="rect4780-1"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8"
+       ry="1.1183628"
+       y="66.493958"
+       x="91.056015"
+       height="9.5514212"
+       width="19.134621"
+       id="rect4780-3"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="0.98714912"
+       y="37.71487"
+       x="7.0977092"
+       height="10.133942"
+       width="23.221214"
+       id="rect4796-2-79-9-3"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="1.0036905"
+       y="51.873863"
+       x="80.600296"
+       height="10.303754"
+       width="13.522519"
+       id="rect4796-2-7-4"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    <rect
+       inkscape:label="#rect4550-8-1-4-21-2-9"
+       ry="1.0036905"
+       y="51.873863"
+       x="96.638374"
+       height="10.303754"
+       width="13.522519"
+       id="rect4796-2-7-4-0"
+       style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
   </g>
   <g
      inkscape:groupmode="layer"
      id="layer36"
      inkscape:label="No Link"
-     style="display:none">
+     style="display:inline">
     <g
        id="nolink"
-       inkscape:label="#g4324">
+       inkscape:label="#g4534">
       <rect
          inkscape:label="#nolink"
-         style="opacity:0.51388891;fill:#483737;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
+         style="opacity:0.51388891;fill:#483737;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.09553051;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
          id="opacity"
-         width="95.066467"
+         width="112.70118"
          height="79.056633"
-         x="0.25920519"
+         x="0.27291748"
          y="0.2592113"
          ry="1.6285406" />
       <rect
          ry="2.3865318"
          y="29.358137"
-         x="18.381285"
+         x="19.781004"
          height="7.3904138"
-         width="62.155273"
+         width="73.685005"
          id="rect8557"
-         style="fill:#ffffff;fill-opacity:1;stroke:#800000;stroke-width:0.53149605;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+         style="fill:#ffffff;fill-opacity:1;stroke:#800000;stroke-width:0.57869613;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
       <text
-         transform="scale(1.0128485,0.98731448)"
+         transform="scale(1.1027957,0.90678628)"
          sodipodi:linespacing="125%"
          id="text8563"
-         y="36.071426"
-         x="33.608711"
-         style="font-size:7.27633667px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#800000;fill-opacity:1;stroke:none;font-family:Arial;-inkscape-font-specification:Arial Bold"
+         y="39.286636"
+         x="34.689133"
+         style="font-size:7.92252016px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#800000;fill-opacity:1;stroke:none;font-family:Arial;-inkscape-font-specification:Arial Bold"
          xml:space="preserve"><tspan
-           y="36.071426"
-           x="33.608711"
+           y="39.286636"
+           x="34.689133"
            id="tspan8565"
            sodipodi:role="line">NO LINK</tspan></text>
       <g
-         transform="matrix(1.0659619,0,0,1.2030901,11.890962,-5.4421624)"
+         transform="matrix(1.2636966,0,0,1.2030901,13.594698,-5.5358311)"
          id="g4267">
         <path
            style="fill:none;stroke:#000000;stroke-width:0.29055119;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
@@ -2961,7 +2909,7 @@
            sodipodi:cy="35.07505"
            sodipodi:rx="0.5"
            sodipodi:ry="0.5"
-           d="m 14,35.07505 c 0,0.276143 -0.223858,0.5 -0.5,0.5 -0.276142,0 -0.5,-0.223857 -0.5,-0.5 0,-0.276142 0.223858,-0.5 0.5,-0.5 0.269688,0 0.490781,0.21388 0.499725,0.483419"
+           d="m 14,35.07505 a 0.5,0.5 0 1 1 -2.75e-4,-0.01658"
            sodipodi:start="0"
            sodipodi:end="6.2500167"
            sodipodi:open="true"
@@ -2973,7 +2921,7 @@
            inkscape:connector-curvature="0" />
       </g>
       <g
-         transform="matrix(1.0659619,0,0,1.2030901,59.303858,-5.5104487)"
+         transform="matrix(1.2636966,0,0,1.2030901,66.606704,-5.5358311)"
          id="g4267-4"
          style="display:inline">
         <path
@@ -2994,7 +2942,7 @@
            sodipodi:cy="35.07505"
            sodipodi:rx="0.5"
            sodipodi:ry="0.5"
-           d="m 14,35.07505 c 0,0.276143 -0.223858,0.5 -0.5,0.5 -0.276142,0 -0.5,-0.223857 -0.5,-0.5 0,-0.276142 0.223858,-0.5 0.5,-0.5 0.269688,0 0.490781,0.21388 0.499725,0.483419"
+           d="m 14,35.07505 a 0.5,0.5 0 1 1 -2.75e-4,-0.01658"
            sodipodi:start="0"
            sodipodi:end="6.2500167"
            sodipodi:open="true"
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
index 6d74e9fa6..3b8a61432 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@@ -125,7 +125,8 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/osdsettings.h \
     $$UAVOBJECT_SYNTHETICS/waypoint.h \
     $$UAVOBJECT_SYNTHETICS/waypointactive.h \
-    $$UAVOBJECT_SYNTHETICS/mpu6000settings.h
+    $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
+    $$UAVOBJECT_SYNTHETICS/takeofflocation.h
 
 SOURCES += \
     $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
@@ -227,4 +228,5 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/waypoint.cpp \
     $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
-    $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp
+    $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
+    $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp
diff --git a/make/tools.mk b/make/tools.mk
index cd129cec7..5771abf28 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -58,12 +58,14 @@ endif
 
 ifeq ($(UNAME), Linux)
     ifeq ($(ARCH), x86_64)
-        ARM_SDK_URL := http://wiki.openpilot.org/download/attachments/18612236/gcc-arm-none-eabi-4_7-2013q1-20130313-linux-amd64.tar.bz2
+        ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2
+        ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2/+md5
         QT_SDK_URL  := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-linux-x64-5.2.1.run
         QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-linux-x64-5.2.1.run.md5
         QT_SDK_ARCH := gcc_64
     else
-        ARM_SDK_URL := http://wiki.openpilot.org/download/attachments/18612236/gcc-arm-none-eabi-4_7-2013q1-20130313-linux-i686.tar.bz2
+        ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2
+        ARM_SDK_MD5_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2/+md5
         QT_SDK_URL  := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-linux-x86-5.2.1.run
         QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-linux-x86-5.2.1.run.md5
         QT_SDK_ARCH := gcc
@@ -71,12 +73,14 @@ ifeq ($(UNAME), Linux)
     UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz
     DOXYGEN_URL    := http://wiki.openpilot.org/download/attachments/18612236/doxygen-1.8.3.1.src.tar.gz
 else ifeq ($(UNAME), Darwin)
-    ARM_SDK_URL    := http://wiki.openpilot.org/download/attachments/18612236/gcc-arm-none-eabi-4_7-2013q1-20130313-mac.tar.bz2
+    ARM_SDK_URL    := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2
+    ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2/+md5
     QT_SDK_URL     := "Please install native Qt 5.1.x SDK using package manager"
     UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz
     DOXYGEN_URL    := http://wiki.openpilot.org/download/attachments/18612236/doxygen-1.8.3.1.src.tar.gz
 else ifeq ($(UNAME), Windows)
-    ARM_SDK_URL    := http://wiki.openpilot.org/download/attachments/18612236/gcc-arm-none-eabi-4_7-2013q1-20130313-windows.tar.bz2
+    ARM_SDK_URL    := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip
+    ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip/+md5
     QT_SDK_URL     := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe
     QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe.md5
     QT_SDK_ARCH    := mingw48_32
@@ -90,7 +94,7 @@ endif
 GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0.zip
 
 # Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri
-ARM_SDK_DIR     := $(TOOLS_DIR)/gcc-arm-none-eabi-4_7-2013q1
+ARM_SDK_DIR     := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1
 QT_SDK_DIR      := $(TOOLS_DIR)/qt-5.2.1
 MINGW_DIR       := $(QT_SDK_DIR)/Tools/mingw48_32
 PYTHON_DIR      := $(QT_SDK_DIR)/Tools/mingw48_32/opt/bin
@@ -111,7 +115,7 @@ QT_SDK_PREFIX := $(QT_SDK_DIR)
 
 BUILD_SDK_TARGETS := arm_sdk qt_sdk
 ifeq ($(UNAME), Windows)
-    BUILD_SDK_TARGETS += mingw sdl python nsis openssl
+    BUILD_SDK_TARGETS += sdl nsis openssl
 endif
 ALL_SDK_TARGETS := $(BUILD_SDK_TARGETS) gtest uncrustify doxygen
 
@@ -275,39 +279,43 @@ endef
 #  $(1) = tool name
 #  $(2) = tool extract/build directory
 #  $(3) = tool distribution URL
-#  $(4) = tool distribution file
-#  $(5) = optional extra build recipes template
-#  $(6) = optional extra clean recipes template
+#  $(4) = tool distribution MD5 URL
+#  $(5) = tool distribution file
+#  $(6) = optional extra build recipes template
+#  $(7) = optional extra clean recipes template
 #
 ##############################
 
+
+
 define TOOL_INSTALL_TEMPLATE
 
 .PHONY: $(addprefix $(1)_, install clean distclean)
 
 $(1)_install: $(1)_clean | $(DL_DIR) $(TOOLS_DIR)
 	
-	$(call DOWNLOAD_TEMPLATE,$(3),$(4),"$(3).md5")
-
+	$(if $(4), $(call DOWNLOAD_TEMPLATE,$(3),$(5),$(4)),$(call DOWNLOAD_TEMPLATE,$(3),$(5),"$(3).md5"))
+	
 	@$(ECHO) $(MSG_EXTRACTING) $$(call toprel, $(2))
 	$(V1) $(MKDIR) -p $$(call toprel, $(dir $(2)))
-	$(if $(filter $(suffix $(4)), .zip),
-		$(V1) $(UNZIP) $(UNZIP_SILENT) -d $$(call toprel, $(dir $(2))) $$(call toprel, $(DL_DIR)/$(4)),
-		$(V1) $(TAR) $(TAR_OPTIONS) -C $$(call toprel, $(dir $(2))) -xf $$(call toprel, $(DL_DIR)/$(4))
+
+	$(if $(filter $(suffix $(5)), .zip),
+		$(V1) $(UNZIP) $(UNZIP_SILENT) -d $$(call toprel, $(dir $(2))) $$(call toprel, $(DL_DIR)/$(5)),
+		$(V1) $(TAR) $(TAR_OPTIONS) -C $$(call toprel, $(dir $(2))) -xf $$(call toprel, $(DL_DIR)/$(5))
 	)
 
-	$(5)
+	$(6)
 
 $(1)_clean:
 	@$(ECHO) $(MSG_CLEANING) $$(call toprel, $(2))
 	$(V1) [ ! -d "$(2)" ] || $(RM) -rf "$(2)"
 
-	$(6)
+	$(7)
 
 $(1)_distclean:
-	@$(ECHO) $(MSG_DISTCLEANING) $$(call toprel, $(DL_DIR)/$(4))
-	$(V1) [ ! -f "$(DL_DIR)/$(4)" ]     || $(RM) "$(DL_DIR)/$(4)"
-	$(V1) [ ! -f "$(DL_DIR)/$(4).md5" ] || $(RM) "$(DL_DIR)/$(4).md5"
+	@$(ECHO) $(MSG_DISTCLEANING) $$(call toprel, $(DL_DIR)/$(5))
+	$(V1) [ ! -f "$(DL_DIR)/$(5)" ]     || $(RM) "$(DL_DIR)/$(5)"
+	$(V1) [ ! -f "$(DL_DIR)/$(5).md5" ] || $(RM) "$(DL_DIR)/$(5).md5"
 
 endef
 
@@ -453,9 +461,13 @@ endef
 # ARM SDK
 #
 ##############################
-
-$(eval $(call TOOL_INSTALL_TEMPLATE,arm_sdk,$(ARM_SDK_DIR),$(ARM_SDK_URL),$(notdir $(ARM_SDK_URL))))
-
+ifeq ($(UNAME), Windows)
+#unfortunately zip package for this release is missing root directory, so adding / at the end of the path 
+# so that template interpret last part as directory and use the full path
+$(eval $(call TOOL_INSTALL_TEMPLATE,arm_sdk,$(ARM_SDK_DIR)/,$(ARM_SDK_URL),$(ARM_SDK_MD5_URL),$(notdir $(ARM_SDK_URL))))
+else
+$(eval $(call TOOL_INSTALL_TEMPLATE,arm_sdk,$(ARM_SDK_DIR),$(ARM_SDK_URL),$(ARM_SDK_MD5_URL),$(notdir $(ARM_SDK_URL))))
+endif
 ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && $(ECHO) "exists"), exists)
     export ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi-
 else
@@ -471,7 +483,7 @@ arm_sdk_version:
 # Template to check ARM toolchain version before building targets
 define ARM_GCC_VERSION_CHECK_TEMPLATE
 	if ! $(ARM_SDK_PREFIX)gcc --version --specs=nano.specs >/dev/null 2>&1; then \
-		$(ECHO) $(MSG_NOTICE) Please install ARM toolchain 4.7+ using \'make arm_sdk_install\' && \
+		$(ECHO) $(MSG_NOTICE) Please install ARM toolchain 4.8 2014q1 using \'make arm_sdk_install\' && \
 		$(ECHO) $(MSG_NOTICE) Older ARM SDKs do not support new \'--specs=nano.specs\' option && \
 		exit 1; \
 	fi
@@ -600,7 +612,7 @@ python_version:
 
 ifeq ($(UNAME), Windows)
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,nsis,$(NSIS_DIR),$(NSIS_URL),$(notdir $(NSIS_URL))))
+$(eval $(call TOOL_INSTALL_TEMPLATE,nsis,$(NSIS_DIR),$(NSIS_URL),,$(notdir $(NSIS_URL))))
 
 ifeq ($(shell [ -d "$(NSIS_DIR)" ] && $(ECHO) "exists"), exists)
     export NSIS := $(NSIS_DIR)/makensis
@@ -624,7 +636,7 @@ endif
 
 ifeq ($(UNAME), Windows)
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,sdl,$(SDL_DIR),$(SDL_URL),$(notdir $(SDL_URL))))
+$(eval $(call TOOL_INSTALL_TEMPLATE,sdl,$(SDL_DIR),$(SDL_URL),,$(notdir $(SDL_URL))))
 
 ifeq ($(shell [ -d "$(SDL_DIR)" ] && $(ECHO) "exists"), exists)
     export SDL_DIR := $(SDL_DIR)
@@ -647,7 +659,7 @@ endif
 
 ifeq ($(UNAME), Windows)
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,openssl,$(OPENSSL_DIR),$(OPENSSL_URL),$(notdir $(OPENSSL_URL))))
+$(eval $(call TOOL_INSTALL_TEMPLATE,openssl,$(OPENSSL_DIR),$(OPENSSL_URL),,$(notdir $(OPENSSL_URL))))
 
 ifeq ($(shell [ -d "$(OPENSSL_DIR)" ] && $(ECHO) "exists"), exists)
     export OPENSSL := "$(OPENSSL_DIR)/bin/openssl"    
@@ -671,7 +683,7 @@ endif
 
 ifeq ($(UNAME), Windows)
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,uncrustify,$(UNCRUSTIFY_DIR),$(UNCRUSTIFY_URL),$(notdir $(UNCRUSTIFY_URL))))
+$(eval $(call TOOL_INSTALL_TEMPLATE,uncrustify,$(UNCRUSTIFY_DIR),$(UNCRUSTIFY_URL),,$(notdir $(UNCRUSTIFY_URL))))
 
 else # Linux or Mac
 
@@ -695,7 +707,7 @@ define UNCRUSTIFY_CLEAN_TEMPLATE
 	-$(V1) [ ! -d "$(UNCRUSTIFY_DIR)" ] || $(RM) -rf "$(UNCRUSTIFY_DIR)"
 endef
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,uncrustify,$(UNCRUSTIFY_BUILD_DIR),$(UNCRUSTIFY_URL),$(notdir $(UNCRUSTIFY_URL)),$(UNCRUSTIFY_BUILD_TEMPLATE),$(UNCRUSTIFY_CLEAN_TEMPLATE)))
+$(eval $(call TOOL_INSTALL_TEMPLATE,uncrustify,$(UNCRUSTIFY_BUILD_DIR),$(UNCRUSTIFY_URL),,$(notdir $(UNCRUSTIFY_URL)),$(UNCRUSTIFY_BUILD_TEMPLATE),$(UNCRUSTIFY_CLEAN_TEMPLATE)))
 
 endif
 
@@ -719,7 +731,7 @@ uncrustify_version:
 
 ifeq ($(UNAME), Windows)
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,doxygen,$(DOXYGEN_DIR),$(DOXYGEN_URL),$(notdir $(DOXYGEN_URL))))
+$(eval $(call TOOL_INSTALL_TEMPLATE,doxygen,$(DOXYGEN_DIR),$(DOXYGEN_URL),,$(notdir $(DOXYGEN_URL))))
 
 else # Linux or Mac
 
@@ -743,7 +755,7 @@ define DOXYGEN_CLEAN_TEMPLATE
 	-$(V1) [ ! -d "$(DOXYGEN_DIR)" ] || $(RM) -rf "$(DOXYGEN_DIR)"
 endef
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,doxygen,$(DOXYGEN_BUILD_DIR),$(DOXYGEN_URL),$(notdir $(DOXYGEN_URL)),$(DOXYGEN_BUILD_TEMPLATE),$(DOXYGEN_CLEAN_TEMPLATE)))
+$(eval $(call TOOL_INSTALL_TEMPLATE,doxygen,$(DOXYGEN_BUILD_DIR),$(DOXYGEN_URL),,$(notdir $(DOXYGEN_URL)),$(DOXYGEN_BUILD_TEMPLATE),$(DOXYGEN_CLEAN_TEMPLATE)))
 
 endif
 
@@ -765,7 +777,7 @@ doxygen_version:
 #
 ##############################
 
-$(eval $(call TOOL_INSTALL_TEMPLATE,gtest,$(GTEST_DIR),$(GTEST_URL),$(notdir $(GTEST_URL))))
+$(eval $(call TOOL_INSTALL_TEMPLATE,gtest,$(GTEST_DIR),$(GTEST_URL),,$(notdir $(GTEST_URL))))
 
 export GTEST_DIR
 
diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
index 9be020de8..83bc91e25 100644
--- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
+++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
@@ -12,6 +12,8 @@
 
         <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
                 <!-- 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"/>
+		<!-- 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.05"/>
                 <!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml
index af68ab276..9bb82e566 100644
--- a/shared/uavobjectdefinition/flightmodesettings.xml
+++ b/shared/uavobjectdefinition/flightmodesettings.xml
@@ -7,69 +7,69 @@
 	<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
         <field name="Stabilization1Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,AxisLock,Manual"
-		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario;"
 			/>
         <field name="Stabilization2Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,Rate,Manual"
-		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario;"
 			/>
         <field name="Stabilization3Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Rate,Rate,Rate,Manual"
-		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario;"
 			/>
         <field name="Stabilization4Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
-		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario;"
 			/>
         <field name="Stabilization5Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,Rate,CruiseControl"
-		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario;"
 			/>
         <field name="Stabilization6Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Rate,Rate,Rate,CruiseControl"
-		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
-			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:AltitudeVario;"
 			/>
 
 	<!-- Note these options values should be identical to those defined in FlightMode -->
@@ -78,37 +78,42 @@
                units=""
                type="enum"
                elements="6"
-               options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"
+               options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,PositionVarioFPV,PositionVarioLOS,PositionVarioNSEW,ReturnToBase,Land,PathPlanner,POI,AutoCruise"
                defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
                limits="\
-                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0903NE:Autotune:AutoCruise;\
                        \
-                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0903NE:Autotune:AutoCruise;\
                        \
-                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0903NE:Autotune:AutoCruise;\
                        \
-                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0903NE:Autotune:AutoCruise;\
                        \
-                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0903NE:Autotune:AutoCruise;\
                        \
-                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
+                       %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
+                       %0903NE:Autotune:AutoCruise"/>
 
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
         <field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
-        <field name="ReturnToHomeAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
+        <field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
+        <field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="10,2"/>
+	<!-- optimized for current vtolpathfollower,
+	     for fixed wing pathfollower set to Horizontal=500,Vertical=5 -->
+        <field name="PositionHoldStartingVelocity" units="m/s" type="float" elements="1" defaultvalue="20"/>
+	<!-- currently ignored by vtolpathfollower -->
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>
diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml
index 494cf474e..794d02b17 100644
--- a/shared/uavobjectdefinition/flightstatus.xml
+++ b/shared/uavobjectdefinition/flightstatus.xml
@@ -4,7 +4,7 @@
         <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
 
 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,PositionVarioFPV,PositionVarioLOS,PositionVarioNSEW,ReturnToBase,Land,PathPlanner,POI,AutoCruise"/>
 
 	<field name="ControlChain" units="bool" type="enum" options="false,true">
 		<elementnames>
diff --git a/shared/uavobjectdefinition/pathdesired.xml b/shared/uavobjectdefinition/pathdesired.xml
index 76f958779..a70604aa9 100644
--- a/shared/uavobjectdefinition/pathdesired.xml
+++ b/shared/uavobjectdefinition/pathdesired.xml
@@ -23,7 +23,7 @@
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="onchange" period="1000"/>
-        <logging updatemode="onchange" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="manual" period="0"/>
     </object>
 </xml>
diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml
index 963dd1440..44cab2fcd 100644
--- a/shared/uavobjectdefinition/stabilizationdesired.xml
+++ b/shared/uavobjectdefinition/stabilizationdesired.xml
@@ -6,7 +6,7 @@
         <field name="Yaw" units="degrees" type="float" elements="1"/>
         <field name="Thrust" units="%" type="float" elements="1"/>
 	<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
-        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"/>
+        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,AltitudeVario,CruiseControl"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>
diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml
index 0a3788b76..e0283c1cb 100644
--- a/shared/uavobjectdefinition/stabilizationstatus.xml
+++ b/shared/uavobjectdefinition/stabilizationstatus.xml
@@ -3,7 +3,7 @@
         <description>Contains status information to control submodules for stabilization.</description>
 
 
-	<field name="OuterLoop" units="" type="enum" options="Direct,Attitude,Rattitude,Weakleveling,Altitude,VerticalVelocity">
+	<field name="OuterLoop" units="" type="enum" options="Direct,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario">
 		<elementnames>
 			<elementname>Roll</elementname>
 			<elementname>Pitch</elementname>
diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml
index 6a7b690ce..785a152b9 100644
--- a/shared/uavobjectdefinition/systemalarms.xml
+++ b/shared/uavobjectdefinition/systemalarms.xml
@@ -45,8 +45,8 @@
 		</elementnames>
 	</field>
         <access gcs="readwrite" flight="readwrite"/>
-        <telemetrygcs acked="true" updatemode="manual" period="0"/>
-        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="onchange" period="0"/>
 	<logging updatemode="manual" period="0"/>
     </object>
 </xml>
diff --git a/shared/uavobjectdefinition/takeofflocation.xml b/shared/uavobjectdefinition/takeofflocation.xml
new file mode 100644
index 000000000..93eadddfc
--- /dev/null
+++ b/shared/uavobjectdefinition/takeofflocation.xml
@@ -0,0 +1,14 @@
+<xml>
+    <object name="TakeOffLocation" singleinstance="true" settings="true" category="Navigation">
+        <description>TakeOffLocation setting which contains the destination of a ReturnToBase operation</description>
+        <field name="North" units="m" type="float" elements="1" defaultvalue="0" />
+        <field name="East" units="m" type="float" elements="1" defaultvalue="0" />
+        <field name="Down" units="m" type="float" elements="1" defaultvalue="0" />
+        <field name="Mode" units="" type="enum" elements="1" options="ArmingLocation,FirstArmingLocation,Preset" defaultvalue="ArmingLocation"/>
+        <field name="Status" units="" type="enum" elements="1" options="Valid,Invalid" defaultvalue="Invalid"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>