Merge

flight/libraries/inc/paths.h

@@ -34,6 +34,6 @@ struct path_status {
     float correction_vector[3];
 };
 
-void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status);
+void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D);
 
 #endif

flight/libraries/inc/plans.h

@@ -63,11 +63,6 @@ void plan_setup_returnToBase();
  */
 void plan_setup_land();
 
-/**
- * @brief execute land
- */
-void plan_run_land();
-
 /**
  * @brief setup pathplanner/pathfollower for braking
  */
@@ -78,11 +73,29 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred);
 #define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN  2
 #define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT                   3
 
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH   0
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST    1
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN    2
+#define PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED                 3
+
+#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH       0
+#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST        1
+#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN        2
+#define PATHDESIRED_MODEPARAMETER_LAND_OPTIONS                    3
+
+#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE                0
+#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH       1
+
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND        0
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1            1
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2            2
+#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3            3
 /**
  * @brief setup pathfollower for positionvario
  */
 void plan_setup_CourseLock();
 void plan_setup_PositionRoam();
+void plan_setup_VelocityRoam();
 void plan_setup_HomeLeash();
 void plan_setup_AbsolutePosition();
 
@@ -91,6 +104,7 @@ void plan_setup_AbsolutePosition();
  */
 void plan_run_CourseLock();
 void plan_run_PositionRoam();
+void plan_run_VelocityRoam();
 void plan_run_HomeLeash();
 void plan_run_AbsolutePosition();
 

flight/libraries/insgps13state.c

@@ -33,25 +33,26 @@
 #include <math.h>
 #include <stdint.h>
 #include <pios_math.h>
+#include <mathmisc.h>
 
 // constants/macros/typdefs
 #define NUMX 13 // number of states, X is the state vector
 #define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
 #define NUMV 10 // number of measurements, v is the measurement noise vector
 #define NUMU 6 // number of deterministic inputs, U is the input vector
-
+#pragma GCC optimize "O3"
 // Private functions
 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
                           float Q[NUMW], float dT, float P[NUMX][NUMX]);
-void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
+static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
-                  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
+                         float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
-                  uint16_t SensorsUsed);
+                         uint16_t SensorsUsed);
-void RungeKutta(float X[NUMX], float U[NUMU], float dT);
+static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
-void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
+static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
-void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
+static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
-                 float G[NUMX][NUMW]);
+                        float G[NUMX][NUMW]);
-void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
+static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
-void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
+static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
 
 // Private variables
 
@@ -60,29 +61,29 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
 // LinearizeFG() and LinearizeH():
 //
 // usage F:        usage G:   usage H:
-// 0123456789abc  012345678  0123456789abc
+// -0123456789abc  012345678  0123456789abc
 // 0...X.........  .........  X............
 // 1....X........  .........  .X...........
 // 2.....X.......  .........  ..X..........
 // 3......XXXX...  ...XXX...  ...X.........
 // 4......XXXX...  ...XXX...  ....X........
 // 5......XXXX...  ...XXX...  .....X.......
-// 6.......XXXXXX  XXX......  ......XXXX...
+// 6.....ooXXXXXX  XXX......  ......XXXX...
-// 7......X.XXXXX  XXX......  ......XXXX...
+// 7.....oXoXXXXX  XXX......  ......XXXX...
-// 8......XX.XXXX  XXX......  ......XXXX...
+// 8.....oXXoXXXX  XXX......  ......XXXX...
-// 9......XXX.XXX  XXX......  ..X..........
+// 9.....oXXXoXXX  XXX......  ..X..........
-// a.............  ......X..
+// a.............  ......Xoo
-// b.............  .......X.
+// b.............  ......oXo
-// c.............  ........X
+// c.............  ......ooX
 
-static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 };
+static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 };
-static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
+static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
 
-static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
+static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
-static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
+static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
 
-static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
+static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
-static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
+static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
 
 static struct EKFData {
     // linearized system matrices
@@ -182,8 +183,8 @@ void INSGetP(float PDiag[NUMX])
     uint8_t i;
 
     // retrieve diagonal elements (aka state variance)
-    for (i = 0; i < NUMX; i++) {
+    if (PDiag != 0) {
-        if (PDiag != 0) {
+        for (i = 0; i < NUMX; i++) {
             PDiag[i] = ekf.P[i][i];
         }
     }
@@ -289,7 +290,7 @@ void INSSetMagNorth(float B[3])
 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
 {
     float U[6];
-    float qmag;
+    float invqmag;
 
     // rate gyro inputs in units of rad/s
     U[0] = gyro_data[0];
@@ -304,11 +305,11 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
     // EKF prediction step
     LinearizeFG(ekf.X, U, ekf.F, ekf.G);
     RungeKutta(ekf.X, U, dT);
-    qmag      = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
+    invqmag   = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
-    ekf.X[6] /= qmag;
+    ekf.X[6] *= invqmag;
-    ekf.X[7] /= qmag;
+    ekf.X[7] *= invqmag;
-    ekf.X[8] /= qmag;
+    ekf.X[8] *= invqmag;
-    ekf.X[9] /= qmag;
+    ekf.X[9] *= invqmag;
     // CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
 
     // Update Nav solution structure
@@ -373,8 +374,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt)
 void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
                    float BaroAlt, uint16_t SensorsUsed)
 {
-    float Z[10], Y[10];
+    float Z[10] = { 0 };
-    float Bmag, qmag;
+    float Y[10] = { 0 };
 
     // GPS Position in meters and in local NED frame
     Z[0] = Pos[0];
@@ -385,14 +386,15 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
     Z[3] = Vel[0];
     Z[4] = Vel[1];
     Z[5] = Vel[2];
-
     // magnetometer data in any units (use unit vector) and in body frame
-    Bmag =
+
-        sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
+
-              mag_data[2] * mag_data[2]);
+    if (SensorsUsed & MAG_SENSORS) {
-    Z[6] = mag_data[0] / Bmag;
+        float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
-    Z[7] = mag_data[1] / Bmag;
+        Z[6] = mag_data[0] * invBmag;
-    Z[8] = mag_data[2] / Bmag;
+        Z[7] = mag_data[1] * invBmag;
+        Z[8] = mag_data[2] * invBmag;
+    }
 
     // barometric altimeter in meters and in local NED frame
     Z[9] = BaroAlt;
@@ -401,12 +403,12 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
     LinearizeH(ekf.X, ekf.Be, ekf.H);
     MeasurementEq(ekf.X, ekf.Be, Y);
     SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
-    qmag       = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
-    ekf.X[6]  /= qmag;
-    ekf.X[7]  /= qmag;
-    ekf.X[8]  /= qmag;
-    ekf.X[9]  /= qmag;
 
+    float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
+    ekf.X[6]  *= invqmag;
+    ekf.X[7]  *= invqmag;
+    ekf.X[8]  *= invqmag;
+    ekf.X[9]  *= invqmag;
     // Update Nav solution structure
     Nav.Pos[0] = ekf.X[0];
     Nav.Pos[1] = ekf.X[1];
@@ -434,60 +436,78 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
 // The first Method is very specific to this implementation
 // ************************************************
 
-__attribute__((optimize("O3")))
 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
                           float Q[NUMW], float dT, float P[NUMX][NUMX])
 {
     // Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')]
 
-    float dT1  = 1.0f / dT; // multiplication is faster than division on fpu.
+    const float dT1  = 1.0f / dT; // multiplication is faster than division on fpu.
-    float dTsq = dT * dT;
+    const float dTsq = dT * dT;
 
     float Dummy[NUMX][NUMX];
     int8_t i;
+    int8_t k;
 
     for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
-        float *Firow   = F[i];
+        float *Firow = F[i];
-        float *Pirow   = P[i];
+        float *Pirow = P[i];
-        float *Dirow   = Dummy[i];
+        float *Dirow = Dummy[i];
-        int8_t Fistart = FrowMin[i];
+        const int8_t Fistart = FrowMin[i];
-        int8_t Fiend   = FrowMax[i];
+        const int8_t Fiend   = FrowMax[i];
         int8_t j;
+
         for (j = 0; j < NUMX; j++) {
             Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
-            int8_t k;
+        }
-            for (k = Fistart; k <= Fiend; k++) {
+        for (k = Fistart; k <= Fiend; k++) {
+            for (j = 0; j < NUMX; j++) {
                 Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
             }
         }
     }
     for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
-        float *Dirow   = Dummy[i];
+        float *Dirow = Dummy[i];
-        float *Girow   = G[i];
+        float *Girow = G[i];
-        float *Pirow   = P[i];
+        float *Pirow = P[i];
-        int8_t Gistart = GrowMin[i];
+        const int8_t Gistart = GrowMin[i];
-        int8_t Giend   = GrowMax[i];
+        const int8_t Giend   = GrowMax[i];
         int8_t j;
-        for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
-            float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
 
-            {
+
-                float *Fjrow   = F[j];
+        for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
-                int8_t Fjstart = FrowMin[j];
+            float Ptmp         = Dirow[j] * dT1; // Pnew = Dummy / T ...
-                int8_t Fjend   = FrowMax[j];
+
-                int8_t k;
+            const float *Fjrow = F[j];
-                for (k = Fjstart; k <= Fjend; k++) {
+            int8_t Fjstart     = FrowMin[j];
-                    Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
+            int8_t Fjend       = FrowMax[j];
-                }
+            k = Fjstart;
+
+            while (k <= Fjend - 3) {
+                Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
+                Ptmp += Dirow[k + 1] * Fjrow[k + 1];
+                Ptmp += Dirow[k + 2] * Fjrow[k + 2];
+                Ptmp += Dirow[k + 3] * Fjrow[k + 3];
+                k    += 4;
+            }
+            while (k <= Fjend) {
+                Ptmp += Dirow[k] * Fjrow[k];
+                k++;
             }
 
-            {
+            float *Gjrow = G[j];
-                float *Gjrow   = G[j];
+            const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
-                int8_t Gjstart = MAX(Gistart, GrowMin[j]);
+            const int8_t Gjend   = MIN(Giend, GrowMax[j]);
-                int8_t Gjend   = MIN(Giend, GrowMax[j]);
+            k = Gjstart;
-                int8_t k;
+            while (k <= Gjend - 2) {
-                for (k = Gjstart; k <= Gjend; k++) {
+                Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
-                    Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
+                Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
+                Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
+                k    += 3;
+            }
+            if (k <= Gjend) {
+                Ptmp += Q[k] * Girow[k] * Gjrow[k];
+                if (k <= Gjend - 1) {
+                    Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
                 }
             }
 
@@ -511,7 +531,6 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
 // The SensorsUsed variable is a bitwise mask indicating which sensors
 // should be used in the update.
 // ************************************************
-
 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
                   float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
                   uint16_t SensorsUsed)
@@ -524,7 +543,10 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
         if (SensorsUsed & (0x01 << m)) { // use this sensor for update
             for (j = 0; j < NUMX; j++) { // Find Hp = H*P
                 HP[j] = 0;
-                for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
+            }
+
+            for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
+                for (j = 0; j < NUMX; j++) { // Find Hp = H*P
                     HP[j] += H[m][k] * P[k][j];
                 }
             }
@@ -532,14 +554,13 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
             for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
                 HPHR += HP[k] * H[m][k];
             }
-
+            float invHPHR = 1.0f / HPHR;
             for (k = 0; k < NUMX; k++) {
-                Km[k] = HP[k] / HPHR; // find K = HP/HPHR
+                Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
             }
             for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
                 for (j = i; j < NUMX; j++) {
-                    P[i][j] = P[j][i] =
+                    P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
-                                  P[i][j] - Km[i] * HP[j];
                 }
             }
 
@@ -560,8 +581,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
 
 void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 {
-    float dT2 =
+    const float dT2 = dT / 2.0f;
-        dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
+    float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
     uint8_t i;
 
     for (i = 0; i < NUMX; i++) {
@@ -585,7 +606,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
     for (i = 0; i < NUMX; i++) {
         X[i] =
             Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
-                             K4[i]) / 6.0f;
+                             K4[i]) * (1.0f / 6.0f);
     }
 }
 
@@ -608,7 +629,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
 // H is output of LinearizeH(), all elements not set should be zero
 // ************************************************
 
-void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
+static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
 {
     float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
 

flight/libraries/math/pid.c

@@ -143,3 +143,95 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
     pid->d    = d;
     pid->iLim = iLim;
 }
+
+
+/**
+ * Configure the settings for a pid2 structure
+ * @param[out] pid The PID2 structure to configure
+ * @param[in] kp proportional gain
+ * @param[in] ki integral gain.  Time constant Ti = kp/ki
+ * @param[in] kd derivative gain. Time constant Td = kd/kp
+ * @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20
+ * @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2
+ * @param[in] dt delta time increment
+ * @param[in] beta setpoint weight on setpoint in P component.  beta=1 error feedback. beta=0 smoothes out response to changes in setpoint
+ * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
+ * @param[in] va constant for compute of actuator output for check against limits for antiwindup
+ * @param[in] vb multiplier for compute of actuator output for check against limits for anti-windup
+ */
+void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb)
+{
+    pid->reconfigure = true;
+    pid->u0   = u0;
+    pid->va   = va;
+    pid->vb   = vb;
+    pid->kp   = kp;
+    pid->beta = beta; // setpoint weight on proportional term
+
+    pid->bi   = ki * dT;
+    pid->br   = kt * dT / vb;
+
+    pid->ad   = Tf / (Tf + dT);
+    pid->bd   = kd / (Tf + dT);
+}
+
+/**
+ * Achieve a bumpless transfer and trigger initialisation of I term
+ * @param[out] pid The PID structure to configure
+ * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
+ */
+void pid2_transfer(struct pid2 *pid, float u0)
+{
+    pid->reconfigure = true;
+    pid->u0 = u0;
+}
+
+/**
+ * pid controller with setpoint weighting, anti-windup, with a low-pass filtered derivative on the process variable
+ * See "Feedback Systems" for an explanation
+ * @param[out] pid The PID structure to configure
+ * @param[in] r setpoint
+ * @param[in] y process variable
+ * @param[in] ulow lower limit on actuator
+ * @param[in] uhigh upper limit on actuator
+ */
+float pid2_apply(
+    struct pid2 *pid,
+    const float r,
+    const float y,
+    const float ulow,
+    const float uhigh)
+{
+    // on reconfigure ensure bumpless transfer
+    // http://www.controlguru.com/2008/021008.html
+    if (pid->reconfigure) {
+        pid->reconfigure = false;
+
+        // initialise derivative terms
+        pid->yold = y;
+        pid->D    = 0.0f;
+
+        // t=0, u=u0, y=y0, v=u
+        pid->I    = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
+    }
+
+    // compute proportional part
+    pid->P = pid->kp * (pid->beta * r - y);
+
+    // update derivative part
+    pid->D = pid->ad * pid->D - pid->bd * (y - pid->yold);
+
+    // compute temporary output
+    float v = pid->va + pid->vb * (pid->P + pid->I + pid->D);
+
+    // simulate actuator saturation
+    float u = boundf(v, ulow, uhigh);
+
+    // update integral
+    pid->I    = pid->I + pid->bi * (r - y) + pid->br * (u - v);
+
+    // update old process output
+    pid->yold = y;
+
+    return u;
+}

flight/libraries/math/pid.h

@@ -44,7 +44,25 @@ struct pid {
     float lastDer;
 };
 
-typedef struct pid_scaler {
+// pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control
+struct pid2 {
+    float   u0;
+    float   va;
+    float   vb;
+    float   kp;
+    float   bi;
+    float   ad;
+    float   bd;
+    float   br;
+    float   beta;
+    float   yold;
+    float   P;
+    float   I;
+    float   D;
+    uint8_t reconfigure;
+};
+
+typedef struct pid_scaler_s {
     float p;
     float i;
     float d;
@@ -57,4 +75,9 @@ void pid_zero(struct pid *pid);
 void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
 void pid_configure_derivative(float cutoff, float gamma);
 
+// Methods for use with pid2 structure
+void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb);
+void pid2_transfer(struct pid2 *pid, float u0);
+float pid2_apply(struct pid2 *pid, const float r, const float y, float ulow, float uhigh);
+
 #endif /* PID_H */

flight/libraries/paths.c

@@ -44,33 +44,27 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta
  * @param[in] cur_point Current location
  * @param[out] status Structure containing progress along path and deviation
  */
-void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
+void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
 {
     switch (path->Mode) {
-    case PATHDESIRED_MODE_BRAKE: // should never get here...
+    case PATHDESIRED_MODE_BRAKE:
-    case PATHDESIRED_MODE_FLYVECTOR:
+    case PATHDESIRED_MODE_FOLLOWVECTOR:
-        return path_vector(path, cur_point, status, true);
+        return path_vector(path, cur_point, status, mode3D);
 
         break;
-    case PATHDESIRED_MODE_DRIVEVECTOR:
+    case PATHDESIRED_MODE_CIRCLERIGHT:
-        return path_vector(path, cur_point, status, false);
+        return path_circle(path, cur_point, status, mode3D);
 
         break;
-    case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+    case PATHDESIRED_MODE_CIRCLELEFT:
-    case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+        return path_circle(path, cur_point, status, mode3D);
-        return path_circle(path, cur_point, status, 1);
 
         break;
-    case PATHDESIRED_MODE_FLYCIRCLELEFT:
+    case PATHDESIRED_MODE_GOTOENDPOINT:
-    case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+        return path_endpoint(path, cur_point, status, mode3D);
-        return path_circle(path, cur_point, status, 0);
 
         break;
-    case PATHDESIRED_MODE_FLYENDPOINT:
+    case PATHDESIRED_MODE_LAND:
-        return path_endpoint(path, cur_point, status, true);
-
-        break;
-    case PATHDESIRED_MODE_DRIVEENDPOINT:
     default:
         // use the endpoint as default failsafe if called in unknown modes
         return path_endpoint(path, cur_point, status, false);

flight/libraries/plans.c

@@ -40,6 +40,7 @@
 #include <manualcontrolcommand.h>
 #include <attitudestate.h>
 #include <vtolpathfollowersettings.h>
+#include <stabilizationbank.h>
 #include <sin_lookup.h>
 
 #define UPDATE_EXPECTED 0.02f
@@ -47,6 +48,31 @@
 #define UPDATE_MAX      1.0f
 #define UPDATE_ALPHA    1.0e-2f
 
+
+static float applyExpo(float value, float expo);
+
+
+static float applyExpo(float value, float expo)
+{
+    // note: fastPow makes a small error, therefore result needs to be bound
+    float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
+
+    // magic number scales expo
+    // so that
+    // expo=100 yields value**10
+    // expo=0 yields value**1
+    // expo=-100 yields value**(1/10)
+    // (pow(2.0,1/100)~=1.00695)
+    if (value > 0.0f) {
+        return boundf(fastPow(value, exp), 0.0f, 1.0f);
+    } else if (value < -0.0f) {
+        return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
+    } else {
+        return 0.0f;
+    }
+}
+
+
 /**
  * @brief initialize UAVOs and structs used by this library
  */
@@ -61,6 +87,7 @@ void plan_initialize()
     ManualControlCommandInitialize();
     VelocityStateInitialize();
     VtolPathFollowerSettingsInitialize();
+    StabilizationBankInitialize();
 }
 
 /**
@@ -85,7 +112,7 @@ void plan_setup_positionHold()
     pathDesired.Start.Down       = positionState.Down;
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
-    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+    pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 
     PathDesiredSet(&pathDesired);
 }
@@ -126,49 +153,61 @@ void plan_setup_returnToBase()
 
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
-    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    uint8_t ReturnToBaseNextCommand;
+    FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1]     = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2]     = 0.0f;
+    pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3]     = 0.0f;
+    pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 
     PathDesiredSet(&pathDesired);
 }
 
-static PiOSDeltatimeConfig landdT;
+
+static void plan_setup_land_helper(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+
+    FlightModeSettingsLandingVelocityGet(&velocity_down);
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]  = velocity_down;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_LAND;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
+}
+
 void plan_setup_land()
 {
-    float descendspeed;
-
-    plan_setup_positionHold();
-
-    FlightModeSettingsLandingVelocityGet(&descendspeed);
     PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
+
-    pathDesired.StartingVelocity = descendspeed;
+    plan_setup_land_helper(&pathDesired);
-    pathDesired.EndingVelocity   = descendspeed;
     PathDesiredSet(&pathDesired);
-    PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 }
 
-/**
+static void plan_setup_land_from_velocityroam()
- * @brief execute land
- */
-void plan_run_land()
 {
-    float downPos, descendspeed;
+    plan_setup_land();
-    PathDesiredEndData pathDesiredEnd;
+    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
-
+    assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
-    PositionStateDownGet(&downPos); // current down position
+    FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
-    PathDesiredEndGet(&pathDesiredEnd); // desired position
-    PathDesiredEndingVelocityGet(&descendspeed);
-
-    // desired position is updated to match the desired descend speed but don't run ahead
-    // too far if the current position can't keep up. This normaly means we have landed.
-    if (pathDesiredEnd.Down - downPos < 10) {
-        pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
-    }
-
-    PathDesiredEndSet(&pathDesiredEnd);
 }
 
-
 /**
  * @brief positionvario functionality
  */
@@ -197,6 +236,14 @@ void plan_setup_PositionRoam()
     plan_setup_PositionVario();
 }
 
+void plan_setup_VelocityRoam()
+{
+    vario_control_lowpass[0] = 0.0f;
+    vario_control_lowpass[1] = 0.0f;
+    vario_control_lowpass[2] = 0.0f;
+    AttitudeStateYawGet(&vario_course);
+}
+
 void plan_setup_HomeLeash()
 {
     plan_setup_PositionVario();
@@ -340,6 +387,9 @@ static void plan_run_PositionVario(vario_type type)
             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;
+
+            // set mode explicitly
+
             PathDesiredSet(&pathDesired);
         }
     } else {
@@ -381,6 +431,106 @@ static void plan_run_PositionVario(vario_type type)
     }
 }
 
+void plan_run_VelocityRoam()
+{
+    // float alpha;
+    PathDesiredData pathDesired;
+    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
+    FlightStatusFlightModeOptions flightMode;
+
+    PathDesiredGet(&pathDesired);
+    FlightModeSettingsPositionHoldOffsetData offset;
+    FlightModeSettingsPositionHoldOffsetGet(&offset);
+    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
+    FlightStatusFlightModeGet(&flightMode);
+    StabilizationBankData stabSettings;
+    StabilizationBankGet(&stabSettings);
+
+    ManualControlCommandData cmd;
+    ManualControlCommandGet(&cmd);
+
+    cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
+    cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
+    cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
+
+    bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
+
+    if (!flagRollPitchHasInput) {
+        // no movement desired, re-enter positionHold at current start-position
+        if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
+            // initiate braking and change assisted control flight mode to braking
+            if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+                // avoid brake then hold sequence to continue descent.
+                plan_setup_land_from_velocityroam();
+            } else {
+                plan_setup_assistedcontrol(false);
+            }
+        }
+        // otherwise nothing to do in braking/hold modes
+    } else {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+
+        // Revert assist control state to primary, which in this case implies
+        // we are in roaming state (a GPS vector assisted velocity roam)
+        assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
+
+        // Calculate desired velocity in each direction
+        float angle;
+        AttitudeStateYawGet(&angle);
+        angle = DEG2RAD(angle);
+        float cos_angle  = cosf(angle);
+        float sine_angle = sinf(angle);
+        float rotated[2] = {
+            -cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
+            -cmd.Pitch * sine_angle + cmd.Roll * cos_angle
+        };
+        // flip pitch to have pitch down (away) point north
+        float horizontalVelMax;
+        float verticalVelMax;
+        VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
+        VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
+        float velocity_north = rotated[0] * horizontalVelMax;
+        float velocity_east  = rotated[1] * horizontalVelMax;
+        float velocity_down  = 0.0f;
+
+        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+            FlightModeSettingsLandingVelocityGet(&velocity_down);
+        }
+
+        float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
+        velocity = sqrtf(velocity);
+
+        // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
+        // expect new stick input
+        // if two stick input pilot is fighting wind manually and we use fly by velocity
+        // in reality setting velocity desired to zero will fight wind anyway.
+
+        pathDesired.Start.North = positionState.North;
+        pathDesired.Start.East  = positionState.East;
+        pathDesired.Start.Down  = positionState.Down;
+        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
+        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]  = velocity_east;
+        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]  = velocity_down;
+
+        pathDesired.End.North = positionState.North;
+        pathDesired.End.East  = positionState.East;
+        pathDesired.End.Down  = positionState.Down;
+
+        pathDesired.StartingVelocity = velocity;
+        pathDesired.EndingVelocity   = velocity;
+        pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
+        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+            pathDesired.Mode = PATHDESIRED_MODE_LAND;
+            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
+        } else {
+            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
+        }
+        PathDesiredSet(&pathDesired);
+        FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
+    }
+}
+
 void plan_run_CourseLock()
 {
     plan_run_PositionVario(COURSE);
@@ -437,7 +587,7 @@ void plan_setup_AutoCruise()
     pathDesired.Start.Down       = pathDesired.End.Down;
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
-    pathDesired.Mode             = PATHDESIRED_MODE_FLYENDPOINT;
+    pathDesired.Mode             = PATHDESIRED_MODE_GOTOENDPOINT;
 
     PathDesiredSet(&pathDesired);
 
@@ -528,22 +678,27 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
 
     PositionStateGet(&positionState);
     PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
 
     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
     FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
 
     if (timeout_occurred) {
-        pathDesired.End.North        = positionState.North;
+        FlightStatusFlightModeOptions flightMode;
-        pathDesired.End.East         = positionState.East;
+        FlightStatusFlightModeGet(&flightMode);
-        pathDesired.End.Down         = positionState.Down;
+        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
-        pathDesired.Start.North      = positionState.North;
+            plan_setup_land_helper(&pathDesired);
-        pathDesired.Start.East       = positionState.East;
+        } else {
-        pathDesired.Start.Down       = positionState.Down;
+            pathDesired.End.North        = positionState.North;
-        pathDesired.StartingVelocity = 0.0f;
+            pathDesired.End.East         = positionState.East;
-        pathDesired.EndingVelocity   = 0.0f;
+            pathDesired.End.Down         = positionState.Down;
-        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            pathDesired.Start.North      = positionState.North;
-        assistedControlFlightMode    = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
+            pathDesired.Start.East       = positionState.East;
+            pathDesired.Start.Down       = positionState.Down;
+            pathDesired.StartingVelocity = 0.0f;
+            pathDesired.EndingVelocity   = 0.0f;
+            pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+        }
+        assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
     } else {
         VelocityStateData velocityState;
         VelocityStateGet(&velocityState);

flight/libraries/sanitycheck.c

@@ -118,7 +118,7 @@ int32_t configuration_check()
             ADDSEVERITY(navCapableFusion);
         }
 
-        switch (modes[i]) {
+        switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
             ADDSEVERITY(!gps_assisted);
             ADDSEVERITY(!multirotor);
@@ -143,24 +143,18 @@ int32_t configuration_check()
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
         {
-            // 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);
             ADDSEVERITY(!gps_assisted);
         }
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
+        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
             ADDSEVERITY(!coptercontrol);
             ADDSEVERITY(navCapableFusion);
             break;
 
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:

flight/modules/Actuator/actuator.c

@@ -367,13 +367,15 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
             }
 
             if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
-                if (curve1 < 0.0f) {
+                float nonreversible_curve1 = curve1;
-                    curve1 = 0.0f;
+                float nonreversible_curve2 = curve2;
+                if (nonreversible_curve1 < 0.0f) {
+                    nonreversible_curve1 = 0.0f;
                 }
-                if (curve2 < 0.0f) {
+                if (nonreversible_curve2 < 0.0f) {
-                    curve2 = 0.0f;
+                    nonreversible_curve2 = 0.0f;
                 }
-                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
+                status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds);
                 // If not armed or motors aren't meant to spin all the time
                 if (!armed ||
                     (!spinWhileArmed && !positiveThrottle)) {
@@ -586,11 +588,7 @@ static float MixerCurveFullRangeAbsolute(const float input, const float *curve,
 
     scale -= (float)idx1; // remainder
     if (curve[0] < -1) {
-        return input;
+        return abs_input;
-    }
-    if (idx1 < 0) {
-        idx1  = 0; // clamp to lowest entry in table
-        scale = 0;
     }
     int idx2 = idx1 + 1;
     if (idx2 >= elements) {

flight/modules/ManualControl/armhandler.c

@@ -310,6 +310,8 @@ static bool okToArm(void)
         return true;
 
         break;
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
+        return false;
 
     default:
         return false;

flight/modules/ManualControl/manualcontrol.c

@@ -397,9 +397,15 @@ static void manualControlTask(void)
         break;
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 
+    // During development the assistedcontrol implementation is optional and set
+    // set in stabi settings.  Once if we decide to always have this on, it can
+    // can be directly set here...i.e. set the flight mode assist as required.
     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+    case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
         if (newFlightModeAssist) {
+            // Set the default thrust state
             switch (newFlightModeAssist) {
             case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@@ -412,28 +418,14 @@ static void manualControlTask(void)
                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
                 break;
             }
-
-            switch (newAssistedControlState) {
-            case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
-                newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
-                break;
-            case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
-                newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
-                break;
-            case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
-                newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
-                break;
-            }
         }
         handler = &handler_PATHFOLLOWER;
         break;
 
     case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
-    case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
     case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-    case FLIGHTSTATUS_FLIGHTMODE_LAND:
     case FLIGHTSTATUS_FLIGHTMODE_POI:
     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
         handler = &handler_PATHFOLLOWER;
@@ -528,11 +520,15 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
             thrustMode = modeSettings->Stabilization6Settings.Thrust;
             break;
         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            // we hard code the "GPS Assisted" PostionHold to use alt-vario which
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
+            // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
             // is a more appropriate throttle mode.  "GPSAssist" adds braking
             // and a better throttle management to the standard Position Hold.
             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
             break;
+        case FLIGHTSTATUS_FLIGHTMODE_LAND:
+            thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
+            break;
 
             // other modes will use cruisecontrol as default
         }

flight/modules/ManualControl/pathfollowerhandler.c

@@ -57,7 +57,9 @@ void pathFollowerHandler(bool newinit)
 
     uint8_t flightMode;
     uint8_t assistedControlFlightMode;
+    uint8_t flightModeAssist;
     FlightStatusFlightModeGet(&flightMode);
+    FlightStatusFlightModeAssistGet(&flightModeAssist);
     FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
 
     if (newinit) {
@@ -67,8 +69,9 @@ void pathFollowerHandler(bool newinit)
             plan_setup_returnToBase();
             break;
         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
+            if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
-                // Just initiated braking after returning from stabi control
+                (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
+                // Switch from primary (just entered this PH flight mode) into brake
                 plan_setup_assistedcontrol(false);
             } else {
                 plan_setup_positionHold();
@@ -78,7 +81,11 @@ void pathFollowerHandler(bool newinit)
             plan_setup_CourseLock();
             break;
         case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
-            plan_setup_PositionRoam();
+            if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+                plan_setup_PositionRoam();
+            } else {
+                plan_setup_VelocityRoam();
+            }
             break;
         case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
             plan_setup_HomeLeash();
@@ -88,7 +95,11 @@ void pathFollowerHandler(bool newinit)
             break;
 
         case FLIGHTSTATUS_FLIGHTMODE_LAND:
-            plan_setup_land();
+            if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+                plan_setup_land();
+            } else {
+                plan_setup_VelocityRoam();
+            }
             break;
         case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
             plan_setup_AutoCruise();
@@ -117,7 +128,11 @@ void pathFollowerHandler(bool newinit)
         plan_run_CourseLock();
         break;
     case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
-        plan_run_PositionRoam();
+        if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+            plan_run_PositionRoam();
+        } else {
+            plan_run_VelocityRoam();
+        }
         break;
     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
         plan_run_HomeLeash();
@@ -126,7 +141,9 @@ void pathFollowerHandler(bool newinit)
         plan_run_AbsolutePosition();
         break;
     case FLIGHTSTATUS_FLIGHTMODE_LAND:
-        plan_run_land();
+        if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+            plan_run_VelocityRoam();
+        }
         break;
     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
         plan_run_AutoCruise();

flight/modules/PathFollower/fixedwingflycontroller.cpp

@@ -0,0 +1,618 @@
+/*
+ ******************************************************************************
+ *
+ * @file       FixedWingFlyController.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Fixed wing fly controller implementation
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <fixedwingpathfollowersettings.h>
+#include <fixedwingpathfollowerstatus.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "fixedwingflycontroller.h"
+
+// Private constants
+
+// pointer to a singleton instance
+FixedWingFlyController *FixedWingFlyController::p_inst = 0;
+
+FixedWingFlyController::FixedWingFlyController()
+    : fixedWingSettings(NULL), mActive(false), mMode(0), indicatedAirspeedStateBias(0.0f)
+{}
+
+// Called when mode first engaged
+void FixedWingFlyController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        resetGlobals();
+        mMode   = pathDesired->Mode;
+    }
+}
+
+uint8_t FixedWingFlyController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t FixedWingFlyController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void FixedWingFlyController::ObjectiveUpdated(void)
+{}
+
+void FixedWingFlyController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        resetGlobals();
+    }
+}
+
+
+void FixedWingFlyController::SettingsUpdated(void)
+{
+    // fixed wing PID only
+    pid_configure(&PIDposH[0], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
+    pid_configure(&PIDposH[1], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
+    pid_configure(&PIDposV, fixedWingSettings->VerticalPosP, 0.0f, 0.0f, 0.0f);
+
+    pid_configure(&PIDcourse, fixedWingSettings->CoursePI.Kp, fixedWingSettings->CoursePI.Ki, 0.0f, fixedWingSettings->CoursePI.ILimit);
+    pid_configure(&PIDspeed, fixedWingSettings->SpeedPI.Kp, fixedWingSettings->SpeedPI.Ki, 0.0f, fixedWingSettings->SpeedPI.ILimit);
+    pid_configure(&PIDpower, fixedWingSettings->PowerPI.Kp, fixedWingSettings->PowerPI.Ki, 0.0f, fixedWingSettings->PowerPI.ILimit);
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingFlyController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
+{
+    PIOS_Assert(ptr_fixedWingSettings);
+
+    fixedWingSettings = ptr_fixedWingSettings;
+
+    resetGlobals();
+
+    return 0;
+}
+
+/**
+ * reset integrals
+ */
+void FixedWingFlyController::resetGlobals()
+{
+    pid_zero(&PIDposH[0]);
+    pid_zero(&PIDposH[1]);
+    pid_zero(&PIDposV);
+    pid_zero(&PIDcourse);
+    pid_zero(&PIDspeed);
+    pid_zero(&PIDpower);
+    pathStatus->path_time = 0.0f;
+}
+
+void FixedWingFlyController::UpdateAutoPilot()
+{
+    uint8_t result = updateAutoPilotFixedWing();
+
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+/**
+ * fixed wing autopilot:
+ * straight forward:
+ * 1. update path velocity for limited motion crafts
+ * 2. update attitude according to default fixed wing pathfollower algorithm
+ */
+uint8_t FixedWingFlyController::updateAutoPilotFixedWing()
+{
+    updatePathVelocity(fixedWingSettings->CourseFeedForward, true);
+    return updateFixedDesiredAttitude();
+}
+
+/**
+ * Compute desired velocity from the current position and path
+ */
+void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    VelocityStateData velocityState;
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
+
+    // look ahead kFF seconds
+    float cur[3]   = { positionState.North + (velocityState.North * kFF),
+                       positionState.East + (velocityState.East * kFF),
+                       positionState.Down + (velocityState.Down * kFF) };
+    struct path_status progress;
+    path_progress(pathDesired, cur, &progress, true);
+
+    // calculate velocity - can be zero if waypoints are too close
+    velocityDesired.North = progress.path_vector[0];
+    velocityDesired.East  = progress.path_vector[1];
+    velocityDesired.Down  = progress.path_vector[2];
+
+    if (limited &&
+        // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
+        // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
+        // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
+        // leading to an S-shape snake course the wrong way
+        // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
+        // turn steep unless there is enough space complete the turn before crossing the flightpath
+        // 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 everything )
+        // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
+        // calculating angles < 90 degrees through dot products
+        (vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
+        ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
+        ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
+        ;
+    } else {
+        // calculate correction
+        velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT);
+        velocityDesired.East  += pid_apply(&PIDposH[1], progress.correction_vector[1], dT);
+    }
+    velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT);
+
+    // update pathstatus
+    pathStatus->error     = progress.error;
+    pathStatus->fractional_progress  = progress.fractional_progress;
+    pathStatus->path_direction_north = progress.path_vector[0];
+    pathStatus->path_direction_east  = progress.path_vector[1];
+    pathStatus->path_direction_down  = progress.path_vector[2];
+
+    pathStatus->correction_direction_north = progress.correction_vector[0];
+    pathStatus->correction_direction_east  = progress.correction_vector[1];
+    pathStatus->correction_direction_down  = progress.correction_vector[2];
+
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+
+/**
+ * Compute desired attitude from the desired velocity for fixed wing craft
+ */
+uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
+{
+    uint8_t result = 1;
+
+    const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
+
+    VelocityDesiredData velocityDesired;
+    VelocityStateData velocityState;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
+    AirspeedStateData airspeedState;
+    SystemSettingsData systemSettings;
+
+    float groundspeedProjection;
+    float indicatedAirspeedState;
+    float indicatedAirspeedDesired;
+    float airspeedError;
+
+    float pitchCommand;
+
+    float descentspeedDesired;
+    float descentspeedError;
+    float powerCommand;
+
+    float airspeedVector[2];
+    float fluidMovement[2];
+    float courseComponent[2];
+    float courseError;
+    float courseCommand;
+
+    FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
+
+    VelocityStateGet(&velocityState);
+    StabilizationDesiredGet(&stabDesired);
+    VelocityDesiredGet(&velocityDesired);
+    AttitudeStateGet(&attitudeState);
+    AirspeedStateGet(&airspeedState);
+    SystemSettingsGet(&systemSettings);
+
+    /**
+     * Compute speed error and course
+     */
+    // missing sensors for airspeed-direction we have to assume within
+    // reasonable error that measured airspeed is actually the airspeed
+    // component in forward pointing direction
+    // airspeedVector is normalized
+    airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
+    airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
+
+    // current ground speed projected in forward direction
+    groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
+
+    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
+    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
+    // than airspeed and gps sensors alone
+    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
+
+    // fluidMovement is a vector describing the aproximate movement vector of
+    // the surrounding fluid in 2d space (aka wind vector)
+    fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
+    fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
+
+    // calculate the movement vector we need to fly to reach velocityDesired -
+    // taking fluidMovement into account
+    courseComponent[0] = velocityDesired.North - fluidMovement[0];
+    courseComponent[1] = velocityDesired.East - fluidMovement[1];
+
+    indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
+                                      fixedWingSettings->HorizontalVelMin,
+                                      fixedWingSettings->HorizontalVelMax);
+
+    // if we could fly at arbitrary speeds, we'd just have to move towards the
+    // courseComponent vector as previously calculated and we'd be fine
+    // unfortunately however we are bound by min and max air speed limits, so
+    // we need to recalculate the correct course to meet at least the
+    // velocityDesired vector direction at our current speed
+    // this overwrites courseComponent
+    bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
+
+    // Error condition: wind speed too high, we can't go where we want anymore
+    fixedWingPathFollowerStatus.Errors.Wind = 0;
+    if ((!valid) &&
+        fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Wind = 1;
+        result = 0;
+    }
+
+    // Airspeed error
+    airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
+
+    // Vertical speed error
+    descentspeedDesired = boundf(
+        velocityDesired.Down,
+        -fixedWingSettings->VerticalVelMax,
+        fixedWingSettings->VerticalVelMax);
+    descentspeedError = descentspeedDesired - velocityState.Down;
+
+    // Error condition: plane too slow or too fast
+    fixedWingPathFollowerStatus.Errors.Highspeed = 0;
+    fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
+    if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
+        fixedWingPathFollowerStatus.Errors.Overspeed = 1;
+        result = 0;
+    }
+    if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
+        fixedWingPathFollowerStatus.Errors.Highspeed = 1;
+        result = 0;
+    }
+    if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
+        fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
+        result = 0;
+    }
+    if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
+        fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
+        result = 0;
+    }
+
+    /**
+     * Compute desired thrust command
+     */
+
+    // Compute the cross feed from vertical speed to pitch, with saturation
+    float speedErrorToPowerCommandComponent = boundf(
+        (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
+        -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
+        fixedWingSettings->AirspeedToPowerCrossFeed.Max
+        );
+
+    // Compute final thrust response
+    powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
+                   speedErrorToPowerCommandComponent;
+
+    // Output internal state to telemetry
+    fixedWingPathFollowerStatus.Error.Power    = descentspeedError;
+    fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
+    fixedWingPathFollowerStatus.Command.Power  = powerCommand;
+
+    // set thrust
+    stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
+                                fixedWingSettings->ThrustLimit.Min,
+                                fixedWingSettings->ThrustLimit.Max);
+
+    // Error condition: plane cannot hold altitude at current speed.
+    fixedWingPathFollowerStatus.Errors.Lowpower = 0;
+    if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
+        velocityState.Down > 0.0f && // we ARE going down
+        descentspeedDesired < 0.0f && // we WANT to go up
+        airspeedError > 0.0f && // we are too slow already
+        fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Lowpower = 1;
+        result = 0;
+    }
+    // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
+    fixedWingPathFollowerStatus.Errors.Highpower = 0;
+    if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
+        velocityState.Down < 0.0f && // we ARE going up
+        descentspeedDesired > 0.0f && // we WANT to go down
+        airspeedError < 0.0f && // we are too fast already
+        fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Highpower = 1;
+        result = 0;
+    }
+
+    /**
+     * Compute desired pitch command
+     */
+    // Compute the cross feed from vertical speed to pitch, with saturation
+    float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
+                                                        -fixedWingSettings->VerticalToPitchCrossFeed.Max,
+                                                        fixedWingSettings->VerticalToPitchCrossFeed.Max
+                                                        );
+
+    // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
+    pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
+
+    fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
+    fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
+    fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
+
+    stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
+                               fixedWingSettings->PitchLimit.Min,
+                               fixedWingSettings->PitchLimit.Max);
+
+    // Error condition: high speed dive
+    fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
+    if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
+        velocityState.Down > 0.0f && // we ARE going down
+        descentspeedDesired < 0.0f && // we WANT to go up
+        airspeedError < 0.0f && // we are too fast already
+        fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
+        fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
+        result = 0;
+    }
+
+    /**
+     * Compute desired roll command
+     */
+    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
+
+    if (courseError < -180.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f) {
+        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 + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll > 0.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll < 0.0f) {
+        courseError -= 360.0f;
+    }
+
+    courseCommand = pid_apply(&PIDcourse, courseError, dT);
+
+    fixedWingPathFollowerStatus.Error.Course    = courseError;
+    fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
+    fixedWingPathFollowerStatus.Command.Course  = courseCommand;
+
+    stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
+                              courseCommand,
+                              fixedWingSettings->RollLimit.Min,
+                              fixedWingSettings->RollLimit.Max);
+
+    // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
+
+
+    /**
+     * Compute desired yaw command
+     */
+    // TODO implement raw control mode for yaw and base on Accels.Y
+    stabDesired.Yaw = 0.0f;
+
+
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+
+    StabilizationDesiredSet(&stabDesired);
+
+    FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
+
+    return result;
+}
+
+
+/**
+ * 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
+ */
+bool FixedWingFlyController::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 = vector_lengthf(F, 2);
+
+    // normalize Cn=V/|V|, |V| must be >0
+    float v = vector_lengthf(V, 2);
+    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;
+    }
+}
+
+
+void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    AirspeedStateData airspeedState;
+    VelocityStateData velocityState;
+
+    AirspeedStateGet(&airspeedState);
+    VelocityStateGet(&velocityState);
+    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 - 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.
+}

flight/modules/PathFollower/grounddrivecontroller.cpp

@@ -0,0 +1,303 @@
+/*
+ ******************************************************************************
+ *
+ * @file       grounddrivecontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Ground drive controller
+ *		the required PathDesired LAND mode.
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <groundpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <pathsummary.h>
+#include <statusgrounddrive.h>
+}
+
+// C++ includes
+#include "grounddrivecontroller.h"
+
+// Private constants
+
+// pointer to a singleton instance
+GroundDriveController *GroundDriveController::p_inst = 0;
+
+GroundDriveController::GroundDriveController()
+    : groundSettings(0), mActive(false), mMode(0)
+{}
+
+// Called when mode first engaged
+void GroundDriveController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        controlNE.Activate();
+        mMode   = pathDesired->Mode;
+    }
+}
+
+uint8_t GroundDriveController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t GroundDriveController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void GroundDriveController::ObjectiveUpdated(void)
+{}
+
+void GroundDriveController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        controlNE.Deactivate();
+    }
+}
+
+
+void GroundDriveController::SettingsUpdated(void)
+{
+    const float dT = groundSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP);
+    controlNE.UpdateParameters(groundSettings->SpeedPI.Kp,
+                               groundSettings->SpeedPI.Ki,
+                               groundSettings->SpeedPI.Kd,
+                               groundSettings->SpeedPI.Beta,
+                               dT,
+                               groundSettings->HorizontalVelMax);
+
+
+    // max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse.
+    controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings)
+{
+    PIOS_Assert(ptr_groundSettings);
+
+    groundSettings = ptr_groundSettings;
+
+    return 0;
+}
+
+void GroundDriveController::UpdateAutoPilot()
+{
+    uint8_t result = updateAutoPilotGround();
+
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+/**
+ * fixed wing autopilot:
+ * straight forward:
+ * 1. update path velocity for limited motion crafts
+ * 2. update attitude according to default fixed wing pathfollower algorithm
+ */
+uint8_t GroundDriveController::updateAutoPilotGround()
+{
+    updatePathVelocity(groundSettings->CourseFeedForward);
+    return updateGroundDesiredAttitude();
+}
+
+/**
+ * Compute desired velocity from the current position and path
+ */
+void GroundDriveController::updatePathVelocity(float kFF)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    VelocityStateData velocityState;
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    // look ahead kFF seconds
+    float cur[3] = { positionState.North + (velocityState.North * kFF),
+                     positionState.East + (velocityState.East * kFF),
+                     positionState.Down + (velocityState.Down * kFF) };
+    struct path_status progress;
+    path_progress(pathDesired, cur, &progress, false);
+
+    // GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector
+    // FOLLOWVECTOR:  correct_vector is distance to vector path, path_vector is the desired velocity vector
+
+    // Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector)
+    controlNE.ControlPositionWithPath(&progress);
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+    velocityDesired.Down  = 0.0f;
+
+    // update pathstatus
+    pathStatus->error     = progress.error;
+    pathStatus->fractional_progress  = progress.fractional_progress;
+    // FOLLOWVECTOR: desired velocity vector
+    pathStatus->path_direction_north = progress.path_vector[0];
+    pathStatus->path_direction_east  = progress.path_vector[1];
+    pathStatus->path_direction_down  = progress.path_vector[2];
+
+    // FOLLOWVECTOR: correction distance to vector path
+    pathStatus->correction_direction_north = progress.correction_vector[0];
+    pathStatus->correction_direction_east  = progress.correction_vector[1];
+    pathStatus->correction_direction_down  = progress.correction_vector[2];
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+/**
+ * Compute desired attitude for ground vehicles
+ */
+uint8_t GroundDriveController::updateGroundDesiredAttitude()
+{
+    StatusGroundDriveData statusGround;
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    AttitudeStateData attitudeState;
+    AttitudeStateGet(&attitudeState);
+    statusGround.State.Yaw = attitudeState.Yaw;
+    statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East);
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    // estimate a north/east command value to control the velocity error.
+    // ThrustLimits.Max(+-) limits the range.  Think of this as a command unit vector
+    // of the ultimate direction to reduce lateral error and achieve the target direction (desired angle).
+    float northCommand, eastCommand;
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    // Get current vehicle orientation
+    float angle_radians  = DEG2RAD(attitudeState.Yaw); // (+-pi)
+    float cos_angle      = cosf(angle_radians);
+    float sine_angle     = sinf(angle_radians);
+
+    float courseCommand  = 0.0f;
+    float speedCommand   = 0.0f;
+    float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
+    float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
+    // +ve facing correct direction, lateral command should just correct angle,
+    if (forwardCommand > 0.0f) {
+        // if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error
+
+        courseCommand = boundf(lateralCommand, -1.0f, 1.0f);
+        speedCommand  = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max);
+
+        // reinstate max thrust
+        controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
+
+        statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK;
+    } else {
+        // -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly.
+
+        // Reduce steering angle based on current velocity
+        float steeringReductionFactor = 1.0f;
+        if (stabDesired.Thrust > 0.3f) {
+            steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax;
+            steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f);
+        }
+
+        // should we turn left or right?
+        if (lateralCommand >= 0.1f) {
+            courseCommand = 1.0f * steeringReductionFactor;
+            statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT;
+        } else {
+            courseCommand = -1.0f * steeringReductionFactor;
+            statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT;
+        }
+
+        // Impose limits to slow down.
+        controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f);
+
+        speedCommand = groundSettings->ThrustLimit.SlowForward;
+    }
+
+    stabDesired.Roll   = 0.0f;
+    stabDesired.Pitch  = 0.0f;
+    stabDesired.Yaw    = courseCommand;
+
+    // Mix yaw into thrust limit TODO
+    stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max);
+
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    StabilizationDesiredSet(&stabDesired);
+
+    statusGround.NECommand.North       = northCommand;
+    statusGround.NECommand.East        = eastCommand;
+    statusGround.State.Thrust          = stabDesired.Thrust;
+    statusGround.BodyCommand.Forward   = forwardCommand;
+    statusGround.BodyCommand.Right     = lateralCommand;
+    statusGround.ControlCommand.Course = courseCommand;
+    statusGround.ControlCommand.Speed  = speedCommand;
+    StatusGroundDriveSet(&statusGround);
+
+    return 1;
+}

flight/modules/PathFollower/inc/fixedwingflycontroller.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup FixedWing CONTROL interface class
+ * @brief CONTROL interface class for pathfollower fixed wing fly controller
+ * @{
+ *
+ * @file       FixedWingCONTROL.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for fixed wing fly objectives
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef FIXEDWINGFLYCONTROLLER_H
+#define FIXEDWINGFLYCONTROLLER_H
+#include "pathfollowercontrol.h"
+
+class FixedWingFlyController : public PathFollowerControl {
+private:
+    static FixedWingFlyController *p_inst;
+    FixedWingFlyController();
+
+
+public:
+    static FixedWingFlyController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new FixedWingFlyController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+    void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev);
+
+private:
+    void resetGlobals();
+    uint8_t updateAutoPilotFixedWing();
+    void updatePathVelocity(float kFF, bool limited);
+    uint8_t updateFixedDesiredAttitude();
+    bool correctCourse(float *C, float *V, float *F, float s);
+
+    FixedWingPathFollowerSettingsData *fixedWingSettings;
+    uint8_t mActive;
+    uint8_t mMode;
+
+    struct pid PIDposH[2];
+    struct pid PIDposV;
+    struct pid PIDcourse;
+    struct pid PIDspeed;
+    struct pid PIDpower;
+    // correct speed by measured airspeed
+    float indicatedAirspeedStateBias;
+};
+
+#endif // FIXEDWINGFLYCONTROLLER_H

flight/modules/PathFollower/inc/grounddrivecontroller.h

@@ -0,0 +1,71 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup Ground CONTROL interface class
+ * @brief CONTROL interface class for ground drive controller
+ * @{
+ *
+ * @file       grounddrivecontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Class definition for ground drive controller implementation
+ *
+ * @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 GROUNDDRIVECONTROLLER_H
+#define GROUNDDRIVECONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontrolne.h"
+
+class GroundDriveController : public PathFollowerControl {
+private:
+    static GroundDriveController *p_inst;
+    GroundDriveController();
+
+
+public:
+    static GroundDriveController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new GroundDriveController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(GroundPathFollowerSettingsData *groundSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    uint8_t updateAutoPilotGround();
+    void updatePathVelocity(float kFF);
+    uint8_t updateGroundDesiredAttitude();
+
+    GroundPathFollowerSettingsData *groundSettings;
+    uint8_t mActive;
+    uint8_t mMode;
+    PIDControlNE controlNE;
+};
+
+#endif // GROUNDDRIVECONTROLLER_H

flight/modules/PathFollower/inc/pathfollowercontrol.h

@@ -0,0 +1,50 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief CONTROL interface class for pathfollower goal implementations
+ * @{
+ *
+ * @file       pathfollowercontrol.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for controllers
+ *
+ * @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 PATHFOLLOWERCONTROL_H
+#define PATHFOLLOWERCONTROL_H
+class PathFollowerControl {
+public:
+    virtual void Activate(void)   = 0;
+    virtual void Deactivate(void) = 0;
+    virtual void SettingsUpdated(void)  = 0;
+    virtual void UpdateAutoPilot(void)  = 0;
+    virtual void ObjectiveUpdated(void) = 0;
+    virtual uint8_t Mode(void) = 0;
+    static int32_t Initialize(PathDesiredData *ptr_pathDesired,
+                              FlightStatusData *ptr_flightStatus,
+                              PathStatusData *ptr_pathStatus);
+protected:
+    static PathDesiredData *pathDesired;
+    static FlightStatusData *flightStatus;
+    static PathStatusData *pathStatus;
+};
+
+#endif // PATHFOLLOWERCONTROL_H

flight/modules/PathFollower/inc/pathfollowerfsm.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM interface class
+ * @brief FSM interface class for pathfollower goal fsm implementations
+ * @{
+ *
+ * @file       pathfollowerfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for PathFollower FSMs
+ *
+ * @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 PATHFOLLOWERFSM_H
+#define PATHFOLLOWERFSM_H
+
+extern "C" {
+#include <stabilizationdesired.h>
+}
+
+typedef enum {
+    PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    PFFSM_STATE_ACTIVE,
+    PFFSM_STATE_DISARMED,
+    PFFSM_STATE_ABORT // Abort on error
+} PathFollowerFSMState_T;
+
+class PathFollowerFSM {
+public:
+    // PathFollowerFSM() {};
+    virtual void Inactive(void) {}
+    virtual void Activate(void) {}
+    virtual void Update(void) {}
+    virtual void SettingsUpdated(void) {}
+    virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) {}
+    virtual PathFollowerFSMState_T GetCurrentState(void)
+    {
+        return PFFSM_STATE_INACTIVE;
+    }
+    virtual void ConstrainStabiDesired(__attribute__((unused)) StabilizationDesiredData *stabDesired) {}
+    virtual float BoundVelocityDown(float velocity)
+    {
+        return velocity;
+    }
+    virtual void CheckPidScaler(__attribute__((unused)) pid_scaler *scaler) {}
+    virtual void GetYaw(bool &yaw_attitude, float &yaw_direction)
+    {
+        yaw_attitude = false; yaw_direction = 0.0f;
+    }
+    virtual void Abort(void) {}
+    virtual uint8_t ManualThrust(void)
+    {
+        return false;
+    }
+    virtual uint8_t PositionHoldState(void)
+    {
+        return false;
+    }
+
+    // virtual ~PathFollowerFSM() = 0;
+};
+
+#endif // PATHFOLLOWERFSM_H

flight/modules/PathFollower/inc/pidcontroldown.h

@@ -0,0 +1,95 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower PID Control implementation
+ * @brief PID Controller for down direction
+ * @{
+ *
+ * @file       PIDControlDown.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes control loop for down direction
+ *
+ * @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 PIDCONTROLDOWN_H
+#define PIDCONTROLDOWN_H
+extern "C" {
+#include <pid.h>
+#include <stabilizationdesired.h>
+}
+#include "pathfollowerfsm.h"
+
+class PIDControlDown {
+public:
+    PIDControlDown();
+    ~PIDControlDown();
+    void Initialize(PathFollowerFSM *fsm);
+    void SetThrustLimits(float min_thrust, float max_thrust);
+    void Deactivate();
+    void Activate();
+    void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
+    void UpdateNeutralThrust(float neutral);
+    void UpdateVelocitySetpoint(float setpoint);
+    void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
+    void UpdateVelocityState(float pv);
+    float GetVelocityDesired(void);
+    float GetDownCommand(void);
+    void UpdatePositionalParameters(float kp);
+    void UpdatePositionState(float pvDown);
+    void UpdatePositionSetpoint(float setpointDown);
+    void ControlPosition();
+    void ControlPositionWithPath(struct path_status *progress);
+    void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
+    void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
+
+private:
+    void setup_neutralThrustCalc();
+    void run_neutralThrustCalc();
+
+    struct pid2 PID;
+    float deltaTime;
+    float mVelocitySetpointTarget;
+    float mVelocitySetpointCurrent;
+    float mVelocityState;
+    float mDownCommand;
+    PathFollowerFSM *mFSM;
+    float mNeutral;
+    float mVelocityMax;
+    struct pid PIDpos;
+    float mPositionSetpointTarget;
+    float mPositionState;
+    float mMinThrust;
+    float mMaxThrust;
+    uint8_t mActive;
+
+    struct NeutralThrustEstimation {
+        uint32_t count;
+        float    sum;
+        float    average;
+        float    correction;
+        float    min;
+        float    max;
+        bool     start_sampling;
+        bool     have_correction;
+    };
+    struct NeutralThrustEstimation neutralThrustEst;
+};
+
+#endif // PIDCONTROLDOWN_H

flight/modules/PathFollower/inc/pidcontrolne.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief PID Controler for NE Class definition
+ * @{
+ *
+ * @file       pidcontrolne.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes pid control loop for NE
+ *
+ * @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 PIDCONTROLNE_H
+#define PIDCONTROLNE_H
+extern "C" {
+#include <pid.h>
+}
+#include "pathfollowerfsm.h"
+
+class PIDControlNE {
+public:
+    PIDControlNE();
+    ~PIDControlNE();
+    void Initialize();
+    void Deactivate();
+    void Activate();
+    void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax);
+    void UpdatePositionalParameters(float kp);
+    void UpdatePositionState(float pvNorth, float pvEast);
+    void UpdatePositionSetpoint(float setpointNorth, float setpointEast);
+    void UpdateVelocitySetpoint(float setpointNorth, float setpointEast);
+    // void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
+    void UpdateVelocityState(float pvNorth, float pvEast);
+    void UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward);
+    void ControlPosition();
+    void ControlPositionWithPath(struct path_status *progress);
+    void GetNECommand(float *northCommand, float *eastCommand);
+    void GetVelocityDesired(float *north, float *east);
+    void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
+    void UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate);
+
+private:
+    struct pid2 PIDvel[2]; // North, East
+    float mVelocitySetpointTarget[2];
+    float mVelocityState[2];
+    struct pid PIDposH[2];
+    float mPositionSetpointTarget[2];
+    float mPositionState[2];
+
+
+    float deltaTime;
+    float mVelocitySetpointCurrent[2];
+    float mNECommand;
+    float mNeutral;
+    float mVelocityMax;
+    float mMinCommand;
+    float mMaxCommand;
+    float mVelocityFeedforward;
+    uint8_t mActive;
+};
+
+#endif // PIDCONTROLNE_H

flight/modules/PathFollower/inc/vtolbrakecontroller.h

@@ -0,0 +1,76 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief CONTROL interface class for brake controller
+ * @{
+ *
+ * @file       vtolbrakecontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes brake controller for vtol
+ *
+ * @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 VTOLBRAKECONTROLLER_H
+#define VTOLBRAKECONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+// forward decl
+class PathFollowerFSM;
+class VtolBrakeController : public PathFollowerControl {
+private:
+    static VtolBrakeController *p_inst;
+    VtolBrakeController();
+
+
+public:
+    static VtolBrakeController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolBrakeController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(void);
+    void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
+
+    PathFollowerFSM *fsm;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlDown controlDown;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+    uint8_t mHoldActive;
+    uint8_t mManualThrust;
+};
+
+#endif // VTOLBRAKECONTROLLER_H

flight/modules/PathFollower/inc/vtolbrakefsm.h

@@ -0,0 +1,111 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM Brake
+ * @brief Executes brake seqeuence
+ * @{
+ *
+ * @file       vtolbrakfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes brake sequence fsm
+ *
+ * @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 VTOLBRAKEFSM_H
+#define VTOLBRAKEFSM_H
+
+#include "pathfollowerfsm.h"
+
+// Brakeing state machine
+typedef enum {
+    BRAKE_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    BRAKE_STATE_BRAKE, // Initiate altitude hold before starting descent
+    BRAKE_STATE_HOLD, // Waiting for attainment of landing descent rate
+    BRAKE_STATE_SIZE
+} PathFollowerFSM_BrakeState_T;
+
+typedef enum {
+    FSMBRAKESTATUS_STATEEXITREASON_NONE = 0
+} VtolBrakeFSMStatusStateExitReasonOptions;
+
+class VtolBrakeFSM : public PathFollowerFSM {
+private:
+    static VtolBrakeFSM *p_inst;
+    VtolBrakeFSM();
+
+public:
+    static VtolBrakeFSM *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolBrakeFSM();
+        }
+        return p_inst;
+    }
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
+                       PathDesiredData *pathDesired,
+                       FlightStatusData *flightStatus,
+                       PathStatusData *ptr_pathStatus);
+    void Inactive(void);
+    void Activate(void);
+    void Update(void);
+    PathFollowerFSMState_T GetCurrentState(void);
+    void Abort(void);
+    uint8_t PositionHoldState(void);
+
+protected:
+
+    // FSM instance data type
+    typedef struct {
+        PathFollowerFSM_BrakeState_T currentState;
+        uint32_t stateRunCount;
+        uint32_t stateTimeoutCount;
+        float    sum1;
+        float    sum2;
+        uint8_t  observationCount;
+        uint8_t  observation2Count;
+    } VtolBrakeFSMData_T;
+
+    // FSM state structure
+    typedef struct {
+        void(VtolBrakeFSM::*setup) (void); // Called to initialise the state
+        void(VtolBrakeFSM::*run) (uint8_t); // Run the event detection code for a state
+    } PathFollowerFSM_BrakeStateHandler_T;
+
+    // Private variables
+    VtolBrakeFSMData_T *mBrakeData;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PathDesiredData *pathDesired;
+    PathStatusData *pathStatus;
+    FlightStatusData *flightStatus;
+
+    void setup_brake(void);
+    void run_brake(uint8_t);
+    void initFSM(void);
+    void setState(PathFollowerFSM_BrakeState_T newState, VtolBrakeFSMStatusStateExitReasonOptions reason);
+    int32_t runState();
+    // void updateVtolBrakeFSMStatus();
+
+    void setStateTimeout(int32_t count);
+    void fallback_to_hold(void);
+
+    static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
+};
+
+#endif // VTOLBRAKEFSM_H

flight/modules/PathFollower/inc/vtolflycontroller.h

@@ -0,0 +1,84 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief vtol fly controller class definition
+ * @{
+ *
+ * @file       vtolflycontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes fly control for vtols
+ *
+ * @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 VTOLFLYCONTROLLER_H
+#define VTOLFLYCONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontrolne.h"
+#include "pidcontroldown.h"
+
+class VtolFlyController : public PathFollowerControl {
+private:
+    static VtolFlyController *p_inst;
+    VtolFlyController();
+
+
+public:
+    static VtolFlyController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolFlyController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void UpdateDesiredAttitudeEmergencyFallback();
+    void fallback_to_hold(void);
+    float updateTailInBearing();
+    float updateCourseBearing();
+    float updatePathBearing();
+    float updatePOIBearing();
+    uint8_t RunAutoPilot();
+
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlNE controlNE;
+    PIDControlDown controlDown;
+    uint8_t mActive;
+    uint8_t mManualThrust;
+    uint8_t mMode;
+    float vtolEmergencyFallback;
+    bool vtolEmergencyFallbackSwitch;
+};
+
+#endif // VTOLFLYCONTROLLER_H

flight/modules/PathFollower/inc/vtollandcontroller.h

@@ -0,0 +1,76 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief vtol land controller class
+ * @{
+ *
+ * @file       vtollandcontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes CONTROL for landing sequence
+ *
+ * @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 VTOLLANDCONTROLLER_H
+#define VTOLLANDCONTROLLER_H
+#include "pathfollowercontrol.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+// forward decl
+class PathFollowerFSM;
+class VtolLandController : public PathFollowerControl {
+private:
+    static VtolLandController *p_inst;
+    VtolLandController();
+
+
+public:
+    static VtolLandController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolLandController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void setArmedIfChanged(uint8_t val);
+
+    PathFollowerFSM *fsm;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlDown controlDown;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+};
+
+#endif // VTOLLANDCONTROLLER_H

flight/modules/PathFollower/inc/vtollandfsm.h

@@ -0,0 +1,148 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower FSM
+ * @brief Executes landing sequence via an FSM
+ * @{
+ *
+ * @file       vtollandfsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes FSM for landing sequence
+ *
+ * @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 VTOLLANDFSM_H
+#define VTOLLANDFSM_H
+
+extern "C" {
+#include "statusvtolland.h"
+}
+#include "pathfollowerfsm.h"
+
+// Landing state machine
+typedef enum {
+    LAND_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
+    LAND_STATE_INIT_ALTHOLD, // Initiate altitude hold before starting descent
+    LAND_STATE_WTG_FOR_DESCENTRATE, // Waiting for attainment of landing descent rate
+    LAND_STATE_AT_DESCENTRATE, // Landing descent rate achieved
+    LAND_STATE_WTG_FOR_GROUNDEFFECT, // Waiting for group effect to be detected
+    LAND_STATE_GROUNDEFFECT, // Ground effect detected
+    LAND_STATE_THRUSTDOWN, // Thrust down sequence
+    LAND_STATE_THRUSTOFF, // Thrust is now off
+    LAND_STATE_DISARMED, // Disarmed
+    LAND_STATE_ABORT, // Abort on error triggerig fallback to hold
+    LAND_STATE_SIZE
+} PathFollowerFSM_LandState_T;
+
+class VtolLandFSM : public PathFollowerFSM {
+private:
+    static VtolLandFSM *p_inst;
+    VtolLandFSM();
+
+public:
+    static VtolLandFSM *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolLandFSM();
+        }
+        return p_inst;
+    }
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
+                       PathDesiredData *pathDesired,
+                       FlightStatusData *flightStatus);
+    void Inactive(void);
+    void Activate(void);
+    void Update(void);
+    void BoundThrust(float &ulow, float &uhigh);
+    PathFollowerFSMState_T GetCurrentState(void);
+    void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
+    float BoundVelocityDown(float);
+    void CheckPidScaler(pid_scaler *scaler);
+    void Abort(void);
+
+protected:
+
+    // FSM instance data type
+    typedef struct {
+        StatusVtolLandData  fsmLandStatus;
+        PathFollowerFSM_LandState_T currentState;
+        TakeOffLocationData takeOffLocation;
+        uint32_t stateRunCount;
+        uint32_t stateTimeoutCount;
+        float    sum1;
+        float    sum2;
+        float    expectedLandPositionNorth;
+        float    expectedLandPositionEast;
+        float    thrustLimit;
+        float    boundThrustMin;
+        float    boundThrustMax;
+        uint8_t  observationCount;
+        uint8_t  observation2Count;
+        uint8_t  flZeroStabiHorizontal;
+        uint8_t  flConstrainThrust;
+        uint8_t  flLowAltitude;
+        uint8_t  flAltitudeHold;
+    } VtolLandFSMData_T;
+
+    // FSM state structure
+    typedef struct {
+        void(VtolLandFSM::*setup) (void); // Called to initialise the state
+        void(VtolLandFSM::*run) (uint8_t); // Run the event detection code for a state
+    } PathFollowerFSM_LandStateHandler_T;
+
+    // Private variables
+    VtolLandFSMData_T *mLandData;
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PathDesiredData *pathDesired;
+    FlightStatusData *flightStatus;
+
+    void setup_inactive(void);
+    void setup_init_althold(void);
+    void setup_wtg_for_descentrate(void);
+    void setup_at_descentrate(void);
+    void setup_wtg_for_groundeffect(void);
+    void run_init_althold(uint8_t);
+    void run_wtg_for_descentrate(uint8_t);
+    void run_at_descentrate(uint8_t);
+    void run_wtg_for_groundeffect(uint8_t);
+    void setup_groundeffect(void);
+    void run_groundeffect(uint8_t);
+    void setup_thrustdown(void);
+    void run_thrustdown(uint8_t);
+    void setup_thrustoff(void);
+    void run_thrustoff(uint8_t);
+    void setup_disarmed(void);
+    void run_disarmed(uint8_t);
+    void setup_abort(void);
+    void run_abort(uint8_t);
+    void initFSM(void);
+    void setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason);
+    int32_t runState();
+    int32_t runAlways();
+    void updateVtolLandFSMStatus();
+    void assessAltitude(void);
+
+    void setStateTimeout(int32_t count);
+    void fallback_to_hold(void);
+
+    static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE];
+};
+
+#endif // VTOLLANDFSM_H

flight/modules/PathFollower/inc/vtolvelocitycontroller.h

@@ -0,0 +1,72 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower Controller
+ * @brief Controller for Vtol velocity roam
+ * @{
+ *
+ * @file       vtolvelocitycontroller.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes velocity roam control
+ *
+ * @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 PATHFOLLOWERCONTROLVELOCITYROAM_H
+#define PATHFOLLOWERCONTROLVELOCITYROAM_H
+#include "pathfollowercontrol.h"
+#include "pidcontrolne.h"
+
+class VtolVelocityController : public PathFollowerControl {
+private:
+    static VtolVelocityController *p_inst;
+    VtolVelocityController();
+
+
+public:
+    static VtolVelocityController *instance()
+    {
+        if (!p_inst) {
+            p_inst = new VtolVelocityController();
+        }
+        return p_inst;
+    }
+
+    int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
+
+
+    void Activate(void);
+    void Deactivate(void);
+    void SettingsUpdated(void);
+    void UpdateAutoPilot(void);
+    void ObjectiveUpdated(void);
+    uint8_t IsActive(void);
+    uint8_t Mode(void);
+
+private:
+    void UpdateVelocityDesired(void);
+    int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void fallback_to_hold(void);
+
+    VtolPathFollowerSettingsData *vtolPathFollowerSettings;
+    PIDControlNE controlNE;
+    uint8_t mActive;
+};
+
+#endif // ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H

flight/modules/PathFollower/pathfollower.c

@@ -1,1544 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pathfollower.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
- * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
- * of @ref ManualControlCommand is Auto.
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * 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
- */
-
-/**
- * Input object: PathDesired
- * Input object: PositionState
- * Input object: ManualControlCommand
- * Output object: StabilizationDesired
- *
- * This module acts as "autopilot" - it controls the setpoints of stabilization
- * based on current flight situation and desired flight path (PathDesired) as
- * directed by flightmode selection or pathplanner
- * This is a periodic delayed callback module
- *
- * Modules have no API, all communication to other modules is done through UAVObjects.
- * However modules may use the API exposed by shared libraries.
- * See the OpenPilot wiki for more details.
- * http://www.openpilot.org/OpenPilot_Application_Architecture
- *
- */
-
-#include <openpilot.h>
-
-#include <callbackinfo.h>
-
-#include <math.h>
-#include <pid.h>
-#include <CoordinateConversions.h>
-#include <sin_lookup.h>
-#include <pathdesired.h>
-#include <paths.h>
-#include "plans.h"
-#include <sanitycheck.h>
-
-
-#include <fixedwingpathfollowersettings.h>
-#include <fixedwingpathfollowerstatus.h>
-#include <vtolpathfollowersettings.h>
-#include <flightstatus.h>
-#include <flightmodesettings.h>
-#include <pathstatus.h>
-#include <positionstate.h>
-#include <velocitystate.h>
-#include <velocitydesired.h>
-#include <stabilizationdesired.h>
-#include <airspeedstate.h>
-#include <attitudestate.h>
-#include <takeofflocation.h>
-#include <poilocation.h>
-#include <manualcontrolcommand.h>
-#include <systemsettings.h>
-#include <stabilizationbank.h>
-#include <vtolselftuningstats.h>
-#include <pathsummary.h>
-
-
-// Private constants
-
-#define CALLBACK_PRIORITY                           CALLBACK_PRIORITY_LOW
-#define CBTASK_PRIORITY                             CALLBACK_TASK_FLIGHTCONTROL
-
-#define PF_IDLE_UPDATE_RATE_MS                      100
-
-#define STACK_SIZE_BYTES                            2048
-
-#define DEADBAND_HIGH                               0.10f
-#define DEADBAND_LOW                                -0.10f
-
-#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
-#define BRAKE_EXIT_VELOCITY_LIMIT                   0.2f
-
-#define BRAKE_RATE_MINIMUM                          0.2f
-
-#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT     0.5f
-#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT          0.2f
-#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT            0.2f
-#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT            0.1f
-
-#define NEUTRALTHRUST_START_DELAY                   (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
-#define NEUTRALTHRUST_END_COUNT                     (NEUTRALTHRUST_START_DELAY + (4 * 20))  // 4 second sample
-
-// Private types
-
-struct Globals {
-    struct pid PIDposH[2];
-    struct pid PIDposV;
-    struct pid PIDvel[3]; // North, East, Down
-    struct pid BrakePIDvel[2]; // North, East
-    struct pid PIDcourse;
-    struct pid PIDspeed;
-    struct pid PIDpower;
-    float poiRadius;
-    float vtolEmergencyFallback;
-    bool  vtolEmergencyFallbackSwitch;
-};
-
-struct NeutralThrustEstimation {
-    uint32_t count;
-    float    sum;
-    float    average;
-    float    correction;
-    float    algo_erro_check;
-    float    min;
-    float    max;
-    bool     start_sampling;
-    bool     have_correction;
-};
-static struct NeutralThrustEstimation neutralThrustEst;
-
-
-// Private variables
-static DelayedCallbackInfo *pathFollowerCBInfo;
-static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
-static struct Globals global;
-static PathStatusData pathStatus;
-static PathDesiredData pathDesired;
-static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
-static VtolPathFollowerSettingsData vtolPathFollowerSettings;
-static FlightStatusData flightStatus;
-static PathSummaryData pathSummary;
-
-// correct speed by measured airspeed
-static float indicatedAirspeedStateBias = 0.0f;
-
-
-// Private functions
-static void pathFollowerTask(void);
-static void resetGlobals();
-static void SettingsUpdatedCb(UAVObjEvent *ev);
-static uint8_t updateAutoPilotByFrameType();
-static uint8_t updateAutoPilotFixedWing();
-static uint8_t updateAutoPilotVtol();
-static float updateTailInBearing();
-static float updateCourseBearing();
-static float updatePathBearing();
-static float updatePOIBearing();
-static void processPOI();
-static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
-static void updatePathVelocity(float kFF, bool limited);
-static uint8_t updateFixedDesiredAttitude();
-static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
-static void updateFixedAttitude();
-static void updateVtolDesiredAttitudeEmergencyFallback();
-static void airspeedStateUpdatedCb(UAVObjEvent *ev);
-static bool correctCourse(float *C, float *V, float *F, float s);
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t PathFollowerStart()
-{
-    // Start main task
-    PathStatusGet(&pathStatus);
-    SettingsUpdatedCb(NULL);
-    PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
-
-    return 0;
-}
-
-/**
- * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
- */
-int32_t PathFollowerInitialize()
-{
-    // initialize objects
-    FixedWingPathFollowerSettingsInitialize();
-    FixedWingPathFollowerStatusInitialize();
-    VtolPathFollowerSettingsInitialize();
-    FlightStatusInitialize();
-    FlightModeSettingsInitialize();
-    PathStatusInitialize();
-    PathSummaryInitialize();
-    PathDesiredInitialize();
-    PositionStateInitialize();
-    VelocityStateInitialize();
-    VelocityDesiredInitialize();
-    StabilizationDesiredInitialize();
-    AirspeedStateInitialize();
-    AttitudeStateInitialize();
-    TakeOffLocationInitialize();
-    PoiLocationInitialize();
-    ManualControlCommandInitialize();
-    SystemSettingsInitialize();
-    StabilizationBankInitialize();
-    VtolSelfTuningStatsInitialize();
-
-
-    // reset integrals
-    resetGlobals();
-
-    // Create object queue
-    pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
-    FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
-    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
-    PathDesiredConnectCallback(SettingsUpdatedCb);
-    AirspeedStateConnectCallback(airspeedStateUpdatedCb);
-
-    return 0;
-}
-MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
-
-
-/**
- * Module thread, should not return.
- */
-static void pathFollowerTask(void)
-{
-    FlightStatusGet(&flightStatus);
-
-    if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
-        resetGlobals();
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
-        PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-        return;
-    }
-
-    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol!
-        processPOI();
-    }
-
-    int16_t old_uid = pathStatus.UID;
-    pathStatus.UID    = pathDesired.UID;
-    pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
-    if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
-        if (old_uid != pathStatus.UID) {
-            pathStatus.path_time = 0.0f;
-        } else {
-            pathStatus.path_time += updatePeriod / 1000.0f;
-        }
-    }
-
-    switch (pathDesired.Mode) {
-    case PATHDESIRED_MODE_FLYENDPOINT:
-    case PATHDESIRED_MODE_FLYVECTOR:
-    case PATHDESIRED_MODE_BRAKE:
-    case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-    case PATHDESIRED_MODE_FLYCIRCLELEFT:
-    {
-        uint8_t result = updateAutoPilotByFrameType();
-        if (result) {
-            AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-        } else {
-            pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-            AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
-        }
-    }
-    break;
-    case PATHDESIRED_MODE_FIXEDATTITUDE:
-        updateFixedAttitude(pathDesired.ModeParameters);
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
-        break;
-    case PATHDESIRED_MODE_DISARMALARM:
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
-        break;
-    default:
-        pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
-        break;
-    }
-    PathStatusSet(&pathStatus);
-
-
-    PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
-}
-
-
-static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
-
-    pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit);
-    pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit);
-    pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit);
-
-
-    VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
-
-    pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
-    pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
-    pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
-
-    pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
-    pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
-
-    PathDesiredGet(&pathDesired);
-}
-
-
-static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    AirspeedStateData airspeedState;
-    VelocityStateData velocityState;
-
-    AirspeedStateGet(&airspeedState);
-    VelocityStateGet(&velocityState);
-    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 - 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.
-}
-
-
-/**
- * reset integrals
- */
-static void resetGlobals()
-{
-    pid_zero(&global.PIDposH[0]);
-    pid_zero(&global.PIDposH[1]);
-    pid_zero(&global.PIDposV);
-    pid_zero(&global.PIDvel[0]);
-    pid_zero(&global.PIDvel[1]);
-    pid_zero(&global.PIDvel[2]);
-    pid_zero(&global.BrakePIDvel[0]);
-    pid_zero(&global.BrakePIDvel[1]);
-    pid_zero(&global.PIDcourse);
-    pid_zero(&global.PIDspeed);
-    pid_zero(&global.PIDpower);
-    global.poiRadius = 0.0f;
-    global.vtolEmergencyFallback = 0;
-    global.vtolEmergencyFallbackSwitch = false;
-
-    // reset neutral thrust assessment. We restart this process
-    // and do once for each position hold engagement
-    neutralThrustEst.start_sampling  = false;
-    neutralThrustEst.count = 0;
-    neutralThrustEst.sum = 0.0f;
-    neutralThrustEst.have_correction = false;
-    neutralThrustEst.average    = 0.0f;
-    neutralThrustEst.correction = 0.0f;
-    neutralThrustEst.min = 0.0f;
-    neutralThrustEst.max = 0.0f;
-
-    pathStatus.path_time = 0.0f;
-}
-
-static uint8_t updateAutoPilotByFrameType()
-{
-    FrameType_t frameType = GetCurrentFrameType();
-
-    if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
-        switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
-        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
-            frameType = FRAME_TYPE_FIXED_WING;
-            break;
-        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
-            frameType = FRAME_TYPE_MULTIROTOR;
-            break;
-        }
-    }
-    switch (frameType) {
-    case FRAME_TYPE_MULTIROTOR:
-    case FRAME_TYPE_HELI:
-        updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
-        return updateAutoPilotVtol();
-
-        break;
-    case FRAME_TYPE_FIXED_WING:
-    default:
-        updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
-        return updateAutoPilotFixedWing();
-
-        break;
-    }
-}
-
-/**
- * fixed wing autopilot:
- * straight forward:
- * 1. update path velocity for limited motion crafts
- * 2. update attitude according to default fixed wing pathfollower algorithm
- */
-static uint8_t updateAutoPilotFixedWing()
-{
-    pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-    pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-    pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
-    updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
-    return updateFixedDesiredAttitude();
-}
-
-/**
- * vtol autopilot
- * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
- * fall back to emergency fallback autopilot to keep minimum amount of flight control
- */
-static uint8_t updateAutoPilotVtol()
-{
-    enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
-    enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
-    uint8_t result = 0;
-
-    // decide on behaviour based on settings and system state
-    if (global.vtolEmergencyFallbackSwitch) {
-        returnmode   = RETURN_0;
-        followermode = FOLLOWER_FALLBACK;
-    } else {
-        if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
-            returnmode   = RETURN_1;
-            followermode = FOLLOWER_FALLBACK;
-        } else {
-            returnmode   = RETURN_RESULT;
-            followermode = FOLLOWER_REGULAR;
-        }
-    }
-
-    // vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings
-    pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
-
-    switch (followermode) {
-    case FOLLOWER_REGULAR:
-    {
-        // horizontal position control PID loop works according to settings in regular mode, allowing integral terms
-        pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-        pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
-        updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
-
-        // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
-        bool yaw_attitude = true;
-        float yaw = 0.0f;
-
-        if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-            yaw_attitude = false;
-        } else {
-            switch (vtolPathFollowerSettings.YawControl) {
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
-                yaw_attitude = false;
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
-                yaw = updateTailInBearing();
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
-                yaw = updateCourseBearing();
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
-                yaw = updatePathBearing();
-                break;
-            case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
-                yaw = updatePOIBearing();
-                break;
-            }
-        }
-        result = updateVtolDesiredAttitude(yaw_attitude, yaw);
-
-        if (!result) {
-            if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-                plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override
-            } else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
-                // switch to emergency follower if follower indicates problems
-                global.vtolEmergencyFallbackSwitch = true;
-            }
-        }
-    }
-    break;
-    case FOLLOWER_FALLBACK:
-    {
-        // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
-        pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
-        pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
-        updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
-
-        // emergency follower has no return value
-        updateVtolDesiredAttitudeEmergencyFallback();
-    }
-    break;
-    }
-
-
-    // Brake mode end condition checks
-    if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-        bool exit_brake = false;
-        VelocityStateData velocityState;
-        if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout
-            pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
-            exit_brake = true;
-        } else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
-            VelocityStateGet(&velocityState);
-            if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
-                pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
-                exit_brake = true;
-            }
-        }
-
-        if (exit_brake) {
-            // Calculate the distance error between the originally desired
-            // stopping point and the actual brake-exit point.
-
-            PositionStateData p;
-            PositionStateGet(&p);
-            float north_offset = pathDesired.End.North - p.North;
-            float east_offset  = pathDesired.End.East - p.East;
-            float down_offset  = pathDesired.End.Down - p.Down;
-            pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
-            pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time;
-            pathSummary.fractional_progress   = pathStatus.fractional_progress;
-            float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
-            cur_velocity = sqrtf(cur_velocity);
-            pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time;
-            pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate;
-            pathSummary.velocityIntoHold = cur_velocity;
-            pathSummary.UID = pathStatus.UID;
-            PathSummarySet(&pathSummary);
-
-            plan_setup_assistedcontrol(true); // braking timeout true
-            // having re-entered hold allow reassessment of neutral thrust
-            neutralThrustEst.have_correction = false;
-        }
-    }
-
-    switch (returnmode) {
-    case RETURN_RESULT:
-        return result;
-
-    default:
-        // returns either 0 or 1 according to enum definition above
-        return returnmode;
-    }
-}
-
-
-/**
- * Compute bearing of current takeoff location
- */
-static float updateTailInBearing()
-{
-    PositionStateData p;
-
-    PositionStateGet(&p);
-    TakeOffLocationData t;
-    TakeOffLocationGet(&t);
-    // atan2f always returns in between + and - 180 degrees
-    return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
-}
-
-/**
- * Compute bearing of current movement direction
- */
-static float updateCourseBearing()
-{
-    VelocityStateData v;
-
-    VelocityStateGet(&v);
-    // atan2f always returns in between + and - 180 degrees
-    return RAD2DEG(atan2f(v.East, v.North));
-}
-
-/**
- * Compute bearing of current path direction
- */
-static float updatePathBearing()
-{
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-
-    float cur[3] = { positionState.North,
-                     positionState.East,
-                     positionState.Down };
-    struct path_status progress;
-
-    path_progress(&pathDesired, cur, &progress);
-
-    // atan2f always returns in between + and - 180 degrees
-    return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
-}
-
-/**
- * Compute bearing between current position and POI
- */
-static float updatePOIBearing()
-{
-    PoiLocationData poi;
-
-    PoiLocationGet(&poi);
-    PositionStateData positionState;
-    PositionStateGet(&positionState);
-
-    const float dT = updatePeriod / 1000.0f;
-    float dLoc[3];
-    float yaw = 0;
-    /*float elevation = 0;*/
-
-    dLoc[0] = positionState.North - poi.North;
-    dLoc[1] = positionState.East - poi.East;
-    dLoc[2] = positionState.Down - poi.Down;
-
-    if (dLoc[1] < 0) {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
-    } else {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
-    }
-    ManualControlCommandData manualControlData;
-    ManualControlCommandGet(&manualControlData);
-
-    float pathAngle = 0;
-    if (manualControlData.Roll > DEADBAND_HIGH) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
-    } else if (manualControlData.Roll < DEADBAND_LOW) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
-    }
-
-    return yaw + (pathAngle / 2.0f);
-}
-
-/**
- * process POI control logic TODO: this should most likely go into manualcontrol!!!!
- * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
- **/
-static void processPOI()
-{
-    const float dT = updatePeriod / 1000.0f;
-
-
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-    // TODO put commented out camera feature code back in place either
-    // permanently or optionally or remove it
-    // CameraDesiredData cameraDesired;
-    // CameraDesiredGet(&cameraDesired);
-    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
-    PoiLocationData poi;
-    PoiLocationGet(&poi);
-
-    float dLoc[3];
-    float yaw = 0;
-    // TODO camera feature
-    /*float elevation = 0;*/
-
-    dLoc[0] = positionState.North - poi.North;
-    dLoc[1] = positionState.East - poi.East;
-    dLoc[2] = positionState.Down - poi.Down;
-
-    if (dLoc[1] < 0) {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
-    } else {
-        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
-    }
-
-    // distance
-    float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
-
-    ManualControlCommandData manualControlData;
-    ManualControlCommandGet(&manualControlData);
-
-    float changeRadius = 0;
-    // Move closer or further, radially
-    if (manualControlData.Pitch > DEADBAND_HIGH) {
-        changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
-    } else if (manualControlData.Pitch < DEADBAND_LOW) {
-        changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
-    }
-
-    // move along circular path
-    float pathAngle = 0;
-    if (manualControlData.Roll > DEADBAND_HIGH) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
-    } else if (manualControlData.Roll < DEADBAND_LOW) {
-        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
-    } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
-        // change radius only when not circling
-        global.poiRadius = distance + changeRadius;
-    }
-
-    // don't try to move any closer
-    if (global.poiRadius >= 3.0f || changeRadius > 0) {
-        if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
-            pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
-            pathDesired.End.East  = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
-            pathDesired.StartingVelocity = 1.0f;
-            pathDesired.EndingVelocity   = 0.0f;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-            PathDesiredSet(&pathDesired);
-        }
-    }
-    // not above
-    if (distance >= 3.0f) {
-        // TODO camera feature
-        // You can feed this into camerastabilization
-        /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
-
-        // cameraDesired.Yaw=yaw;
-        // cameraDesired.PitchOrServo2=elevation;
-
-        // CameraDesiredSet(&cameraDesired);
-    }
-}
-
-static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
-{
-    if (startingVelocity >= 0.0f) {
-        *updatedVelocity = startingVelocity - dT * brakeRate;
-        if (*updatedVelocity > currentVelocity) {
-            *updatedVelocity = currentVelocity;
-        }
-        if (*updatedVelocity < 0.0f) {
-            *updatedVelocity = 0.0f;
-        }
-    } else {
-        *updatedVelocity = startingVelocity + dT * brakeRate;
-        if (*updatedVelocity < currentVelocity) {
-            *updatedVelocity = currentVelocity;
-        }
-        if (*updatedVelocity > 0.0f) {
-            *updatedVelocity = 0.0f;
-        }
-    }
-}
-
-
-/**
- * Compute desired velocity from the current position and path
- */
-static void updatePathVelocity(float kFF, bool limited)
-{
-    PositionStateData positionState;
-
-    PositionStateGet(&positionState);
-    VelocityStateData velocityState;
-    VelocityStateGet(&velocityState);
-    VelocityDesiredData velocityDesired;
-
-    const float dT = updatePeriod / 1000.0f;
-
-    if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-        float brakeRate = vtolPathFollowerSettings.BrakeRate;
-        if (brakeRate < BRAKE_RATE_MINIMUM) {
-            brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
-        }
-        updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North);
-        updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East);
-        updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down);
-
-        float cur_velocity     = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
-        cur_velocity     = sqrtf(cur_velocity);
-        float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
-        desired_velocity = sqrtf(desired_velocity);
-
-        // update pathstatus
-        pathStatus.error = cur_velocity - desired_velocity;
-        pathStatus.fractional_progress = 1.0f;
-        if (pathDesired.StartingVelocity > 0.0f) {
-            pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity;
-
-            // sometimes current velocity can exceed starting velocity due to initial acceleration
-            if (pathStatus.fractional_progress < 0.0f) {
-                pathStatus.fractional_progress = 0.0f;
-            }
-        }
-        pathStatus.path_direction_north = velocityDesired.North;
-        pathStatus.path_direction_east  = velocityDesired.East;
-        pathStatus.path_direction_down  = velocityDesired.Down;
-
-        pathStatus.correction_direction_north = velocityDesired.North - velocityState.North;
-        pathStatus.correction_direction_east  = velocityDesired.East - velocityState.East;
-        pathStatus.correction_direction_down  = velocityDesired.Down - velocityState.Down;
-    } else {
-        // look ahead kFF seconds
-        float cur[3] = { positionState.North + (velocityState.North * kFF),
-                         positionState.East + (velocityState.East * kFF),
-                         positionState.Down + (velocityState.Down * kFF) };
-        struct path_status progress;
-        path_progress(&pathDesired, cur, &progress);
-
-        // calculate velocity - can be zero if waypoints are too close
-        velocityDesired.North = progress.path_vector[0];
-        velocityDesired.East  = progress.path_vector[1];
-        velocityDesired.Down  = progress.path_vector[2];
-
-        if (limited &&
-            // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
-            // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
-            // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
-            // leading to an S-shape snake course the wrong way
-            // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
-            // turn steep unless there is enough space complete the turn before crossing the flightpath
-            // 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 everything )
-            // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
-            // calculating angles < 90 degrees through dot products
-            (vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
-            ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
-            ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
-            ;
-        } else {
-            // calculate correction
-            velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
-            velocityDesired.East  += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
-        }
-        velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
-
-        // update pathstatus
-        pathStatus.error      = progress.error;
-        pathStatus.fractional_progress  = progress.fractional_progress;
-        pathStatus.path_direction_north = progress.path_vector[0];
-        pathStatus.path_direction_east  = progress.path_vector[1];
-        pathStatus.path_direction_down  = progress.path_vector[2];
-
-        pathStatus.correction_direction_north = progress.correction_vector[0];
-        pathStatus.correction_direction_east  = progress.correction_vector[1];
-        pathStatus.correction_direction_down  = progress.correction_vector[2];
-    }
-
-
-    VelocityDesiredSet(&velocityDesired);
-}
-
-
-/**
- * Compute desired attitude from the desired velocity for fixed wing craft
- */
-static uint8_t updateFixedDesiredAttitude()
-{
-    uint8_t result = 1;
-
-    const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
-
-    VelocityDesiredData velocityDesired;
-    VelocityStateData velocityState;
-    StabilizationDesiredData stabDesired;
-    AttitudeStateData attitudeState;
-    FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
-    AirspeedStateData airspeedState;
-    SystemSettingsData systemSettings;
-
-    float groundspeedProjection;
-    float indicatedAirspeedState;
-    float indicatedAirspeedDesired;
-    float airspeedError;
-
-    float pitchCommand;
-
-    float descentspeedDesired;
-    float descentspeedError;
-    float powerCommand;
-
-    float airspeedVector[2];
-    float fluidMovement[2];
-    float courseComponent[2];
-    float courseError;
-    float courseCommand;
-
-    FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
-
-    VelocityStateGet(&velocityState);
-    StabilizationDesiredGet(&stabDesired);
-    VelocityDesiredGet(&velocityDesired);
-    AttitudeStateGet(&attitudeState);
-    AirspeedStateGet(&airspeedState);
-    SystemSettingsGet(&systemSettings);
-
-    /**
-     * Compute speed error and course
-     */
-    // missing sensors for airspeed-direction we have to assume within
-    // reasonable error that measured airspeed is actually the airspeed
-    // component in forward pointing direction
-    // airspeedVector is normalized
-    airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
-    airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
-
-    // current ground speed projected in forward direction
-    groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
-
-    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
-    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
-    // than airspeed and gps sensors alone
-    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
-
-    // fluidMovement is a vector describing the aproximate movement vector of
-    // the surrounding fluid in 2d space (aka wind vector)
-    fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
-    fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
-
-    // calculate the movement vector we need to fly to reach velocityDesired -
-    // taking fluidMovement into account
-    courseComponent[0] = velocityDesired.North - fluidMovement[0];
-    courseComponent[1] = velocityDesired.East - fluidMovement[1];
-
-    indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
-                                      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;
-
-    // Vertical speed error
-    descentspeedDesired = boundf(
-        velocityDesired.Down,
-        -fixedWingPathFollowerSettings.VerticalVelMax,
-        fixedWingPathFollowerSettings.VerticalVelMax);
-    descentspeedError = descentspeedDesired - velocityState.Down;
-
-    // Error condition: plane too slow or too fast
-    fixedWingPathFollowerStatus.Errors.Highspeed = 0;
-    fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
-    if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
-        fixedWingPathFollowerStatus.Errors.Overspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
-        fixedWingPathFollowerStatus.Errors.Highspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
-        fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
-        result = 0;
-    }
-    if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
-        fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
-        result = 0;
-    }
-
-    /**
-     * Compute desired thrust command
-     */
-
-    // Compute the cross feed from vertical speed to pitch, with saturation
-    float speedErrorToPowerCommandComponent = boundf(
-        (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
-        -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
-        fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
-        );
-
-    // Compute final thrust response
-    powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) +
-                   speedErrorToPowerCommandComponent;
-
-    // Output internal state to telemetry
-    fixedWingPathFollowerStatus.Error.Power    = descentspeedError;
-    fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Power  = powerCommand;
-
-    // set thrust
-    stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand,
-                                fixedWingPathFollowerSettings.ThrustLimit.Min,
-                                fixedWingPathFollowerSettings.ThrustLimit.Max);
-
-    // 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.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;
-    }
-    // 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.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;
-    }
-
-    /**
-     * Compute desired pitch command
-     */
-    // Compute the cross feed from vertical speed to pitch, with saturation
-    float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp,
-                                                        -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max,
-                                                        fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max
-                                                        );
-
-    // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
-    pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
-
-    fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
-    fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
-
-    stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
-                               fixedWingPathFollowerSettings.PitchLimit.Min,
-                               fixedWingPathFollowerSettings.PitchLimit.Max);
-
-    // 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.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;
-    }
-
-    /**
-     * Compute desired roll command
-     */
-    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
-
-    if (courseError < -180.0f) {
-        courseError += 360.0f;
-    }
-    if (courseError > 180.0f) {
-        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;
-    }
-
-    courseCommand = pid_apply(&global.PIDcourse, courseError, dT);
-
-    fixedWingPathFollowerStatus.Error.Course    = courseError;
-    fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator;
-    fixedWingPathFollowerStatus.Command.Course  = courseCommand;
-
-    stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral +
-                              courseCommand,
-                              fixedWingPathFollowerSettings.RollLimit.Min,
-                              fixedWingPathFollowerSettings.RollLimit.Max);
-
-    // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
-
-
-    /**
-     * Compute desired yaw command
-     */
-    // TODO implement raw control mode for yaw and base on Accels.Y
-    stabDesired.Yaw = 0.0f;
-
-
-    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
-
-    StabilizationDesiredSet(&stabDesired);
-
-    FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
-
-    return result;
-}
-
-
-/**
- * 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 = vector_lengthf(F, 2);
-
-    // normalize Cn=V/|V|, |V| must be >0
-    float v = vector_lengthf(V, 2);
-    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;
-    }
-}
-
-/**
- * Compute desired attitude from the desired velocity
- *
- * Takes in @ref NedState which has the acceleration in the
- * NED frame as the feedback term and then compares the
- * @ref VelocityState against the @ref VelocityDesired
- */
-static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
-{
-    const float dT     = updatePeriod / 1000.0f;
-    uint8_t result     = 1;
-    bool manual_thrust = false;
-
-    VelocityDesiredData velocityDesired;
-    VelocityStateData velocityState;
-    StabilizationDesiredData stabDesired;
-    AttitudeStateData attitudeState;
-    StabilizationBankData stabSettings;
-    SystemSettingsData systemSettings;
-    VtolSelfTuningStatsData vtolSelfTuningStats;
-
-    float northError;
-    float northCommand;
-
-    float eastError;
-    float eastCommand;
-
-    float downError;
-    float downCommand;
-
-    SystemSettingsGet(&systemSettings);
-    VelocityStateGet(&velocityState);
-    VelocityDesiredGet(&velocityDesired);
-    StabilizationDesiredGet(&stabDesired);
-    VelocityDesiredGet(&velocityDesired);
-    AttitudeStateGet(&attitudeState);
-    StabilizationBankGet(&stabSettings);
-    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
-
-
-    if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
-        // scale velocity if it is above configured maximum
-        // for braking, we can not help it if initial velocity was greater
-        float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
-        if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
-            velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
-            velocityDesired.East  *= vtolPathFollowerSettings.HorizontalVelMax / velH;
-        }
-        if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
-            velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
-        }
-    }
-
-    // calculate the velocity errors between desired and actual
-    northError = velocityDesired.North - velocityState.North;
-    eastError  = velocityDesired.East - velocityState.East;
-    downError  = velocityDesired.Down - velocityState.Down;
-
-    // Must flip this sign
-    downError  = -downError;
-
-    // Compute desired commands
-    if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
-        northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
-        eastCommand  = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
-    } else {
-        northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward;
-        eastCommand  = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward;
-    }
-    downCommand = pid_apply(&global.PIDvel[2], downError, dT);
-
-
-    if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL &&
-         flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
-        (flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
-        manual_thrust = true;
-    }
-
-    // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
-    // note that arming into this flight mode is not allowed, so assumption here is that
-    // we have not landed.  If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
-    // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
-    // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
-    // avoiding auto-takeoffs.  Therefore no need to check that here.
-    if (!manual_thrust && neutralThrustEst.have_correction != true) {
-        // Assess if position hold state running.  This can be normal position hold or
-        // another mode with assist-hold active.
-        bool ph_active =
-            ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD &&
-              flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
-             (flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE &&
-              flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD));
-
-
-        bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
-                       fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
-                       fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
-                       fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
-
-        if (ph_active && stable) {
-            if (neutralThrustEst.start_sampling) {
-                neutralThrustEst.count++;
-
-
-                // delay count for 2 seconds into hold allowing for stablisation
-                if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
-                    neutralThrustEst.sum += global.PIDvel[2].iAccumulator;
-                    if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) {
-                        neutralThrustEst.min = global.PIDvel[2].iAccumulator;
-                    }
-                    if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) {
-                        neutralThrustEst.max = global.PIDvel[2].iAccumulator;
-                    }
-                }
-
-                if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
-                    // 6 seconds have past
-                    // lets take an average
-                    neutralThrustEst.average         = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
-                    neutralThrustEst.correction      = neutralThrustEst.average / 1000.0f;
-
-                    global.PIDvel[2].iAccumulator   -= neutralThrustEst.average;
-
-                    neutralThrustEst.start_sampling  = false;
-                    neutralThrustEst.have_correction = true;
-
-                    // Write a new adjustment value
-                    // vtolSelfTuningStats.NeutralThrustOffset  was incremental adjusted above
-                    VtolSelfTuningStatsData new_vtolSelfTuningStats;
-                    // add the average remaining i value to the
-                    new_vtolSelfTuningStats.NeutralThrustOffset      = vtolSelfTuningStats.NeutralThrustOffset + neutralThrustEst.correction;
-                    new_vtolSelfTuningStats.NeutralThrustCorrection  = neutralThrustEst.correction; // the i term thrust correction value applied
-                    new_vtolSelfTuningStats.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
-                    new_vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
-                    VtolSelfTuningStatsSet(&new_vtolSelfTuningStats);
-                }
-            } else {
-                // start a tick count
-                neutralThrustEst.start_sampling = true;
-                neutralThrustEst.count = 0;
-                neutralThrustEst.sum   = 0.0f;
-                neutralThrustEst.max   = 0.0f;
-                neutralThrustEst.min   = 0.0f;
-            }
-        } else {
-            // reset sampling as we did't get 6 continuous seconds
-            neutralThrustEst.start_sampling = false;
-        }
-    } // else we already have a correction for this PH run
-
-    // Generally in braking the downError will be an increased altitude.  We really will rely on cruisecontrol to backoff.
-    stabDesired.Thrust = boundf(vtolSelfTuningStats.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
-
-
-    // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
-    if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
-        attitudeState.Yaw += 120.0f;
-        if (attitudeState.Yaw > 180.0f) {
-            attitudeState.Yaw -= 360.0f;
-        }
-    }
-
-
-    if ( // emergency flyaway detection
-        ( // integral already at its limit
-            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
-            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
-        ) &&
-        // angle between desired and actual velocity >90 degrees (by dot product)
-        (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
-        // quad is moving at significant speed (during flyaway it would keep speeding up)
-        squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
-        ) {
-        global.vtolEmergencyFallback += dT;
-        if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
-            // after emergency timeout, trigger alarm - everything else is handled by callers
-            // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
-            result = 0;
-        }
-    } else {
-        global.vtolEmergencyFallback = 0.0f;
-    }
-
-    // Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
-    // craft should move similarly for 5 deg roll versus 5 deg pitch
-
-    float maxPitch = vtolPathFollowerSettings.MaxRollPitch;
-    if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
-        maxPitch = vtolPathFollowerSettings.BrakeMaxPitch;
-    }
-
-    stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-                               -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-                               -maxPitch, maxPitch);
-    stabDesired.Roll  = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
-                               eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-                               -maxPitch, maxPitch);
-
-    ManualControlCommandData manualControl;
-    ManualControlCommandGet(&manualControl);
-
-
-    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    if (yaw_attitude) {
-        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-        stabDesired.Yaw = yaw_direction;
-    } else {
-        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
-    }
-
-    // default thrust mode to cruise control
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
-
-    // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
-    if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
-        FlightModeSettingsData settings;
-        FlightModeSettingsGet(&settings);
-        FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
-
-        switch (flightStatus.FlightMode) {
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-            thrustMode = settings.Stabilization1Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-            thrustMode = settings.Stabilization2Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-            thrustMode = settings.Stabilization3Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
-            thrustMode = settings.Stabilization4Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
-            thrustMode = settings.Stabilization5Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
-            thrustMode = settings.Stabilization6Settings.Thrust;
-            break;
-        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            // we hard code the "GPS Assisted" PostionHold to use alt-vario which
-            // is a more appropriate throttle mode.  "GPSAssist" adds braking
-            // and a better throttle management to the standard Position Hold.
-            thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
-            break;
-        }
-        stabDesired.StabilizationMode.Thrust = thrustMode;
-        stabDesired.Thrust = manualControl.Thrust;
-    } else if (manual_thrust) {
-        stabDesired.Thrust = manualControl.Thrust;
-    }
-    // else thrust is set by the PID controller
-
-    StabilizationDesiredSet(&stabDesired);
-
-    return result;
-}
-
-/**
- * Compute desired attitude for vtols - emergency fallback
- */
-static void updateVtolDesiredAttitudeEmergencyFallback()
-{
-    const float dT = updatePeriod / 1000.0f;
-
-    VelocityDesiredData velocityDesired;
-    VelocityStateData velocityState;
-    StabilizationDesiredData stabDesired;
-
-    float courseError;
-    float courseCommand;
-
-    float downError;
-    float downCommand;
-
-    VelocityStateGet(&velocityState);
-    VelocityDesiredGet(&velocityDesired);
-
-    ManualControlCommandData manualControlData;
-    ManualControlCommandGet(&manualControlData);
-
-    courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
-
-    if (courseError < -180.0f) {
-        courseError += 360.0f;
-    }
-    if (courseError > 180.0f) {
-        courseError -= 360.0f;
-    }
-
-
-    courseCommand   = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
-
-    stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
-
-    // Compute desired down command
-    downError   = velocityDesired.Down - velocityState.Down;
-    // Must flip this sign
-    downError   = -downError;
-    downCommand = pid_apply(&global.PIDvel[2], downError, dT);
-
-    stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
-
-
-    stabDesired.Roll   = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
-    stabDesired.Pitch  = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
-
-    if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
-        // For now override thrust with manual control.  Disable at your risk, quad goes to China.
-        ManualControlCommandData manualControl;
-        ManualControlCommandGet(&manualControl);
-        stabDesired.Thrust = manualControl.Thrust;
-    }
-
-    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
-    StabilizationDesiredSet(&stabDesired);
-}
-
-
-/**
- * Compute desired attitude from a fixed preset
- *
- */
-static void updateFixedAttitude(float *attitude)
-{
-    StabilizationDesiredData stabDesired;
-
-    StabilizationDesiredGet(&stabDesired);
-    stabDesired.Roll   = attitude[0];
-    stabDesired.Pitch  = attitude[1];
-    stabDesired.Yaw    = attitude[2];
-    stabDesired.Thrust = attitude[3];
-    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
-    StabilizationDesiredSet(&stabDesired);
-}

flight/modules/PathFollower/pathfollower.cpp

@@ -0,0 +1,462 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pathfollower.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
+ * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
+ * of @ref ManualControlCommand is Auto.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * 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
+ */
+
+/**
+ * Input object: PathDesired
+ * Input object: PositionState
+ * Input object: ManualControlCommand
+ * Output object: StabilizationDesired
+ *
+ * This module acts as "autopilot" - it controls the setpoints of stabilization
+ * based on current flight situation and desired flight path (PathDesired) as
+ * directed by flightmode selection or pathplanner
+ * This is a periodic delayed callback module
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <groundpathfollowersettings.h>
+#include <fixedwingpathfollowersettings.h>
+#include <fixedwingpathfollowerstatus.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+#include <pidstatus.h>
+#include <homelocation.h>
+#include <accelstate.h>
+#include <statusvtolland.h>
+#include <statusgrounddrive.h>
+}
+
+#include "pathfollowercontrol.h"
+#include "vtollandcontroller.h"
+#include "vtolvelocitycontroller.h"
+#include "vtolbrakecontroller.h"
+#include "vtolflycontroller.h"
+#include "fixedwingflycontroller.h"
+#include "grounddrivecontroller.h"
+
+// Private constants
+
+#define CALLBACK_PRIORITY      CALLBACK_PRIORITY_LOW
+#define CBTASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
+
+#define PF_IDLE_UPDATE_RATE_MS 100
+
+#define STACK_SIZE_BYTES       2048
+
+
+// Private types
+
+// Private variables
+static DelayedCallbackInfo *pathFollowerCBInfo;
+static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
+static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
+static PathStatusData pathStatus;
+static PathDesiredData pathDesired;
+static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
+static GroundPathFollowerSettingsData groundPathFollowerSettings;
+static VtolPathFollowerSettingsData vtolPathFollowerSettings;
+static FlightStatusData flightStatus;
+static PathFollowerControl *activeController = 0;
+
+// Private functions
+static void pathFollowerTask(void);
+static void SettingsUpdatedCb(UAVObjEvent *ev);
+static void airspeedStateUpdatedCb(UAVObjEvent *ev);
+static void pathFollowerObjectiveUpdatedCb(UAVObjEvent *ev);
+static void flightStatusUpdatedCb(UAVObjEvent *ev);
+static void updateFixedAttitude(float *attitude);
+static void pathFollowerInitializeControllersForFrameType();
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+extern "C" int32_t PathFollowerStart()
+{
+    // Start main task
+    PathStatusGet(&pathStatus);
+    SettingsUpdatedCb(NULL);
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
+
+    return 0;
+}
+
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+extern "C" int32_t PathFollowerInitialize()
+{
+    // initialize objects
+    GroundPathFollowerSettingsInitialize();
+    FixedWingPathFollowerSettingsInitialize();
+    FixedWingPathFollowerStatusInitialize();
+    VtolPathFollowerSettingsInitialize();
+    FlightStatusInitialize();
+    FlightModeSettingsInitialize();
+    PathStatusInitialize();
+    PathSummaryInitialize();
+    PathDesiredInitialize();
+    PositionStateInitialize();
+    VelocityStateInitialize();
+    VelocityDesiredInitialize();
+    StabilizationDesiredInitialize();
+    AirspeedStateInitialize();
+    AttitudeStateInitialize();
+    TakeOffLocationInitialize();
+    PoiLocationInitialize();
+    ManualControlCommandInitialize();
+    SystemSettingsInitialize();
+    StabilizationBankInitialize();
+    VtolSelfTuningStatsInitialize();
+    PIDStatusInitialize();
+    StatusVtolLandInitialize();
+    StatusGroundDriveInitialize();
+
+    // VtolLandFSM additional objects
+    HomeLocationInitialize();
+    AccelStateInitialize();
+
+    // Init references to controllers
+    PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus);
+
+    // Create object queue
+    pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
+    FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
+    PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb);
+    FlightStatusConnectCallback(&flightStatusUpdatedCb);
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
+    AirspeedStateConnectCallback(&airspeedStateUpdatedCb);
+    VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
+
+    return 0;
+}
+MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
+
+void pathFollowerInitializeControllersForFrameType()
+{
+    static uint8_t multirotor_initialised = 0;
+    static uint8_t fixedwing_initialised  = 0;
+    static uint8_t ground_initialised     = 0;
+
+    switch (frameType) {
+    case FRAME_TYPE_MULTIROTOR:
+    case FRAME_TYPE_HELI:
+        if (!multirotor_initialised) {
+            VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
+            VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
+            multirotor_initialised = 1;
+        }
+        break;
+
+    case FRAME_TYPE_FIXED_WING:
+        if (!fixedwing_initialised) {
+            FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
+            fixedwing_initialised = 1;
+        }
+        break;
+
+    case FRAME_TYPE_GROUND:
+        if (!ground_initialised) {
+            GroundDriveController::instance()->Initialize(&groundPathFollowerSettings);
+            ground_initialised = 1;
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
+static void pathFollowerSetActiveController(void)
+{
+    // Init controllers for the frame type
+    if (activeController == 0) {
+        // Initialise
+        pathFollowerInitializeControllersForFrameType();
+
+        switch (frameType) {
+        case FRAME_TYPE_MULTIROTOR:
+        case FRAME_TYPE_HELI:
+
+            switch (pathDesired.Mode) {
+            case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller
+                activeController = VtolBrakeController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_VELOCITY: // velocity roam controller
+                activeController = VtolVelocityController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_GOTOENDPOINT:
+            case PATHDESIRED_MODE_FOLLOWVECTOR:
+            case PATHDESIRED_MODE_CIRCLERIGHT:
+            case PATHDESIRED_MODE_CIRCLELEFT:
+                activeController = VtolFlyController::instance();
+                activeController->Activate();
+                break;
+            case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
+                activeController = VtolLandController::instance();
+                activeController->Activate();
+                break;
+            default:
+                activeController = 0;
+                break;
+            }
+            break;
+
+        case FRAME_TYPE_FIXED_WING:
+
+            switch (pathDesired.Mode) {
+            case PATHDESIRED_MODE_GOTOENDPOINT:
+            case PATHDESIRED_MODE_FOLLOWVECTOR:
+            case PATHDESIRED_MODE_CIRCLERIGHT:
+            case PATHDESIRED_MODE_CIRCLELEFT:
+                activeController = FixedWingFlyController::instance();
+                activeController->Activate();
+                break;
+            default:
+                activeController = 0;
+                break;
+            }
+            break;
+
+        case FRAME_TYPE_GROUND:
+
+            switch (pathDesired.Mode) {
+            case PATHDESIRED_MODE_GOTOENDPOINT:
+            case PATHDESIRED_MODE_FOLLOWVECTOR:
+            case PATHDESIRED_MODE_CIRCLERIGHT:
+            case PATHDESIRED_MODE_CIRCLELEFT:
+                activeController = GroundDriveController::instance();
+                activeController->Activate();
+                break;
+            default:
+                activeController = 0;
+                break;
+            }
+            break;
+
+        default:
+            activeController = 0;
+            break;
+        }
+    }
+}
+
+/**
+ * Module thread, should not return.
+ */
+static void pathFollowerTask(void)
+{
+    FlightStatusGet(&flightStatus);
+
+    if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
+        PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
+        return;
+    }
+
+    pathStatus.UID    = pathDesired.UID;
+    pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+
+    pathFollowerSetActiveController();
+
+    if (activeController) {
+        activeController->UpdateAutoPilot();
+        PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
+        return;
+    }
+
+    switch (pathDesired.Mode) {
+    case PATHDESIRED_MODE_FIXEDATTITUDE:
+        updateFixedAttitude(pathDesired.ModeParameters);
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+        break;
+    case PATHDESIRED_MODE_DISARMALARM:
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
+        break;
+    default:
+        pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+        break;
+    }
+    PathStatusSet(&pathStatus);
+
+    PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
+}
+
+
+static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PathDesiredGet(&pathDesired);
+
+    if (activeController && pathDesired.Mode != activeController->Mode()) {
+        activeController->Deactivate();
+        activeController = 0;
+    }
+
+    pathFollowerSetActiveController();
+
+    if (activeController) {
+        activeController->ObjectiveUpdated();
+    }
+}
+
+// we use this to deactivate active controllers as the pathdesired
+// objective never gets updated with switching to a non-pathfollower flight mode
+static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    if (!activeController) {
+        return;
+    }
+
+    FlightStatusControlChainData controlChain;
+    FlightStatusControlChainGet(&controlChain);
+
+    if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+        activeController->Deactivate();
+        activeController = 0;
+    }
+}
+
+
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
+
+    FrameType_t previous_frameType = frameType;
+    frameType = GetCurrentFrameType();
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            break;
+        }
+    }
+
+    // If frame type changes, initialise the frame type controllers
+    if (frameType != previous_frameType) {
+        pathFollowerInitializeControllersForFrameType();
+    }
+
+    switch (frameType) {
+    case FRAME_TYPE_MULTIROTOR:
+    case FRAME_TYPE_HELI:
+        updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
+        break;
+    case FRAME_TYPE_FIXED_WING:
+        FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
+        updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
+        break;
+    case FRAME_TYPE_GROUND:
+        GroundPathFollowerSettingsGet(&groundPathFollowerSettings);
+        updatePeriod = groundPathFollowerSettings.UpdatePeriod;
+        break;
+    default:
+        updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
+        break;
+    }
+
+    if (activeController) {
+        activeController->SettingsUpdated();
+    }
+}
+
+
+static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
+}
+
+
+/**
+ * Compute desired attitude from a fixed preset
+ *
+ */
+static void updateFixedAttitude(float *attitude)
+{
+    StabilizationDesiredData stabDesired;
+
+    StabilizationDesiredGet(&stabDesired);
+    stabDesired.Roll   = attitude[0];
+    stabDesired.Pitch  = attitude[1];
+    stabDesired.Yaw    = attitude[2];
+    stabDesired.Thrust = attitude[3];
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
+    StabilizationDesiredSet(&stabDesired);
+}

flight/modules/PathFollower/pathfollowercontrol.cpp

@@ -0,0 +1,52 @@
+/*
+ ******************************************************************************
+ *
+ * @file       PathFollowerControl.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Controller interface class implementation
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+#include <flightstatus.h>
+#include <pathstatus.h>
+#include <pathdesired.h>
+}
+
+// C++ includes
+#include "pathfollowercontrol.h"
+
+PathDesiredData *PathFollowerControl::pathDesired   = 0;
+FlightStatusData *PathFollowerControl::flightStatus = 0;
+PathStatusData *PathFollowerControl::pathStatus     = 0;
+
+int32_t PathFollowerControl::Initialize(PathDesiredData *ptr_pathDesired,
+                                        FlightStatusData *ptr_flightStatus,
+                                        PathStatusData *ptr_pathStatus)
+{
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+    PIOS_Assert(ptr_pathStatus);
+
+    pathDesired  = ptr_pathDesired;
+    flightStatus = ptr_flightStatus;
+    pathStatus   = ptr_pathStatus;
+    return 0;
+}

flight/modules/PathFollower/pidcontroldown.cpp

@@ -0,0 +1,375 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower PID interface class
+ * @brief PID loop for down control
+ * @{
+ *
+ * @file       pidcontroldown.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes PID control for down direction
+ *
+ * @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
+ */
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <stabilizationdesired.h>
+#include <pidstatus.h>
+#include <vtolselftuningstats.h>
+}
+
+#include "pathfollowerfsm.h"
+#include "pidcontroldown.h"
+
+#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
+#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT      0.2f
+#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT        0.2f
+#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT        0.1f
+
+#define NEUTRALTHRUST_START_DELAY               (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
+#define NEUTRALTHRUST_END_COUNT                 (NEUTRALTHRUST_START_DELAY + (4 * 20))  // 4 second sample
+
+
+PIDControlDown::PIDControlDown()
+    : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
+    mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
+    mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
+{
+    Deactivate();
+}
+
+PIDControlDown::~PIDControlDown() {}
+
+void PIDControlDown::Initialize(PathFollowerFSM *fsm)
+{
+    mFSM = fsm;
+}
+
+void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
+{
+    mMinThrust = min_thrust;
+    mMaxThrust = max_thrust;
+}
+
+void PIDControlDown::Deactivate()
+{
+    // pid_zero(&PID);
+    mActive = false;
+}
+
+void PIDControlDown::Activate()
+{
+    float currentThrust;
+
+    StabilizationDesiredThrustGet(&currentThrust);
+    float u0 = currentThrust - mNeutral;
+    pid2_transfer(&PID, u0);
+    mActive = true;
+}
+
+void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
+{
+    // pid_configure(&PID, kp, ki, kd, ilimit);
+    float Ti = kp / ki;
+    float Td = kd / kp;
+    float Tt = (Ti + Td) / 2.0f;
+    float kt = 1.0f / Tt;
+    float N  = 10.0f;
+    float Tf = Td / N;
+
+    if (ki < 1e-6f) {
+        // Avoid Ti being infinite
+        Ti = 1e6f;
+        // Tt antiwindup time constant - we don't need antiwindup with no I term
+        Tt = 1e6f;
+        kt = 0.0f;
+    }
+
+    if (kd < 1e-6f) {
+        // PI Controller or P Controller
+        Tf = Ti / N;
+    }
+
+    if (beta > 1.0f) {
+        beta = 1.0f;
+    } else if (beta < 0.4f) {
+        beta = 0.4f;
+    }
+
+    pid2_configure(&PID, kp, ki, kd, Tf, kt, dT, beta, mNeutral, mNeutral, -1.0f);
+    deltaTime    = dT;
+    mVelocityMax = velocityMax;
+}
+
+
+void PIDControlDown::UpdatePositionalParameters(float kp)
+{
+    pid_configure(&PIDpos, kp, 0.0f, 0.0f, 0.0f);
+}
+void PIDControlDown::UpdatePositionSetpoint(float setpointDown)
+{
+    mPositionSetpointTarget = setpointDown;
+}
+void PIDControlDown::UpdatePositionState(float pvDown)
+{
+    mPositionState = pvDown;
+    setup_neutralThrustCalc();
+}
+// This is a pure position hold position control
+void PIDControlDown::ControlPosition()
+{
+    // Current progress location relative to end
+    float velDown = 0.0f;
+
+    velDown = pid_apply(&PIDpos, mPositionSetpointTarget - mPositionState, deltaTime);
+    UpdateVelocitySetpoint(velDown);
+
+    run_neutralThrustCalc();
+}
+
+
+void PIDControlDown::ControlPositionWithPath(struct path_status *progress)
+{
+    // Current progress location relative to end
+    float velDown = progress->path_vector[2];
+
+    velDown += pid_apply(&PIDpos, progress->correction_vector[2], deltaTime);
+    UpdateVelocitySetpoint(velDown);
+}
+
+
+void PIDControlDown::run_neutralThrustCalc(void)
+{
+    // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
+    // note that arming into this flight mode is not allowed, so assumption here is that
+    // we have not landed.  If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
+    // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
+    // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
+    // avoiding auto-takeoffs.  Therefore no need to check that here.
+
+    if (neutralThrustEst.have_correction != true) {
+        // Make FSM specific
+        bool stable = (fabsf(mPositionSetpointTarget - mPositionState) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
+                       fabsf(mVelocitySetpointCurrent) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
+                       fabsf(mVelocityState) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
+                       fabsf(mVelocitySetpointCurrent - mVelocityState) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
+
+        if (stable) {
+            if (neutralThrustEst.start_sampling) {
+                neutralThrustEst.count++;
+
+
+                // delay count for 2 seconds into hold allowing for stablisation
+                if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
+                    neutralThrustEst.sum += PID.I;
+                    if (PID.I < neutralThrustEst.min) {
+                        neutralThrustEst.min = PID.I;
+                    }
+                    if (PID.I > neutralThrustEst.max) {
+                        neutralThrustEst.max = PID.I;
+                    }
+                }
+
+                if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
+                    // 6 seconds have past
+                    // lets take an average
+                    neutralThrustEst.average    = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
+                    neutralThrustEst.correction = neutralThrustEst.average;
+
+                    PID.I -= neutralThrustEst.average;
+
+                    neutralThrustEst.start_sampling  = false;
+                    neutralThrustEst.have_correction = true;
+
+                    // Write a new adjustment value
+                    // vtolSelfTuningStats.NeutralThrustOffset  was incremental adjusted above
+                    VtolSelfTuningStatsData vtolSelfTuningStats;
+                    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+                    // add the average remaining i value to the
+                    vtolSelfTuningStats.NeutralThrustOffset     += neutralThrustEst.correction;
+                    vtolSelfTuningStats.NeutralThrustCorrection  = neutralThrustEst.correction; // the i term thrust correction value applied
+                    vtolSelfTuningStats.NeutralThrustAccumulator = PID.I; // the actual iaccumulator value after correction
+                    vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
+                    VtolSelfTuningStatsSet(&vtolSelfTuningStats);
+                }
+            } else {
+                // start a tick count
+                neutralThrustEst.start_sampling = true;
+                neutralThrustEst.count = 0;
+                neutralThrustEst.sum   = 0.0f;
+                neutralThrustEst.max   = 0.0f;
+                neutralThrustEst.min   = 0.0f;
+            }
+        } else {
+            // reset sampling as we did't get 6 continuous seconds
+            neutralThrustEst.start_sampling = false;
+        }
+    } // else we already have a correction for this PH run
+}
+
+
+void PIDControlDown::setup_neutralThrustCalc(void)
+{
+    // reset neutral thrust assessment.
+    // and do once for each position hold engagement
+    neutralThrustEst.start_sampling  = false;
+    neutralThrustEst.count = 0;
+    neutralThrustEst.sum = 0.0f;
+    neutralThrustEst.have_correction = false;
+    neutralThrustEst.average    = 0.0f;
+    neutralThrustEst.correction = 0.0f;
+    neutralThrustEst.min = 0.0f;
+    neutralThrustEst.max = 0.0f;
+}
+
+
+void PIDControlDown::UpdateNeutralThrust(float neutral)
+{
+    if (mActive) {
+        // adjust neutral and achieve bumpless transfer
+        PID.va = neutral;
+        pid2_transfer(&PID, mDownCommand);
+    }
+    mNeutral = neutral;
+}
+
+void PIDControlDown::UpdateVelocitySetpoint(float setpoint)
+{
+    mVelocitySetpointTarget = setpoint;
+    if (fabsf(mVelocitySetpointTarget) > mVelocityMax) {
+        // maintain sign but set to max
+        mVelocitySetpointTarget *= mVelocityMax / fabsf(mVelocitySetpointTarget);
+    }
+}
+
+void PIDControlDown::RateLimit(float *spDesired, float *spCurrent, float rateLimit)
+{
+    float velocity_delta = *spDesired - *spCurrent;
+
+    if (fabsf(velocity_delta) < 1e-6f) {
+        *spCurrent = *spDesired;
+        return;
+    }
+
+    // Calculate the rate of change
+    float accelerationDesired = velocity_delta / deltaTime;
+
+    if (fabsf(accelerationDesired) > rateLimit) {
+        accelerationDesired *= rateLimit / accelerationDesired;
+    }
+
+    if (fabsf(accelerationDesired) < 0.1f) {
+        *spCurrent = *spDesired;
+    } else {
+        *spCurrent += accelerationDesired * deltaTime;
+    }
+}
+
+void PIDControlDown::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
+{
+    if (startingVelocity >= 0.0f) {
+        *updatedVelocity = startingVelocity - dT * brakeRate;
+        if (*updatedVelocity > currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity < 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    } else {
+        *updatedVelocity = startingVelocity + dT * brakeRate;
+        if (*updatedVelocity < currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity > 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    }
+}
+
+void PIDControlDown::UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate)
+{
+    mVelocityState = pvDown;
+
+    float velocitySetpointDesired;
+
+    UpdateBrakeVelocity(mVelocitySetpointTarget, path_time, brakeRate, pvDown, &velocitySetpointDesired);
+
+    // Calculate the rate of change
+    // RateLimit(velocitySetpointDesired[iaxis], mVelocitySetpointCurrent[iaxis], 2.0f );
+
+    mVelocitySetpointCurrent = velocitySetpointDesired;
+}
+
+
+// Update velocity state called per dT. Also update current
+// desired velocity
+void PIDControlDown::UpdateVelocityState(float pv)
+{
+    mVelocityState = pv;
+
+    if (mFSM) {
+        // The FSM controls the actual descent velocity and introduces step changes as required
+        float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget);
+        // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
+        mVelocitySetpointCurrent = velocitySetpointDesired;
+    } else {
+        mVelocitySetpointCurrent = mVelocitySetpointTarget;
+    }
+}
+
+float PIDControlDown::GetVelocityDesired(void)
+{
+    return mVelocitySetpointCurrent;
+}
+
+float PIDControlDown::GetDownCommand(void)
+{
+    PIDStatusData pidStatus;
+    float ulow  = mMinThrust;
+    float uhigh = mMaxThrust;
+
+    if (mFSM) {
+        mFSM->BoundThrust(ulow, uhigh);
+    }
+    float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
+    pidStatus.setpoint = mVelocitySetpointCurrent;
+    pidStatus.actual   = mVelocityState;
+    pidStatus.error    = mVelocitySetpointCurrent - mVelocityState;
+    pidStatus.setpoint = mVelocitySetpointCurrent;
+    pidStatus.ulow     = ulow;
+    pidStatus.uhigh    = uhigh;
+    pidStatus.command  = downCommand;
+    pidStatus.P  = PID.P;
+    pidStatus.I  = PID.I;
+    pidStatus.D  = PID.D;
+    PIDStatusSet(&pidStatus);
+    mDownCommand = downCommand;
+    return mDownCommand;
+}

flight/modules/PathFollower/pidcontrolne.cpp

@@ -0,0 +1,249 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PathFollower CONTROL interface class
+ * @brief PID controller for NE
+ * @{
+ *
+ * @file       PIDControlNE.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Executes PID control loops for NE directions
+ *
+ * @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
+ */
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <pidstatus.h>
+}
+#include "pathfollowerfsm.h"
+#include "pidcontrolne.h"
+
+PIDControlNE::PIDControlNE()
+    : deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
+{}
+
+PIDControlNE::~PIDControlNE() {}
+
+void PIDControlNE::Initialize()
+{}
+
+void PIDControlNE::Deactivate()
+{
+    mActive = false;
+}
+
+void PIDControlNE::Activate()
+{
+    // Do we need to initialise any loops for smooth transition
+    // float currentNE;
+    // StabilizationDesiredNEGet(&currentNE);
+    // float u0 = currentNE - mNeutral;
+    // pid2_transfer(&PID, u0);
+    mActive = true;
+}
+
+void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
+{
+    // pid_configure(&PID, kp, ki, kd, ilimit);
+    float Ti = kp / ki;
+    float Td = kd / kp;
+    float Tt = (Ti + Td) / 2.0f;
+    float kt = 1.0f / Tt;
+    float u0 = 0.0f;
+    float N  = 10.0f;
+    float Tf = Td / N;
+
+    if (ki < 1e-6f) {
+        // Avoid Ti being infinite
+        Ti = 1e6f;
+        // Tt antiwindup time constant - we don't need antiwindup with no I term
+        Tt = 1e6f;
+        kt = 0.0f;
+    }
+
+    if (kd < 1e-6f) {
+        // PI Controller
+        Tf = Ti / N;
+    }
+
+    if (beta > 1.0f) {
+        beta = 1.0f;
+    } else if (beta < 0.4f) {
+        beta = 0.4f;
+    }
+
+    pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
+    pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
+    deltaTime    = dT;
+    mVelocityMax = velocityMax;
+}
+
+void PIDControlNE::UpdatePositionalParameters(float kp)
+{
+    pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f);
+    pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f);
+}
+void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast)
+{
+    mPositionSetpointTarget[0] = setpointNorth;
+    mPositionSetpointTarget[1] = setpointEast;
+}
+void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast)
+{
+    mPositionState[0] = pvNorth;
+    mPositionState[1] = pvEast;
+}
+// This is a pure position hold position control
+void PIDControlNE::ControlPosition()
+{
+    // Current progress location relative to end
+    float velNorth = 0.0f;
+    float velEast  = 0.0f;
+
+    velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime);
+    velEast  = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime);
+    UpdateVelocitySetpoint(velNorth, velEast);
+}
+
+void PIDControlNE::ControlPositionWithPath(struct path_status *progress)
+{
+    // Current progress location relative to end
+    float velNorth = progress->path_vector[0];
+    float velEast  = progress->path_vector[1];
+
+    velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime);
+    velEast  += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime);
+    UpdateVelocitySetpoint(velNorth, velEast);
+}
+
+void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast)
+{
+    // scale velocity if it is above configured maximum
+    // for braking, we can not help it if initial velocity was greater
+    float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast);
+
+    if (velH > mVelocityMax) {
+        setpointNorth *= mVelocityMax / velH;
+        setpointEast  *= mVelocityMax / velH;
+    }
+
+    mVelocitySetpointTarget[0] = setpointNorth;
+    mVelocitySetpointTarget[1] = setpointEast;
+}
+
+
+void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
+{
+    if (startingVelocity >= 0.0f) {
+        *updatedVelocity = startingVelocity - dT * brakeRate;
+        if (*updatedVelocity > currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity < 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    } else {
+        *updatedVelocity = startingVelocity + dT * brakeRate;
+        if (*updatedVelocity < currentVelocity) {
+            *updatedVelocity = currentVelocity;
+        }
+        if (*updatedVelocity > 0.0f) {
+            *updatedVelocity = 0.0f;
+        }
+    }
+}
+
+void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate)
+{
+    mVelocityState[0] = pvNorth;
+    mVelocityState[1] = pvEast;
+
+    float velocitySetpointDesired[2];
+
+    UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]);
+    UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]);
+
+    // If rate of change limits required, add here
+    for (int iaxis = 0; iaxis < 2; iaxis++) {
+        mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
+    }
+}
+
+void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast)
+{
+    mVelocityState[0] = pvNorth;
+    mVelocityState[1] = pvEast;
+
+    // The FSM controls the actual descent velocity and introduces step changes as required
+    float velocitySetpointDesired[2];
+    velocitySetpointDesired[0] = mVelocitySetpointTarget[0];
+    velocitySetpointDesired[1] = mVelocitySetpointTarget[1];
+
+    // If rate of change limits required, add here
+    for (int iaxis = 0; iaxis < 2; iaxis++) {
+        mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
+    }
+}
+
+
+void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward)
+{
+    mMinCommand = minCommand;
+    mMaxCommand = maxCommand;
+    mVelocityFeedforward = velocityFeedforward;
+}
+
+
+void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand)
+{
+    PIDvel[0].va  = mVelocitySetpointCurrent[0] * mVelocityFeedforward;
+    *northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand);
+    PIDvel[1].va  = mVelocitySetpointCurrent[1] * mVelocityFeedforward;
+    *eastCommand  = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand);
+
+    PIDStatusData pidStatus;
+    pidStatus.setpoint = mVelocitySetpointCurrent[0];
+    pidStatus.actual   = mVelocityState[0];
+    pidStatus.error    = mVelocitySetpointCurrent[0] - mVelocityState[0];
+    pidStatus.setpoint = mVelocitySetpointCurrent[0];
+    pidStatus.ulow     = mMinCommand;
+    pidStatus.uhigh    = mMaxCommand;
+    pidStatus.command  = *northCommand;
+    pidStatus.P = PIDvel[0].P;
+    pidStatus.I = PIDvel[0].I;
+    pidStatus.D = PIDvel[0].D;
+    PIDStatusSet(&pidStatus);
+}
+
+void PIDControlNE::GetVelocityDesired(float *north, float *east)
+{
+    *north = mVelocitySetpointCurrent[0];
+    *east  = mVelocitySetpointCurrent[1];
+}

flight/modules/PathFollower/vtolbrakecontroller.cpp

@@ -0,0 +1,355 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolbrakecontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      This landing state machine is a helper state machine to the
+ *              pathfollower task/thread to implement detailed landing controls.
+ *		This is to be called only from the pathfollower task.
+ *		Note that initiation of the land occurs in the manual control
+ *		command thread calling plans.c plan_setup_land which writes
+ *		the required PathDesired LAND mode.
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolbrakecontroller.h"
+#include "pathfollowerfsm.h"
+#include "vtolbrakefsm.h"
+#include "pidcontroldown.h"
+
+// Private constants
+#define BRAKE_RATE_MINIMUM 0.2f
+
+// pointer to a singleton instance
+VtolBrakeController *VtolBrakeController::p_inst = 0;
+
+VtolBrakeController::VtolBrakeController()
+    : fsm(0), vtolPathFollowerSettings(0), mActive(false), mHoldActive(false), mManualThrust(false)
+{}
+
+// Called when mode first engaged
+void VtolBrakeController::Activate(void)
+{
+    if (!mActive) {
+        mActive       = true;
+        mHoldActive   = false;
+        mManualThrust = false;
+        SettingsUpdated();
+        fsm->Activate();
+        controlDown.Activate();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolBrakeController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolBrakeController::Mode(void)
+{
+    return PATHDESIRED_MODE_BRAKE;
+}
+
+// Objective updated in pathdesired
+void VtolBrakeController::ObjectiveUpdated(void)
+{
+    // Set the objective's target velocity
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]);
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]);
+    if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
+        pathStatus->path_time = 0.0f;
+    }
+}
+void VtolBrakeController::Deactivate(void)
+{
+    if (mActive) {
+        mActive       = false;
+        mHoldActive   = false;
+        mManualThrust = false;
+        fsm->Inactive();
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+    }
+}
+
+
+void VtolBrakeController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit,
+                               dT,
+                               10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+                                 dT,
+                                 10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    fsm->SettingsUpdated();
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolBrakeController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    if (fsm == 0) {
+        VtolBrakeFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus, pathStatus);
+        fsm = (PathFollowerFSM *)VtolBrakeFSM::instance();
+        controlDown.Initialize(fsm);
+    }
+    return 0;
+}
+
+
+void VtolBrakeController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    float brakeRate = vtolPathFollowerSettings->BrakeRate;
+    if (brakeRate < BRAKE_RATE_MINIMUM) {
+        brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
+    }
+
+    if (fsm->PositionHoldState()) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+
+        // On first engagement set the position setpoint
+        if (!mHoldActive) {
+            mHoldActive = true;
+            controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
+            if (!mManualThrust) {
+                controlDown.UpdatePositionSetpoint(positionState.Down);
+            }
+        }
+
+        // Update position state and control position to create inputs to velocity control
+        controlNE.UpdatePositionState(positionState.North, positionState.East);
+        controlNE.ControlPosition();
+        if (!mManualThrust) {
+            controlDown.UpdatePositionState(positionState.Down);
+            controlDown.ControlPosition();
+        }
+
+        controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+        if (!mManualThrust) {
+            controlDown.UpdateVelocityState(velocityState.Down);
+        }
+    } else {
+        controlNE.UpdateVelocityStateWithBrake(velocityState.North, velocityState.East, pathStatus->path_time, brakeRate);
+        if (!mManualThrust) {
+            controlDown.UpdateVelocityStateWithBrake(velocityState.Down, pathStatus->path_time, brakeRate);
+        }
+    }
+
+    if (!mManualThrust) {
+        velocityDesired.Down = controlDown.GetVelocityDesired();
+    } else { velocityDesired.Down = 0.0f; }
+
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    VelocityDesiredSet(&velocityDesired);
+
+    // update pathstatus
+    float cur_velocity     = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
+    cur_velocity      = sqrtf(cur_velocity);
+    float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
+    desired_velocity  = sqrtf(desired_velocity);
+    pathStatus->error = cur_velocity - desired_velocity;
+    pathStatus->fractional_progress = 1.0f;
+    if (pathDesired->StartingVelocity > 0.0f) {
+        pathStatus->fractional_progress = (pathDesired->StartingVelocity - cur_velocity) / pathDesired->StartingVelocity;
+
+        // sometimes current velocity can exceed starting velocity due to initial acceleration
+        if (pathStatus->fractional_progress < 0.0f) {
+            pathStatus->fractional_progress = 0.0f;
+        }
+    }
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+}
+
+int8_t VtolBrakeController::UpdateStabilizationDesired(void)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->BrakeMaxPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
+    if (flightStatus->FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
+        FlightModeSettingsData settings;
+        FlightModeSettingsGet(&settings);
+        uint8_t thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
+
+        switch (flightStatus->FlightMode) {
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+            thrustMode = settings.Stabilization1Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+            thrustMode = settings.Stabilization2Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+            thrustMode = settings.Stabilization3Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
+            thrustMode = settings.Stabilization4Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
+            thrustMode = settings.Stabilization5Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
+            thrustMode = settings.Stabilization6Settings.Thrust;
+            break;
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+            // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
+            // is a more appropriate throttle mode.  "GPSAssist" adds braking
+            // and a better throttle management to the standard Position Hold.
+            thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
+            break;
+        }
+        stabDesired.StabilizationMode.Thrust = thrustMode;
+    }
+
+    // set the thrust value
+    if (mManualThrust) {
+        stabDesired.Thrust = manualControl.Thrust;
+    } else {
+        stabDesired.Thrust = controlDown.GetDownCommand();
+    }
+
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+
+void VtolBrakeController::UpdateAutoPilot()
+{
+    if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
+        pathStatus->path_time += vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+    }
+
+    if (flightStatus->FlightModeAssist && flightStatus->AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
+        mManualThrust = true;
+    } else {
+        mManualThrust = false;
+    }
+
+    fsm->Update(); // This will run above end condition checks
+
+    UpdateVelocityDesired();
+
+    int8_t result = UpdateStabilizationDesired();
+
+    if (!result) {
+        fsm->Abort(); // enter PH
+    }
+
+    PathStatusSet(pathStatus);
+}

flight/modules/PathFollower/vtolbrakefsm.cpp

@@ -0,0 +1,261 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolbrakefsm.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Vtol brake finate state machine to regulate behaviour of the
+ *              brake controller.
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include <vtolbrakefsm.h>
+
+
+// Private constants
+#define TIMER_COUNT_PER_SECOND                      (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
+#define BRAKE_EXIT_VELOCITY_LIMIT                   0.2f
+
+VtolBrakeFSM::PathFollowerFSM_BrakeStateHandler_T VtolBrakeFSM::sBrakeStateTable[BRAKE_STATE_SIZE] = {
+    [BRAKE_STATE_INACTIVE] = { .setup = 0,                          .run = 0                        },
+    [BRAKE_STATE_BRAKE]    = { .setup = &VtolBrakeFSM::setup_brake, .run = &VtolBrakeFSM::run_brake },
+    [BRAKE_STATE_HOLD]     = { .setup = 0,                          .run = 0                        }
+};
+
+// pointer to a singleton instance
+VtolBrakeFSM *VtolBrakeFSM::p_inst = 0;
+
+
+VtolBrakeFSM::VtolBrakeFSM()
+    : mBrakeData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
+{}
+
+
+// Private types
+
+// Private functions
+// Public API methods
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
+                                 PathDesiredData *ptr_pathDesired,
+                                 FlightStatusData *ptr_flightStatus,
+                                 PathStatusData *ptr_pathStatus)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+
+    // allow for Initialize being called more than once.
+    if (!mBrakeData) {
+        mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T));
+        PIOS_Assert(mBrakeData);
+    }
+    memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0);
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    pathDesired  = ptr_pathDesired;
+    flightStatus = ptr_flightStatus;
+    pathStatus   = ptr_pathStatus;
+    initFSM();
+
+    return 0;
+}
+
+void VtolBrakeFSM::Inactive(void)
+{
+    memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0);
+    initFSM();
+}
+
+// Initialise the FSM
+void VtolBrakeFSM::initFSM(void)
+{
+    mBrakeData->currentState = BRAKE_STATE_INACTIVE;
+}
+
+void VtolBrakeFSM::Activate()
+{
+    memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0);
+    mBrakeData->currentState = BRAKE_STATE_INACTIVE;
+    setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE);
+}
+
+void VtolBrakeFSM::Abort(void)
+{
+    setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
+}
+
+PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void)
+{
+    switch (mBrakeData->currentState) {
+    case BRAKE_STATE_INACTIVE:
+        return PFFSM_STATE_INACTIVE;
+
+        break;
+    default:
+        return PFFSM_STATE_ACTIVE;
+
+        break;
+    }
+}
+
+void VtolBrakeFSM::Update()
+{
+    runState();
+}
+
+int32_t VtolBrakeFSM::runState(void)
+{
+    uint8_t flTimeout = false;
+
+    mBrakeData->stateRunCount++;
+
+    if (mBrakeData->stateTimeoutCount > 0 && mBrakeData->stateRunCount > mBrakeData->stateTimeoutCount) {
+        flTimeout = true;
+    }
+
+    // If the current state has a static function, call it
+    if (sBrakeStateTable[mBrakeData->currentState].run) {
+        (this->*sBrakeStateTable[mBrakeData->currentState].run)(flTimeout);
+    }
+    return 0;
+}
+
+// Set the new state and perform setup for subsequent state run calls
+// This is called by state run functions on event detection that drive
+// state transitions.
+void VtolBrakeFSM::setState(PathFollowerFSM_BrakeState_T newState, __attribute__((unused)) VtolBrakeFSMStatusStateExitReasonOptions reason)
+{
+    // mBrakeData->fsmBrakeStatus.StateExitReason[mBrakeData->currentState] = reason;
+
+    if (mBrakeData->currentState == newState) {
+        return;
+    }
+    mBrakeData->currentState      = newState;
+
+    // Restart state timer counter
+    mBrakeData->stateRunCount     = 0;
+
+    // Reset state timeout to disabled/zero
+    mBrakeData->stateTimeoutCount = 0;
+
+    if (sBrakeStateTable[mBrakeData->currentState].setup) {
+        (this->*sBrakeStateTable[mBrakeData->currentState].setup)();
+    }
+}
+
+
+// Timeout utility function for use by state init implementations
+void VtolBrakeFSM::setStateTimeout(int32_t count)
+{
+    mBrakeData->stateTimeoutCount = count;
+}
+
+// FSM Setup and Run method implementation
+
+// State: WAITING FOR DESCENT RATE
+void VtolBrakeFSM::setup_brake(void)
+{
+    setStateTimeout(TIMER_COUNT_PER_SECOND * pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]);
+    mBrakeData->observationCount  = 0;
+    mBrakeData->observation2Count = 0;
+}
+
+
+void VtolBrakeFSM::run_brake(uint8_t flTimeout)
+{
+    // Brake mode end condition checks
+    bool exit_brake = false;
+    VelocityStateData velocityState;
+    PathSummaryData pathSummary;
+
+    if (flTimeout) {
+        pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
+        exit_brake = true;
+    } else if (pathStatus->fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
+        VelocityStateGet(&velocityState);
+        if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
+            pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
+            exit_brake = true;
+        }
+    }
+
+    if (exit_brake) {
+        // Calculate the distance error between the originally desired
+        // stopping point and the actual brake-exit point.
+
+        PositionStateData p;
+        PositionStateGet(&p);
+        float north_offset = pathDesired->End.North - p.North;
+        float east_offset  = pathDesired->End.East - p.East;
+        float down_offset  = pathDesired->End.Down - p.Down;
+        pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
+        pathSummary.time_remaining = pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus->path_time;
+        pathSummary.fractional_progress   = pathStatus->fractional_progress;
+        float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
+        cur_velocity = sqrtf(cur_velocity);
+        pathSummary.decelrate = (pathDesired->StartingVelocity - cur_velocity) / pathStatus->path_time;
+        pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings->BrakeRate;
+        pathSummary.velocityIntoHold = cur_velocity;
+        pathSummary.Mode = PATHSUMMARY_MODE_BRAKE;
+        pathSummary.UID  = pathStatus->UID;
+        PathSummarySet(&pathSummary);
+
+        setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
+    }
+}
+
+uint8_t VtolBrakeFSM::PositionHoldState(void)
+{
+    return mBrakeData->currentState == BRAKE_STATE_HOLD;
+}

flight/modules/PathFollower/vtolflycontroller.cpp

@@ -0,0 +1,547 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtolflycontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Class implements the fly controller for vtols
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolflycontroller.h"
+#include "pathfollowerfsm.h"
+#include "pidcontroldown.h"
+#include "pidcontrolne.h"
+
+// Private constants
+#define DEADBAND_HIGH                                        0.10f
+#define DEADBAND_LOW                                         -0.10f
+#define RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS            0.95f
+#define RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE 2.0f
+
+// pointer to a singleton instance
+VtolFlyController *VtolFlyController::p_inst = 0;
+
+VtolFlyController::VtolFlyController()
+    : vtolPathFollowerSettings(NULL), mActive(false), mManualThrust(false), mMode(0), vtolEmergencyFallback(0.0f), vtolEmergencyFallbackSwitch(false)
+{}
+
+// Called when mode first engaged
+void VtolFlyController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        mManualThrust = false;
+        SettingsUpdated();
+        controlDown.Activate();
+        controlNE.Activate();
+        mMode = pathDesired->Mode;
+
+        vtolEmergencyFallback = 0.0f;
+        vtolEmergencyFallbackSwitch = false;
+    }
+}
+
+uint8_t VtolFlyController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolFlyController::Mode(void)
+{
+    return mMode;
+}
+
+// Objective updated in pathdesired
+void VtolFlyController::ObjectiveUpdated(void)
+{}
+
+
+void VtolFlyController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        mManualThrust = false;
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+        vtolEmergencyFallback = 0.0f;
+        vtolEmergencyFallbackSwitch = false;
+    }
+}
+
+
+void VtolFlyController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->VerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->VerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA
+                                 dT,
+                                 vtolPathFollowerSettings->VerticalVelMax);
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolFlyController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+
+    return 0;
+}
+
+
+/**
+ * Compute desired velocity from the current position and path
+ */
+void VtolFlyController::UpdateVelocityDesired()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+
+    VelocityStateData velocityState;
+    VelocityStateGet(&velocityState);
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+    controlDown.UpdateVelocityState(velocityState.Down);
+
+    VelocityDesiredData velocityDesired;
+
+    // look ahead kFF seconds
+    float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
+                     positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
+                     positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
+    struct path_status progress;
+    path_progress(pathDesired, cur, &progress, true);
+
+    controlNE.ControlPositionWithPath(&progress);
+    if (!mManualThrust) {
+        controlDown.ControlPositionWithPath(&progress);
+    }
+
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+    if (!mManualThrust) {
+        velocityDesired.Down = controlDown.GetVelocityDesired();
+    } else { velocityDesired.Down = 0.0f; }
+
+    // update pathstatus
+    pathStatus->error = progress.error;
+    pathStatus->fractional_progress  = progress.fractional_progress;
+    pathStatus->path_direction_north = progress.path_vector[0];
+    pathStatus->path_direction_east  = progress.path_vector[1];
+    pathStatus->path_direction_down  = progress.path_vector[2];
+
+    pathStatus->correction_direction_north = progress.correction_vector[0];
+    pathStatus->correction_direction_east  = progress.correction_vector[1];
+    pathStatus->correction_direction_down  = progress.correction_vector[2];
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+
+int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    // TODO The below need to be rewritten because the PID implementation has changed.
+#if 0
+    // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
+    if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
+        attitudeState.Yaw += 120.0f;
+        if (attitudeState.Yaw > 180.0f) {
+            attitudeState.Yaw -= 360.0f;
+        }
+    }
+
+
+    if ( // emergency flyaway detection
+        ( // integral already at its limit
+            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
+            vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
+        ) &&
+        // angle between desired and actual velocity >90 degrees (by dot product)
+        (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
+        // quad is moving at significant speed (during flyaway it would keep speeding up)
+        squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
+        ) {
+        vtolEmergencyFallback += dT;
+        if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
+            // after emergency timeout, trigger alarm - everything else is handled by callers
+            // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
+            result = 0;
+        }
+    } else {
+        vtolEmergencyFallback = 0.0f;
+    }
+#endif // if 0
+
+    if (yaw_attitude) {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stabDesired.Yaw = yaw_direction;
+    } else {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+    }
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    if (mManualThrust) {
+        stabDesired.Thrust = manualControl.Thrust;
+    } else {
+        stabDesired.Thrust = controlDown.GetDownCommand();
+    }
+
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+/**
+ * Compute desired attitude for vtols - emergency fallback
+ */
+void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
+{
+    VelocityDesiredData velocityDesired;
+    VelocityStateData velocityState;
+    StabilizationDesiredData stabDesired;
+
+    float courseError;
+    float courseCommand;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredGet(&velocityDesired);
+
+    ManualControlCommandData manualControlData;
+    ManualControlCommandGet(&manualControlData);
+
+    courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
+
+    if (courseError < -180.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f) {
+        courseError -= 360.0f;
+    }
+
+
+    courseCommand   = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP);
+    stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max);
+
+    controlDown.UpdateVelocitySetpoint(velocityDesired.Down);
+    controlDown.UpdateVelocityState(velocityState.Down);
+    stabDesired.Thrust = controlDown.GetDownCommand();
+
+
+    stabDesired.Roll   = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll;
+    stabDesired.Pitch  = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch;
+
+    if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
+        stabDesired.Thrust = manualControlData.Thrust;
+    }
+
+    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+    StabilizationDesiredSet(&stabDesired);
+}
+
+
+void VtolFlyController::UpdateAutoPilot()
+{
+    if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
+        mManualThrust = true;
+    }
+
+    uint8_t result = RunAutoPilot();
+
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+    }
+
+    PathStatusSet(pathStatus);
+
+    // If rtbl, detect arrival at the endpoint and then triggers a change
+    // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
+    // can't manage this.  And pathplanner whilst similar does not manage this as it is not a
+    // waypoint traversal and is not aware of flight modes other than path plan.
+    if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
+        if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
+            if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
+                plan_setup_land();
+            }
+        }
+    }
+}
+/**
+ * vtol autopilot
+ * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
+ * fall back to emergency fallback autopilot to keep minimum amount of flight control
+ */
+uint8_t VtolFlyController::RunAutoPilot()
+{
+    enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
+    enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
+    uint8_t result = 0;
+
+    // decide on behaviour based on settings and system state
+    if (vtolEmergencyFallbackSwitch) {
+        returnmode   = RETURN_0;
+        followermode = FOLLOWER_FALLBACK;
+    } else {
+        if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
+            returnmode   = RETURN_1;
+            followermode = FOLLOWER_FALLBACK;
+        } else {
+            returnmode   = RETURN_RESULT;
+            followermode = FOLLOWER_REGULAR;
+        }
+    }
+
+    switch (followermode) {
+    case FOLLOWER_REGULAR:
+    {
+        // horizontal position control PID loop works according to settings in regular mode, allowing integral terms
+        UpdateVelocityDesired();
+
+        // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
+        bool yaw_attitude = true;
+        float yaw = 0.0f;
+
+        switch (vtolPathFollowerSettings->YawControl) {
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
+            yaw_attitude = false;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
+            yaw = updateTailInBearing();
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
+            yaw = updateCourseBearing();
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
+            yaw = updatePathBearing();
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
+            yaw = updatePOIBearing();
+            break;
+        }
+
+        result = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+        if (!result) {
+            if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
+                // switch to emergency follower if follower indicates problems
+                vtolEmergencyFallbackSwitch = true;
+            }
+        }
+    }
+    break;
+    case FOLLOWER_FALLBACK:
+    {
+        // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
+        controlNE.UpdatePositionalParameters(1.0f);
+        UpdateVelocityDesired();
+
+        // emergency follower has no return value
+        UpdateDesiredAttitudeEmergencyFallback();
+    }
+    break;
+    }
+
+    switch (returnmode) {
+    case RETURN_RESULT:
+        return result;
+
+    default:
+        // returns either 0 or 1 according to enum definition above
+        return returnmode;
+    }
+}
+
+
+/**
+ * Compute bearing of current takeoff location
+ */
+float VtolFlyController::updateTailInBearing()
+{
+    PositionStateData p;
+
+    PositionStateGet(&p);
+    TakeOffLocationData t;
+    TakeOffLocationGet(&t);
+    // atan2f always returns in between + and - 180 degrees
+    return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
+}
+
+
+/**
+ * Compute bearing of current movement direction
+ */
+float VtolFlyController::updateCourseBearing()
+{
+    VelocityStateData v;
+
+    VelocityStateGet(&v);
+    // atan2f always returns in between + and - 180 degrees
+    return RAD2DEG(atan2f(v.East, v.North));
+}
+
+
+/**
+ * Compute bearing of current path direction
+ */
+float VtolFlyController::updatePathBearing()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+
+    float cur[3] = { positionState.North,
+                     positionState.East,
+                     positionState.Down };
+    struct path_status progress;
+
+    path_progress(pathDesired, cur, &progress, true);
+
+    // atan2f always returns in between + and - 180 degrees
+    return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
+}
+
+
+/**
+ * Compute bearing between current position and POI
+ */
+float VtolFlyController::updatePOIBearing()
+{
+    PoiLocationData poi;
+
+    PoiLocationGet(&poi);
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+    float dLoc[3];
+    float yaw = 0;
+    /*float elevation = 0;*/
+
+    dLoc[0] = positionState.North - poi.North;
+    dLoc[1] = positionState.East - poi.East;
+    dLoc[2] = positionState.Down - poi.Down;
+
+    if (dLoc[1] < 0) {
+        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
+    } else {
+        yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
+    }
+    ManualControlCommandData manualControlData;
+    ManualControlCommandGet(&manualControlData);
+
+    float pathAngle = 0;
+    if (manualControlData.Roll > DEADBAND_HIGH) {
+        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
+    } else if (manualControlData.Roll < DEADBAND_LOW) {
+        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
+    }
+
+    return yaw + (pathAngle / 2.0f);
+}

flight/modules/PathFollower/vtollandcontroller.cpp

@@ -0,0 +1,283 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtollandcontroller.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Vtol landing controller loop
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <accelstate.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtollandcontroller.h"
+#include "pathfollowerfsm.h"
+#include "vtollandfsm.h"
+#include "pidcontroldown.h"
+
+// Private constants
+
+// pointer to a singleton instance
+VtolLandController *VtolLandController::p_inst = 0;
+
+VtolLandController::VtolLandController()
+    : fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
+{}
+
+// Called when mode first engaged
+void VtolLandController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        fsm->Activate();
+        controlDown.Activate();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolLandController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolLandController::Mode(void)
+{
+    return PATHDESIRED_MODE_LAND;
+}
+
+// Objective updated in pathdesired, e.g. same flight mode but new target velocity
+void VtolLandController::ObjectiveUpdated(void)
+{
+    // Set the objective's target velocity
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+}
+void VtolLandController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        fsm->Inactive();
+        controlDown.Deactivate();
+        controlNE.Deactivate();
+    }
+}
+
+
+void VtolLandController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+
+
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+
+    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+                                 dT,
+                                 vtolPathFollowerSettings->VerticalVelMax);
+
+    // The following is not currently used in the landing control.
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    // initialise limits on thrust but note the FSM can override.
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+    fsm->SettingsUpdated();
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    if (fsm == 0) {
+        fsm = (PathFollowerFSM *)VtolLandFSM::instance();
+        VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
+        vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+        controlDown.Initialize(fsm);
+    }
+
+    return 0;
+}
+
+
+void VtolLandController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    controlDown.UpdateVelocityState(velocityState.Down);
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    // Implement optional horizontal position hold.
+    if (((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) {
+        // landing flight mode has stored original horizontal position in pathdesired
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        controlNE.UpdatePositionState(positionState.North, positionState.East);
+        controlNE.ControlPosition();
+    }
+
+    velocityDesired.Down  = controlDown.GetVelocityDesired();
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    // update pathstatus
+    pathStatus->error     = 0.0f;
+    pathStatus->fractional_progress  = 0.0f;
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+    pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
+
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+    stabDesired.Thrust = controlDown.GetDownCommand();
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    if (yaw_attitude) {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stabDesired.Yaw = yaw_direction;
+    } else {
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+    }
+
+    // default thrust mode to cruise control
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+
+    fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+void VtolLandController::UpdateAutoPilot()
+{
+    fsm->Update();
+
+    UpdateVelocityDesired();
+
+    // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
+    bool yaw_attitude = false;
+    float yaw = 0.0f;
+
+    fsm->GetYaw(yaw_attitude, yaw);
+
+    int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+    if (!result) {
+        fsm->Abort();
+    }
+
+    if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+void VtolLandController::setArmedIfChanged(uint8_t val)
+{
+    if (flightStatus->Armed != val) {
+        flightStatus->Armed = val;
+        FlightStatusSet(flightStatus);
+    }
+}

flight/modules/PathFollower/vtollandfsm.cpp

@@ -0,0 +1,703 @@
+/*
+ ******************************************************************************
+ *
+ * @file       vtollandfsm.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      This landing state machine is a helper state machine to the
+ *             VtolLandController.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <homelocation.h>
+#include <accelstate.h>
+#include <fixedwingpathfollowersettings.h>
+#include <fixedwingpathfollowerstatus.h>
+#include <vtolpathfollowersettings.h>
+#include <flightstatus.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <airspeedstate.h>
+#include <attitudestate.h>
+#include <takeofflocation.h>
+#include <poilocation.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <vtolselftuningstats.h>
+#include <statusvtolland.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include <vtollandfsm.h>
+
+
+// Private constants
+#define TIMER_COUNT_PER_SECOND            (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define MIN_LANDRATE                      0.1f
+#define MAX_LANDRATE                      0.6f
+#define LOW_ALT_DESCENT_REDUCTION_FACTOR  0.7f  // TODO Need to make the transition smooth
+#define LANDRATE_LOWLIMIT_FACTOR          0.5f
+#define LANDRATE_HILIMIT_FACTOR           1.5f
+#define TIMEOUT_INIT_ALTHOLD              (3 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_WTG_FOR_DESCENTRATE       (10 * TIMER_COUNT_PER_SECOND)
+#define WTG_FOR_DESCENTRATE_COUNT_LIMIT   10
+#define TIMEOUT_AT_DESCENTRATE            10
+#define TIMEOUT_GROUNDEFFECT              (1 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTDOWN                (2 * TIMER_COUNT_PER_SECOND)
+#define LANDING_PID_SCALAR_P              2.0f
+#define LANDING_PID_SCALAR_I              10.0f
+#define LANDING_SLOWDOWN_HEIGHT           -5.0f
+#define BOUNCE_VELOCITY_TRIGGER_LIMIT     -0.3f
+#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -6.0f
+#define BOUNCE_TRIGGER_COUNT              4
+#define GROUNDEFFECT_SLOWDOWN_FACTOR      0.3f
+#define GROUNDEFFECT_SLOWDOWN_COUNT       4
+
+VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
+    [LAND_STATE_INACTIVE]       =       { .setup = &VtolLandFSM::setup_inactive,             .run = 0                                      },
+    [LAND_STATE_INIT_ALTHOLD]   =       { .setup = &VtolLandFSM::setup_init_althold,         .run = &VtolLandFSM::run_init_althold         },
+    [LAND_STATE_WTG_FOR_DESCENTRATE] =  { .setup = &VtolLandFSM::setup_wtg_for_descentrate,  .run = &VtolLandFSM::run_wtg_for_descentrate  },
+    [LAND_STATE_AT_DESCENTRATE] =       { .setup = &VtolLandFSM::setup_at_descentrate,       .run = &VtolLandFSM::run_at_descentrate       },
+    [LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
+    [LAND_STATE_GROUNDEFFECT]   =       { .setup = &VtolLandFSM::setup_groundeffect,         .run = &VtolLandFSM::run_groundeffect         },
+    [LAND_STATE_THRUSTDOWN]     =       { .setup = &VtolLandFSM::setup_thrustdown,           .run = &VtolLandFSM::run_thrustdown           },
+    [LAND_STATE_THRUSTOFF]      =       { .setup = &VtolLandFSM::setup_thrustoff,            .run = &VtolLandFSM::run_thrustoff            },
+    [LAND_STATE_DISARMED]       =       { .setup = &VtolLandFSM::setup_disarmed,             .run = &VtolLandFSM::run_disarmed             },
+    [LAND_STATE_ABORT] =                { .setup = &VtolLandFSM::setup_abort,                .run = &VtolLandFSM::run_abort                }
+};
+
+// pointer to a singleton instance
+VtolLandFSM *VtolLandFSM::p_inst = 0;
+
+
+VtolLandFSM::VtolLandFSM()
+    : mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
+{}
+
+// Private types
+
+// Private functions
+// Public API methods
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
+                                PathDesiredData *ptr_pathDesired,
+                                FlightStatusData *ptr_flightStatus)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_flightStatus);
+
+    if (mLandData == 0) {
+        mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
+        PIOS_Assert(mLandData);
+    }
+    memset(mLandData, sizeof(VtolLandFSMData_T), 0);
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    pathDesired  = ptr_pathDesired;
+    flightStatus = ptr_flightStatus;
+    initFSM();
+
+    return 0;
+}
+
+void VtolLandFSM::Inactive(void)
+{
+    memset(mLandData, sizeof(VtolLandFSMData_T), 0);
+    initFSM();
+}
+
+// Initialise the FSM
+void VtolLandFSM::initFSM(void)
+{
+    if (vtolPathFollowerSettings != 0) {
+        setState(LAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    } else {
+        mLandData->currentState = LAND_STATE_INACTIVE;
+    }
+}
+
+void VtolLandFSM::Activate()
+{
+    memset(mLandData, sizeof(VtolLandFSMData_T), 0);
+    mLandData->currentState   = LAND_STATE_INACTIVE;
+    mLandData->flLowAltitude  = false;
+    mLandData->flAltitudeHold = false;
+    mLandData->fsmLandStatus.averageDescentRate      = MIN_LANDRATE;
+    mLandData->fsmLandStatus.averageDescentThrust    = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+    TakeOffLocationGet(&(mLandData->takeOffLocation));
+    mLandData->fsmLandStatus.AltitudeAtState[LAND_STATE_INACTIVE] = 0.0f;
+    assessAltitude();
+
+    if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
+#ifndef DEBUG_GROUNDIMPACT
+        setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
+#else
+        setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+#endif
+    } else {
+        // move to error state and callback to position hold
+        setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    }
+}
+
+void VtolLandFSM::Abort(void)
+{
+    setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+}
+
+PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
+{
+    switch (mLandData->currentState) {
+    case LAND_STATE_INACTIVE:
+        return PFFSM_STATE_INACTIVE;
+
+        break;
+    case LAND_STATE_ABORT:
+        return PFFSM_STATE_ABORT;
+
+        break;
+    case LAND_STATE_DISARMED:
+        return PFFSM_STATE_DISARMED;
+
+        break;
+    default:
+        return PFFSM_STATE_ACTIVE;
+
+        break;
+    }
+}
+
+void VtolLandFSM::Update()
+{
+    runState();
+    if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
+        runAlways();
+    }
+}
+
+int32_t VtolLandFSM::runState(void)
+{
+    uint8_t flTimeout = false;
+
+    mLandData->stateRunCount++;
+
+    if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
+        flTimeout = true;
+    }
+
+    // If the current state has a static function, call it
+    if (sLandStateTable[mLandData->currentState].run) {
+        (this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
+    }
+    return 0;
+}
+
+int32_t VtolLandFSM::runAlways(void)
+{
+    void assessAltitude(void);
+
+    return 0;
+}
+
+// PathFollower implements the PID scheme and has a objective
+// set by a PathDesired object.  Based on the mode, pathfollower
+// uses FSM's as helper functions that manage state and event detection.
+// PathFollower calls into FSM methods to alter its commands.
+
+void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
+{
+    ulow  = mLandData->boundThrustMin;
+    uhigh = mLandData->boundThrustMax;
+
+
+    if (mLandData->flConstrainThrust) {
+        uhigh = mLandData->thrustLimit;
+    }
+}
+
+void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
+{
+    if (mLandData->flZeroStabiHorizontal && stabDesired) {
+        stabDesired->Pitch = 0.0f;
+        stabDesired->Roll  = 0.0f;
+        stabDesired->Yaw   = 0.0f;
+    }
+}
+
+void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
+{
+    if (mLandData->flLowAltitude) {
+        local_scaler->p = LANDING_PID_SCALAR_P;
+        local_scaler->i = LANDING_PID_SCALAR_I;
+    }
+}
+
+
+// Set the new state and perform setup for subsequent state run calls
+// This is called by state run functions on event detection that drive
+// state transitions.
+void VtolLandFSM::setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason)
+{
+    mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
+
+    if (mLandData->currentState == newState) {
+        return;
+    }
+    mLandData->currentState = newState;
+
+    if (newState != LAND_STATE_INACTIVE) {
+        PositionStateData positionState;
+        PositionStateGet(&positionState);
+        float takeOffDown = 0.0f;
+        if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+            takeOffDown = mLandData->takeOffLocation.Down;
+        }
+        mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
+        assessAltitude();
+    }
+
+    // Restart state timer counter
+    mLandData->stateRunCount     = 0;
+
+    // Reset state timeout to disabled/zero
+    mLandData->stateTimeoutCount = 0;
+
+    if (sLandStateTable[mLandData->currentState].setup) {
+        (this->*sLandStateTable[mLandData->currentState].setup)();
+    }
+
+    updateVtolLandFSMStatus();
+}
+
+
+// Timeout utility function for use by state init implementations
+void VtolLandFSM::setStateTimeout(int32_t count)
+{
+    mLandData->stateTimeoutCount = count;
+}
+
+void VtolLandFSM::updateVtolLandFSMStatus()
+{
+    mLandData->fsmLandStatus.State = mLandData->currentState;
+    if (mLandData->flLowAltitude) {
+        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
+    } else {
+        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
+    }
+    StatusVtolLandSet(&mLandData->fsmLandStatus);
+}
+
+
+float VtolLandFSM::BoundVelocityDown(float velocity_down)
+{
+    velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
+    if (mLandData->flLowAltitude) {
+        velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
+    }
+    mLandData->fsmLandStatus.targetDescentRate = velocity_down;
+
+    if (mLandData->flAltitudeHold) {
+        return 0.0f;
+    } else {
+        return velocity_down;
+    }
+}
+
+void VtolLandFSM::assessAltitude(void)
+{
+    float positionDown;
+
+    PositionStateDownGet(&positionDown);
+    float takeOffDown = 0.0f;
+    if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+        takeOffDown = mLandData->takeOffLocation.Down;
+    }
+    float positionDownRelativeToTakeoff = positionDown - takeOffDown;
+    if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
+        mLandData->flLowAltitude = false;
+    } else {
+        mLandData->flLowAltitude = true;
+    }
+}
+
+
+// FSM Setup and Run method implementation
+
+// State: INACTIVE
+void VtolLandFSM::setup_inactive(void)
+{
+    // Re-initialise local variables
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flConstrainThrust     = false;
+}
+
+// State: INIT ALTHOLD
+void VtolLandFSM::setup_init_althold(void)
+{
+    setStateTimeout(TIMEOUT_INIT_ALTHOLD);
+    // get target descent velocity
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
+    mLandData->flConstrainThrust     = false;
+    mLandData->flAltitudeHold = true;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_init_althold(uint8_t flTimeout)
+{
+    if (flTimeout) {
+        mLandData->flAltitudeHold = false;
+        setState(LAND_STATE_WTG_FOR_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+
+// State: WAITING FOR DESCENT RATE
+void VtolLandFSM::setup_wtg_for_descentrate(void)
+{
+    setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
+    // get target descent velocity
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount = 0;
+    mLandData->observation2Count     = 0;
+    mLandData->flConstrainThrust     = false;
+    mLandData->flAltitudeHold = false;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
+{
+    // Look at current actual thrust...are we already shutdown??
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    // We don't expect PID to get exactly the target descent rate, so have a lower
+    // water mark but need to see 5 observations to be confident that we have semi-stable
+    // descent achieved
+
+    // we need to see velocity down within a range of control before we proceed, without which we
+    // really don't have confidence to allow later states to run.
+    if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
+        velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
+        if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
+            setState(LAND_STATE_AT_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
+            return;
+        }
+    }
+
+    if (flTimeout) {
+        setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+
+// State: AT DESCENT RATE
+void VtolLandFSM::setup_at_descentrate(void)
+{
+    setStateTimeout(TIMEOUT_AT_DESCENTRATE);
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount  = 0;
+    mLandData->sum1 = 0.0f;
+    mLandData->sum2 = 0.0f;
+    mLandData->flConstrainThrust = false;
+    mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
+    mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->boundThrustMin    = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax    = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    mLandData->sum1 += velocityState.Down;
+    mLandData->sum2 += stabDesired.Thrust;
+    mLandData->observationCount++;
+    if (flTimeout) {
+        mLandData->fsmLandStatus.averageDescentRate   = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
+        mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+        // We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
+        // As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
+        // detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
+        mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
+        mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
+
+
+        setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
+    }
+}
+
+
+// State: WAITING FOR GROUND EFFECT
+void VtolLandFSM::setup_wtg_for_groundeffect(void)
+{
+    // No timeout
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount = 0;
+    mLandData->observation2Count     = 0;
+    mLandData->sum1 = 0.0f;
+    mLandData->sum2 = 0.0f;
+    mLandData->flConstrainThrust     = false;
+    mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
+    mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel    = 0.0f;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+}
+
+void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
+{
+    // detect material downrating in thrust for 1 second.
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    AccelStateData accelState;
+    AccelStateGet(&accelState);
+
+    // +ve 9.8 expected
+    float g_e;
+    HomeLocationg_eGet(&g_e);
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+
+    // detect bounce
+    uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
+    if (flBounce) {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
+    } else {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
+    }
+
+    // invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
+    // a relative acceleration term.
+    float bounceAccel     = -accelState.z - g_e;
+    uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
+    if (flBounceAccel) {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
+    } else {
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
+    }
+
+    if (flBounce || flBounceAccel) {
+        mLandData->observation2Count++;
+        if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
+            setState(LAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
+            return;
+        }
+    } else {
+        mLandData->observation2Count = 0;
+    }
+
+    // detect low descent rate
+    uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
+    if (flDescentRateLow) {
+        mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
+        mLandData->observationCount++;
+        if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
+#ifndef DEBUG_GROUNDIMPACT
+            setState(LAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
+#endif
+            return;
+        }
+    } else {
+        mLandData->observationCount = 0;
+    }
+
+    updateVtolLandFSMStatus();
+}
+
+// STATE: GROUNDEFFET
+void VtolLandFSM::setup_groundeffect(void)
+{
+    setStateTimeout(TIMEOUT_GROUNDEFFECT);
+    mLandData->flZeroStabiHorizontal     = true;
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    mLandData->expectedLandPositionNorth = positionState.North;
+    mLandData->expectedLandPositionEast  = positionState.East;
+    mLandData->flConstrainThrust = false;
+
+    // now that we have ground effect limit max thrust to neutral
+    mLandData->boundThrustMin    = -0.1f;
+    mLandData->boundThrustMax    = mLandData->fsmLandStatus.calculatedNeutralThrust;
+}
+void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
+{
+    StabilizationDesiredData stabDesired;
+
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust < 0.0f) {
+        setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
+        return;
+    }
+
+    // Stay in this state until we get a low altitude flag.
+    if (mLandData->flLowAltitude == false) {
+        // worst case scenario is that we land and the pid brings thrust down to zero.
+        return;
+    }
+
+    // detect broad sideways drift.  If for some reason we have a hard landing that the bounce detection misses, this will kick in
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    float north_error   = mLandData->expectedLandPositionNorth - positionState.North;
+    float east_error    = mLandData->expectedLandPositionEast - positionState.East;
+    float positionError = sqrtf(north_error * north_error + east_error * east_error);
+    if (positionError > 0.3f) {
+        setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
+        return;
+    }
+
+    if (flTimeout) {
+        setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+// STATE: THRUSTDOWN
+void VtolLandFSM::setup_thrustdown(void)
+{
+    setStateTimeout(TIMEOUT_THRUSTDOWN);
+    mLandData->flZeroStabiHorizontal = true;
+    mLandData->flConstrainThrust     = true;
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    mLandData->thrustLimit    = stabDesired.Thrust;
+    mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
+    mLandData->boundThrustMin = -0.1f;
+    mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
+}
+
+void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
+{
+    // reduce thrust setpoint step by step
+    mLandData->thrustLimit -= mLandData->sum1;
+
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
+        setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
+    }
+
+    if (flTimeout) {
+        setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+    }
+}
+
+// STATE: THRUSTOFF
+void VtolLandFSM::setup_thrustoff(void)
+{
+    mLandData->thrustLimit       = -1.0f;
+    mLandData->flConstrainThrust = true;
+    mLandData->boundThrustMin    = -0.1f;
+    mLandData->boundThrustMax    = 0.0f;
+}
+
+void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
+{
+    setState(LAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
+}
+
+// STATE: DISARMED
+void VtolLandFSM::setup_disarmed(void)
+{
+    // nothing to do
+    mLandData->flConstrainThrust     = false;
+    mLandData->flZeroStabiHorizontal = false;
+    mLandData->observationCount = 0;
+    mLandData->boundThrustMin   = -0.1f;
+    mLandData->boundThrustMax   = 0.0f;
+}
+
+void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
+{
+#ifdef DEBUG_GROUNDIMPACT
+    if (mLandData->observationCount++ > 100) {
+        setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    }
+#endif
+}
+
+void VtolLandFSM::fallback_to_hold(void)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    pathDesired->End.North        = positionState.North;
+    pathDesired->End.East         = positionState.East;
+    pathDesired->End.Down         = positionState.Down;
+    pathDesired->Start.North      = positionState.North;
+    pathDesired->Start.East       = positionState.East;
+    pathDesired->Start.Down       = positionState.Down;
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+
+    PathDesiredSet(pathDesired);
+}
+
+// abort repeatedly overwrites pathfollower's objective on a landing abort and
+// continues to do so until a flight mode change.
+void VtolLandFSM::setup_abort(void)
+{
+    mLandData->boundThrustMin        = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMax        = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->flConstrainThrust     = false;
+    mLandData->flZeroStabiHorizontal = false;
+    fallback_to_hold();
+}
+
+void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
+{}

flight/modules/PathFollower/vtolvelocitycontroller.cpp

@@ -0,0 +1,227 @@
+/*
+ ******************************************************************************
+ *
+ * @file       VtolVelocityController.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Velocity roam controller for vtols
+ * @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
+ */
+
+extern "C" {
+#include <openpilot.h>
+
+#include <callbackinfo.h>
+
+#include <math.h>
+#include <pid.h>
+#include <CoordinateConversions.h>
+#include <sin_lookup.h>
+#include <pathdesired.h>
+#include <paths.h>
+#include "plans.h"
+#include <sanitycheck.h>
+
+#include <vtolpathfollowersettings.h>
+#include <flightmodesettings.h>
+#include <pathstatus.h>
+#include <positionstate.h>
+#include <velocitystate.h>
+#include <velocitydesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <flightstatus.h>
+#include <manualcontrolcommand.h>
+#include <systemsettings.h>
+#include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <pathsummary.h>
+}
+
+// C++ includes
+#include "vtolvelocitycontroller.h"
+#include "pidcontrolne.h"
+
+// Private constants
+
+// pointer to a singleton instance
+VtolVelocityController *VtolVelocityController::p_inst = 0;
+
+VtolVelocityController::VtolVelocityController()
+    : vtolPathFollowerSettings(0), mActive(false)
+{}
+
+// Called when mode first engaged
+void VtolVelocityController::Activate(void)
+{
+    if (!mActive) {
+        mActive = true;
+        SettingsUpdated();
+        controlNE.Activate();
+    }
+}
+
+uint8_t VtolVelocityController::IsActive(void)
+{
+    return mActive;
+}
+
+uint8_t VtolVelocityController::Mode(void)
+{
+    return PATHDESIRED_MODE_VELOCITY;
+}
+
+void VtolVelocityController::ObjectiveUpdated(void)
+{
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+}
+
+void VtolVelocityController::Deactivate(void)
+{
+    if (mActive) {
+        mActive = false;
+        controlNE.Deactivate();
+    }
+}
+
+void VtolVelocityController::SettingsUpdated(void)
+{
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               dT,
+                               vtolPathFollowerSettings->HorizontalVelMax);
+
+
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+}
+
+int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+{
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
+
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    return 0;
+}
+
+
+void VtolVelocityController::UpdateVelocityDesired()
+{
+    VelocityStateData velocityState;
+
+    VelocityStateGet(&velocityState);
+    VelocityDesiredData velocityDesired;
+
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+
+    velocityDesired.Down  = 0.0f;
+    float north, east;
+    controlNE.GetVelocityDesired(&north, &east);
+    velocityDesired.North = north;
+    velocityDesired.East  = east;
+
+    // update pathstatus
+    pathStatus->error     = 0.0f;
+    pathStatus->fractional_progress  = 0.0f;
+    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_down  = velocityDesired.Down;
+
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+    pathStatus->correction_direction_down  = 0.0f;
+
+    VelocityDesiredSet(&velocityDesired);
+}
+
+int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)) bool yaw_attitude, __attribute__((unused)) float yaw_direction)
+{
+    uint8_t result = 1;
+    StabilizationDesiredData stabDesired;
+    AttitudeStateData attitudeState;
+    StabilizationBankData stabSettings;
+    float northCommand;
+    float eastCommand;
+
+    StabilizationDesiredGet(&stabDesired);
+    AttitudeStateGet(&attitudeState);
+    StabilizationBankGet(&stabSettings);
+
+    controlNE.GetNECommand(&northCommand, &eastCommand);
+
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float cos_angle     = cosf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+
+    ManualControlCommandData manualControl;
+    ManualControlCommandGet(&manualControl);
+
+    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+
+    // default thrust mode to altvario
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
+
+    StabilizationDesiredSet(&stabDesired);
+
+    return result;
+}
+
+void VtolVelocityController::UpdateAutoPilot()
+{
+    UpdateVelocityDesired();
+
+    bool yaw_attitude = false;
+    float yaw = 0.0f;
+
+    int8_t result     = UpdateStabilizationDesired(yaw_attitude, yaw);
+
+    if (!result) {
+        fallback_to_hold();
+    }
+
+    PathStatusSet(pathStatus);
+}
+
+void VtolVelocityController::fallback_to_hold(void)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    pathDesired->End.North        = positionState.North;
+    pathDesired->End.East         = positionState.East;
+    pathDesired->End.Down         = positionState.Down;
+    pathDesired->Start.North      = positionState.North;
+    pathDesired->Start.East       = positionState.East;
+    pathDesired->Start.Down       = positionState.Down;
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+
+    PathDesiredSet(pathDesired);
+}

flight/modules/PathPlanner/pathplanner.c

@@ -43,8 +43,11 @@
 #include "waypoint.h"
 #include "waypointactive.h"
 #include "flightmodesettings.h"
+#include <systemsettings.h>
 #include "paths.h"
 #include "plans.h"
+#include <sanitycheck.h>
+#include <vtolpathfollowersettings.h>
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -73,6 +76,7 @@ static uint8_t conditionAboveSpeed();
 static uint8_t conditionPointingTowardsNext();
 static uint8_t conditionPythonScript();
 static uint8_t conditionImmediate();
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
 
 
 // Private variables
@@ -82,7 +86,10 @@ static WaypointActiveData waypointActive;
 static WaypointData waypoint;
 static PathActionData pathAction;
 static bool pathplanner_active = false;
+static FrameType_t frameType;
+static bool mode3D;
 
+extern FrameType_t GetCurrentFrameType();
 
 /**
  * Module initialization
@@ -95,6 +102,9 @@ int32_t PathPlannerStart()
     WaypointActiveConnectCallback(commandUpdated);
     PathActionConnectCallback(commandUpdated);
     PathStatusConnectCallback(statusUpdated);
+    SettingsUpdatedCb(NULL);
+    SystemSettingsConnectCallback(&SettingsUpdatedCb);
+    VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 
     // Start main task callback
     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
@@ -125,6 +135,34 @@ int32_t PathPlannerInitialize()
 
 MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
 
+
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    uint8_t TreatCustomCraftAs;
+
+    VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
+
+    frameType = GetCurrentFrameType();
+
+    if (frameType == FRAME_TYPE_CUSTOM) {
+        switch (TreatCustomCraftAs) {
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
+            frameType = FRAME_TYPE_FIXED_WING;
+            mode3D    = true;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
+            frameType = FRAME_TYPE_MULTIROTOR;
+            mode3D    = true;
+            break;
+        case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
+            frameType = FRAME_TYPE_GROUND;
+            mode3D    = false;
+            break;
+        }
+    }
+}
+
+
 /**
  * Module task
  */
@@ -160,22 +198,17 @@ static void pathPlannerTask()
 
     static uint8_t failsafeRTHset = 0;
     if (!validPathPlan) {
-        // At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
-        // Failsafe: behave as if in return to base mode
-        // what to do in this case is debatable, it might be better to just
-        // make a forced disarming but rth has a higher chance of survival when
-        // in flight.
         pathplanner_active = false;
 
         if (!failsafeRTHset) {
             failsafeRTHset = 1;
-            // copy pasta: same calculation as in manualcontrol, set return to home coordinates
             plan_setup_positionHold();
         }
         AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
 
         return;
     }
+
     failsafeRTHset = 0;
     AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
 
@@ -525,7 +558,7 @@ static uint8_t conditionLegRemaining()
     struct path_status progress;
 
     path_progress(&pathDesired,
-                  cur, &progress);
+                  cur, &progress, mode3D);
     if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
         return true;
     }
@@ -549,7 +582,7 @@ static uint8_t conditionBelowError()
     struct path_status progress;
 
     path_progress(&pathDesired,
-                  cur, &progress);
+                  cur, &progress, mode3D);
     if (progress.error <= pathAction.ConditionParameters[0]) {
         return true;
     }

flight/modules/RadioComBridge/RadioComBridge.c

@@ -108,8 +108,8 @@ static void radioRxTask(void *parameters);
 static void PPMInputTask(void *parameters);
 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
 static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
-static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
+static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
-static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
+static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
 static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
 static void registerObject(UAVObjHandle obj);
 
@@ -403,14 +403,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
         PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX);
 #endif
         if (PIOS_COM_RADIO) {
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
             if (bytes_to_process > 0) {
                 if (data->parseUAVTalk) {
                     // Pass the data through the UAVTalk parser.
-                    for (uint8_t i = 0; i < bytes_to_process; i++) {
+                    ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process);
-                        ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]);
-                    }
                 } else if (PIOS_COM_TELEMETRY) {
                     // Send the data straight to the telemetry port.
                     // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
@@ -448,12 +446,10 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
         }
 #endif /* PIOS_INCLUDE_USB */
         if (inputPort) {
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
             if (bytes_to_process > 0) {
-                for (uint8_t i = 0; i < bytes_to_process; i++) {
+                ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data, bytes_to_process);
-                    ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]);
-                }
             }
         } else {
             vTaskDelay(5);
@@ -597,42 +593,45 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
  * @param[in] outConnectionHandle  The UAVTalk connection handle on the radio port.
  * @param[in] rxbyte  The received byte.
  */
-static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
+static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
 {
-    // Keep reading until we receive a completed packet.
+    uint8_t position = 0;
-    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_COMPLETE) {
+    // Keep reading until we receive a completed packet.
-        // We only want to unpack certain telemetry objects
+    while (position < length) {
-        uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
+        UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
-        switch (objId) {
+        if (state == UAVTALK_STATE_COMPLETE) {
-        case OPLINKSTATUS_OBJID:
+            // We only want to unpack certain telemetry objects
-        case OPLINKSETTINGS_OBJID:
+            uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
-        case OPLINKRECEIVER_OBJID:
+            switch (objId) {
-        case MetaObjectId(OPLINKSTATUS_OBJID):
+            case OPLINKSTATUS_OBJID:
-        case MetaObjectId(OPLINKSETTINGS_OBJID):
+            case OPLINKSETTINGS_OBJID:
-        case MetaObjectId(OPLINKRECEIVER_OBJID):
+            case OPLINKRECEIVER_OBJID:
-            UAVTalkReceiveObject(inConnectionHandle);
+            case MetaObjectId(OPLINKSTATUS_OBJID):
-            break;
+            case MetaObjectId(OPLINKSETTINGS_OBJID):
-        case OBJECTPERSISTENCE_OBJID:
+            case MetaObjectId(OPLINKRECEIVER_OBJID):
-        case MetaObjectId(OBJECTPERSISTENCE_OBJID):
+                UAVTalkReceiveObject(inConnectionHandle);
-            // receive object locally
+                break;
-            // some objects will send back a response to telemetry
+            case OBJECTPERSISTENCE_OBJID:
-            // FIXME:
+            case MetaObjectId(OBJECTPERSISTENCE_OBJID):
-            // OPLM will ack or nack all objects requests and acked object sends
+                // receive object locally
-            // Receiver will probably also ack / nack the same messages
+                // some objects will send back a response to telemetry
-            // This has some consequences like :
+                // FIXME:
-            // Second ack/nack will not match an open transaction or will apply to wrong transaction
+                // OPLM will ack or nack all objects requests and acked object sends
-            // Question : how does GCS handle receiving the same object twice
+                // Receiver will probably also ack / nack the same messages
-            // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
+                // This has some consequences like :
-            UAVTalkReceiveObject(inConnectionHandle);
+                // Second ack/nack will not match an open transaction or will apply to wrong transaction
-            // relay packet to remote modem
+                // Question : how does GCS handle receiving the same object twice
-            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
-            break;
+                UAVTalkReceiveObject(inConnectionHandle);
-        default:
+                // relay packet to remote modem
-            // all other packets are relayed to the remote modem
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
-            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
-            break;
+            default:
+                // all other packets are relayed to the remote modem
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
+            }
         }
     }
 }
@@ -642,39 +641,43 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk
  *
  * @param[in] inConnectionHandle  The UAVTalk connection handle on the radio port.
  * @param[in] outConnectionHandle  The UAVTalk connection handle on the telemetry port.
- * @param[in] rxbyte  The received byte.
+ * @param[in] rxbuffer  The received buffer.
+ * @param[in] length  buffer length
  */
-static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
+static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
 {
-    // Keep reading until we receive a completed packet.
+    uint8_t position = 0;
-    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_COMPLETE) {
+    // Keep reading until we receive a completed packet.
-        // We only want to unpack certain objects from the remote modem
+    while (position < length) {
-        // Similarly we only want to relay certain objects to the telemetry port
+        UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
-        uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
+        if (state == UAVTALK_STATE_COMPLETE) {
-        switch (objId) {
+            // We only want to unpack certain objects from the remote modem
-        case OPLINKSTATUS_OBJID:
+            // Similarly we only want to relay certain objects to the telemetry port
-        case OPLINKSETTINGS_OBJID:
+            uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
-        case MetaObjectId(OPLINKSTATUS_OBJID):
+            switch (objId) {
-        case MetaObjectId(OPLINKSETTINGS_OBJID):
+            case OPLINKSTATUS_OBJID:
-            // Ignore object...
+            case OPLINKSETTINGS_OBJID:
-            // These objects are shadowed by the modem and are not transmitted to the telemetry port
+            case MetaObjectId(OPLINKSTATUS_OBJID):
-            // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
+            case MetaObjectId(OPLINKSETTINGS_OBJID):
-            // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
+                // Ignore object...
-            break;
+                // These objects are shadowed by the modem and are not transmitted to the telemetry port
-        case OPLINKRECEIVER_OBJID:
+                // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
-        case MetaObjectId(OPLINKRECEIVER_OBJID):
+                // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
-            // Receive object locally
+                break;
-            // These objects are received by the modem and are not transmitted to the telemetry port
+            case OPLINKRECEIVER_OBJID:
-            // - OPLINKRECEIVER_OBJID : not sure why
+            case MetaObjectId(OPLINKRECEIVER_OBJID):
-            // some objects will send back a response to the remote modem
+                // Receive object locally
-            UAVTalkReceiveObject(inConnectionHandle);
+                // These objects are received by the modem and are not transmitted to the telemetry port
-            break;
+                // - OPLINKRECEIVER_OBJID : not sure why
-        default:
+                // some objects will send back a response to the remote modem
-            // all other packets are relayed to the telemetry port
+                UAVTalkReceiveObject(inConnectionHandle);
-            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
-            break;
+            default:
+                // all other packets are relayed to the telemetry port
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+                break;
+            }
         }
     }
 }

flight/modules/Sensors/sensors.c

@@ -411,9 +411,9 @@ static void handleAccel(float *samples, float temperature)
     AccelSensorData accelSensorData;
 
     updateAccelTempBias(temperature);
-    float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
+    float accels_out[3] = { (samples[0] - agcal.accel_bias.X) * agcal.accel_scale.X - accel_temp_bias[0],
-                            samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
+                            (samples[1] - agcal.accel_bias.Y) * agcal.accel_scale.Y - accel_temp_bias[1],
-                            samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
+                            (samples[2] - agcal.accel_bias.Z) * agcal.accel_scale.Z - accel_temp_bias[2] };
 
     rot_mult(R, accels_out, samples);
     accelSensorData.x = samples[0];
@@ -485,7 +485,10 @@ static void updateAccelTempBias(float temperature)
     if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
         accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
         if (accel_temp_calibrated) {
-            float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+            float ctemp = boundf(accel_temperature,
+                                 agcal.temp_calibrated_extent.max,
+                                 agcal.temp_calibrated_extent.min);
+
             accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
             accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
             accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
@@ -588,8 +591,12 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 
     RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
     RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
-    baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
+    baro_temp_correction_enabled =
-                                     (baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
+        (baroCorrectionExtent.max - baroCorrectionExtent.min > 0.1f &&
+         (fabsf(baroCorrection.a) > 1e-9f ||
+          fabsf(baroCorrection.b) > 1e-9f ||
+          fabsf(baroCorrection.c) > 1e-9f ||
+          fabsf(baroCorrection.d) > 1e-9f));
 }
 /**
  * @}

flight/modules/StateEstimation/stateestimation.c

@@ -345,7 +345,6 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
  */
 static void StateEstimationCb(void)
 {
-    static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
     static filterResult alarm     = FILTERRESULT_OK;
     static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
     static uint16_t alarmcounter  = 0;
@@ -361,193 +360,171 @@ static void StateEstimationCb(void)
         return;
     }
 
-    switch (runState) {
+    alarm = FILTERRESULT_OK;
-    case RUNSTATE_LOAD:
 
-        alarm = FILTERRESULT_OK;
+    // set alarm to warning if called through timeout
-
+    if (updatedSensors == 0) {
-        // set alarm to warning if called through timeout
+        if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
-        if (updatedSensors == 0) {
+            alarm = FILTERRESULT_WARNING;
-            if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
-                alarm = FILTERRESULT_WARNING;
-            }
-        } else {
-            last_time = PIOS_DELAY_GetRaw();
         }
+    } else {
+        last_time = PIOS_DELAY_GetRaw();
+    }
 
-        // check if a new filter chain should be initialized
+    // check if a new filter chain should be initialized
-        if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
+    if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
-            FlightStatusData fs;
+        FlightStatusData fs;
-            FlightStatusGet(&fs);
+        FlightStatusGet(&fs);
-            if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
+        if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
-                const filterPipeline *newFilterChain;
+            const filterPipeline *newFilterChain;
-                switch (revoSettings.FusionAlgorithm) {
+            switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) {
-                case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
+            case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
-                    newFilterChain = cfQueue;
+                newFilterChain = cfQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
-                    newFilterChain = cfmiQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
-                    newFilterChain = cfmQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
-                    newFilterChain = ekf13iQueue;
-                    break;
-                case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
-                    newFilterChain = ekf13Queue;
-                    break;
-                default:
-                    newFilterChain = NULL;
-                }
-                // initialize filters in chain
-                current = newFilterChain;
-                bool error = 0;
-                while (current != NULL) {
-                    int32_t result = current->filter->init((stateFilter *)current->filter);
-                    if (result != 0) {
-                        error = 1;
-                        break;
-                    }
-                    current = current->next;
-                }
-                if (error) {
-                    AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
-                    return;
-                } else {
-                    // set new fusion algortithm
-                    filterChain     = newFilterChain;
-                    fusionAlgorithm = revoSettings.FusionAlgorithm;
-                }
-            }
-        }
-
-        // read updated sensor UAVObjects and set initial state
-        states.updated = updatedSensors;
-        updatedSensors = 0;
-
-        // fetch sensors, check values, and load into state struct
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
-        if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-            gyroRaw[0] = states.gyro[0];
-            gyroRaw[1] = states.gyro[1];
-            gyroRaw[2] = states.gyro[2];
-        }
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
-
-        // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
-
-        // at this point sensor state is stored in "states" with some rudimentary filtering applied
-
-        // apply all filters in the current filter chain
-        current  = filterChain;
-
-        // we are not done, re-dispatch self execution
-        runState = RUNSTATE_FILTER;
-        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
-        break;
-
-    case RUNSTATE_FILTER:
-
-        if (current != NULL) {
-            filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
-            if (result > alarm) {
-                alarm = result;
-            }
-            current = current->next;
-        }
-
-        // we are not done, re-dispatch self execution
-        if (!current) {
-            runState = RUNSTATE_SAVE;
-        }
-        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
-        break;
-
-    case RUNSTATE_SAVE:
-
-        // the final output of filters is saved in state variables
-        // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
-        if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-            gyroDelta[0] = states.gyro[0] - gyroRaw[0];
-            gyroDelta[1] = states.gyro[1] - gyroRaw[1];
-            gyroDelta[2] = states.gyro[2] - gyroRaw[2];
-        }
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
-        if (IS_SET(states.updated, SENSORUPDATES_mag)) {
-            MagStateData s;
-
-            MagStateGet(&s);
-            s.x = states.mag[0];
-            s.y = states.mag[1];
-            s.z = states.mag[2];
-            switch (states.magStatus) {
-            case MAGSTATUS_OK:
-                s.Source = MAGSTATE_SOURCE_ONBOARD;
                 break;
-            case MAGSTATUS_AUX:
+            case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
-                s.Source = MAGSTATE_SOURCE_AUX;
+                newFilterChain = cfmiQueue;
+                break;
+            case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
+                newFilterChain = cfmQueue;
+                break;
+            case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
+                newFilterChain = ekf13iQueue;
+                break;
+            case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
+                newFilterChain = ekf13Queue;
                 break;
             default:
-                s.Source = MAGSTATE_SOURCE_INVALID;
+                newFilterChain = NULL;
+            }
+            // initialize filters in chain
+            current = newFilterChain;
+            bool error = 0;
+            while (current != NULL) {
+                int32_t result = current->filter->init((stateFilter *)current->filter);
+                if (result != 0) {
+                    error = 1;
+                    break;
+                }
+                current = current->next;
+            }
+            if (error) {
+                AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
+                return;
+            } else {
+                // set new fusion algortithm
+                filterChain     = newFilterChain;
+                fusionAlgorithm = revoSettings.FusionAlgorithm;
             }
-            MagStateSet(&s);
         }
+    }
 
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
+    // read updated sensor UAVObjects and set initial state
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
+    states.updated = updatedSensors;
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
+    updatedSensors = 0;
-        // attitude nees manual conversion from quaternion to euler
+
-        if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
+    // fetch sensors, check values, and load into state struct
-            AttitudeStateData s;
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
-            AttitudeStateGet(&s);
+    if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-            s.q1 = states.attitude[0];
+        gyroRaw[0] = states.gyro[0];
-            s.q2 = states.attitude[1];
+        gyroRaw[1] = states.gyro[1];
-            s.q3 = states.attitude[2];
+        gyroRaw[2] = states.gyro[2];
-            s.q4 = states.attitude[3];
+    }
-            Quaternion2RPY(&s.q1, &s.Roll);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
-            AttitudeStateSet(&s);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
+    FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
+
+    // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
+
+    // at this point sensor state is stored in "states" with some rudimentary filtering applied
+
+    // apply all filters in the current filter chain
+    current = filterChain;
+
+    // we are not done, re-dispatch self execution
+
+    while (current) {
+        filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
+        if (result > alarm) {
+            alarm = result;
         }
+        current = current->next;
+    }
 
-        // throttle alarms, raise alarm flags immediately
+    // the final output of filters is saved in state variables
-        // but require system to run for a while before decreasing
+    // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
-        // to prevent alarm flapping
+    if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
-        if (alarm >= lastAlarm) {
+        gyroDelta[0] = states.gyro[0] - gyroRaw[0];
+        gyroDelta[1] = states.gyro[1] - gyroRaw[1];
+        gyroDelta[2] = states.gyro[2] - gyroRaw[2];
+    }
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
+    if (IS_SET(states.updated, SENSORUPDATES_mag)) {
+        MagStateData s;
+
+        MagStateGet(&s);
+        s.x = states.mag[0];
+        s.y = states.mag[1];
+        s.z = states.mag[2];
+        switch (states.magStatus) {
+        case MAGSTATUS_OK:
+            s.Source = MAGSTATE_SOURCE_ONBOARD;
+            break;
+        case MAGSTATUS_AUX:
+            s.Source = MAGSTATE_SOURCE_AUX;
+            break;
+        default:
+            s.Source = MAGSTATE_SOURCE_INVALID;
+        }
+        MagStateSet(&s);
+    }
+
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
+    EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
+    // attitude nees manual conversion from quaternion to euler
+    if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
+        AttitudeStateData s;
+        AttitudeStateGet(&s);
+        s.q1 = states.attitude[0];
+        s.q2 = states.attitude[1];
+        s.q3 = states.attitude[2];
+        s.q4 = states.attitude[3];
+        Quaternion2RPY(&s.q1, &s.Roll);
+        AttitudeStateSet(&s);
+    }
+    // throttle alarms, raise alarm flags immediately
+    // but require system to run for a while before decreasing
+    // to prevent alarm flapping
+    if (alarm >= lastAlarm) {
+        lastAlarm    = alarm;
+        alarmcounter = 0;
+    } else {
+        if (alarmcounter < 100) {
+            alarmcounter++;
+        } else {
             lastAlarm    = alarm;
             alarmcounter = 0;
-        } else {
-            if (alarmcounter < 100) {
-                alarmcounter++;
-            } else {
-                lastAlarm    = alarm;
-                alarmcounter = 0;
-            }
         }
+    }
 
-        // clear alarms if everything is alright, then schedule callback execution after timeout
+    // clear alarms if everything is alright, then schedule callback execution after timeout
-        if (lastAlarm == FILTERRESULT_WARNING) {
+    if (lastAlarm == FILTERRESULT_WARNING) {
-            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
-        } else if (lastAlarm == FILTERRESULT_CRITICAL) {
+    } else if (lastAlarm == FILTERRESULT_CRITICAL) {
-            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
-        } else if (lastAlarm >= FILTERRESULT_ERROR) {
+    } else if (lastAlarm >= FILTERRESULT_ERROR) {
-            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
-        } else {
+    } else {
-            AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+        AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
-        }
+    }
 
-        // we are done, re-schedule next self execution
+    if (updatedSensors) {
-        runState = RUNSTATE_LOAD;
+        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
-        if (updatedSensors) {
+    } else {
-            PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
+        PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
-        } else {
-            PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
-        }
-        break;
     }
 }
 

flight/modules/Telemetry/telemetry.c

@@ -130,8 +130,10 @@ int32_t TelemetryStart(void)
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
 
 #ifdef PIOS_INCLUDE_RFM22B
-    xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
+    if (radioPort) {
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
+        xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
+        PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
+    }
 #endif
 
     return 0;
@@ -432,14 +434,12 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
 
         if (inputPort) {
             // Block until data are available
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process;
 
             bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
             if (bytes_to_process > 0) {
-                for (uint8_t i = 0; i < bytes_to_process; i++) {
+                UAVTalkProcessInputStream(uavTalkCon, serial_data, bytes_to_process);
-                    UAVTalkProcessInputStream(uavTalkCon, serial_data[i]);
-                }
             }
         } else {
             vTaskDelay(5);
@@ -457,14 +457,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
     while (1) {
         if (radioPort) {
             // Block until data are available
-            uint8_t serial_data[1];
+            uint8_t serial_data[16];
             uint16_t bytes_to_process;
 
             bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
             if (bytes_to_process > 0) {
-                for (uint8_t i = 0; i < bytes_to_process; i++) {
+                UAVTalkProcessInputStream(radioUavTalkCon, serial_data, bytes_to_process);
-                    UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
-                }
             }
         } else {
             vTaskDelay(5);

flight/pios/common/pios_ms5611.c

@@ -42,7 +42,9 @@
 #ifndef PIOS_MS5611_SLOW_TEMP_RATE
 #define PIOS_MS5611_SLOW_TEMP_RATE 1
 #endif
-
+// Running moving average smoothing factor
+#define PIOS_MS5611_TEMP_SMOOTHING 10
+//
 /* Local Types */
 typedef struct {
     uint16_t C[6];
@@ -72,6 +74,7 @@ static uint32_t RawTemperature;
 static uint32_t RawPressure;
 static int64_t Pressure;
 static int64_t Temperature;
+static int64_t FilteredTemperature = INT32_MIN;
 static int32_t lastConversionStart;
 
 static uint32_t conversionDelayMs;
@@ -247,6 +250,12 @@ int32_t PIOS_MS5611_ReadADC(void)
         // Actual temperature (-40…85°C with 0.01°C resolution)
         // TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23
         Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23));
+        if (FilteredTemperature != INT32_MIN) {
+            FilteredTemperature = (FilteredTemperature * (PIOS_MS5611_TEMP_SMOOTHING - 1)
+                                   + Temperature) / PIOS_MS5611_TEMP_SMOOTHING;
+        } else {
+            FilteredTemperature = Temperature;
+        }
     } else {
         int64_t Offset;
         int64_t Sens;
@@ -259,21 +268,21 @@ int32_t PIOS_MS5611_ReadADC(void)
             return -2;
         }
         // check if temperature is less than 20°C
-        if (Temperature < 2000) {
+        if (FilteredTemperature < 2000) {
             // Apply compensation
             // T2 = dT^2 / 2^31
             // OFF2 = 5 ⋅ (TEMP – 2000)^2/2
             // SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2
 
-            int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
+            int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 2000);
             Offset2 = (5 * tcorr) / 2;
             Sens2   = (5 * tcorr) / 4;
             compensation_t2 = (deltaTemp * deltaTemp) >> 31;
             // Apply the "Very low temperature compensation" when temp is less than -15°C
-            if (Temperature < -1500) {
+            if (FilteredTemperature < -1500) {
                 // OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
                 // SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
-                int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
+                int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 1500);
                 Offset2 += 7 * tcorr2;
                 Sens2   += (11 * tcorr2) / 2;
             }
@@ -302,7 +311,7 @@ int32_t PIOS_MS5611_ReadADC(void)
 static float PIOS_MS5611_GetTemperature(void)
 {
     // Apply the second order low and very low temperature compensation offset
-    return ((float)(Temperature - compensation_t2)) / 100.0f;
+    return ((float)(FilteredTemperature - compensation_t2)) / 100.0f;
 }
 
 /**

flight/pios/inc/pios_posix.h

@@ -28,7 +28,9 @@
 
 #include <stdint.h>
 
+#ifndef __cplusplus
 typedef enum { FALSE = 0, TRUE = !FALSE } bool;
+#endif
 
 #ifndef false
         #define false FALSE

flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc

@@ -19,6 +19,7 @@
 # These are the UAVObjects supposed to be build as part of the OpenPilot target
 # (all architectures)
 UAVOBJSRCFILENAMES =
+UAVOBJSRCFILENAMES += statusvtolland
 UAVOBJSRCFILENAMES += vtolselftuningstats
 UAVOBJSRCFILENAMES += accelgyrosettings
 UAVOBJSRCFILENAMES += accessorydesired
@@ -61,6 +62,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
+UAVOBJSRCFILENAMES += groundpathfollowersettings
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats
 UAVOBJSRCFILENAMES += manualcontrolcommand

flight/targets/boards/revolution/firmware/UAVObjects.inc

@@ -19,6 +19,9 @@
 # These are the UAVObjects supposed to be build as part of the OpenPilot target
 # (all architectures)
 UAVOBJSRCFILENAMES =
+UAVOBJSRCFILENAMES += statusgrounddrive
+UAVOBJSRCFILENAMES += pidstatus
+UAVOBJSRCFILENAMES += statusvtolland
 UAVOBJSRCFILENAMES += vtolselftuningstats
 UAVOBJSRCFILENAMES += accelgyrosettings
 UAVOBJSRCFILENAMES += accessorydesired
@@ -61,6 +64,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
+UAVOBJSRCFILENAMES += groundpathfollowersettings
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats
 UAVOBJSRCFILENAMES += manualcontrolcommand

flight/targets/boards/revoproto/firmware/UAVObjects.inc

@@ -19,6 +19,9 @@
 # These are the UAVObjects supposed to be build as part of the OpenPilot target
 # (all architectures)
 UAVOBJSRCFILENAMES =
+UAVOBJSRCFILENAMES += statusgrounddrive
+UAVOBJSRCFILENAMES += pidstatus
+UAVOBJSRCFILENAMES += statusvtolland
 UAVOBJSRCFILENAMES += vtolselftuningstats
 UAVOBJSRCFILENAMES += accelgyrosettings
 UAVOBJSRCFILENAMES += accessorydesired
@@ -61,6 +64,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
+UAVOBJSRCFILENAMES += groundpathfollowersettings
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats
 UAVOBJSRCFILENAMES += manualcontrolcommand

flight/targets/boards/simposix/firmware/Makefile

@@ -20,6 +20,9 @@
 # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 #####
 
+# REVO C++ support 
+USE_CXX = YES
+
 override ARM_SDK_PREFIX :=
 override THUMB :=
 
@@ -83,10 +86,11 @@ endif
 
 ## MODULES
 SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
+CPPSRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.cpp}}
 SRC += ${OUTDIR}/InitMods.c
 ## OPENPILOT CORE:
 SRC += ${OPMODULEDIR}/System/systemmod.c
-SRC += $(OPSYSTEM)/simposix.c
+CPPSRC += $(OPSYSTEM)/simposix.cpp
 SRC += $(OPSYSTEM)/pios_board.c
 SRC += $(FLIGHTLIB)/alarms.c
 SRC += $(OPUAVTALK)/uavtalk.c
@@ -124,6 +128,7 @@ SRC += $(PIOSCORECOMMON)/pios_mem.c
 ## PIOS Hardware
 include $(PIOS)/posix/library.mk
 
+
 include ./UAVObjects.inc
 SRC += $(UAVOBJSRC)
 
@@ -315,7 +320,7 @@ endif
 
 
 # Link: create ELF output file from object files.
-$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
+$(eval $(call LINK_CXX_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
 
 # Assemble: create object files from assembler source files.
 $(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
@@ -330,7 +335,7 @@ $(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
 $(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
 
 # Compile: create object files from C++ source files.
-$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
+$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CXX_TEMPLATE, $(src))))
 
 # Compile: create object files from C++ source files. ARM-only
 $(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))

flight/targets/boards/simposix/firmware/UAVObjects.inc

@@ -24,6 +24,9 @@
 # (all architectures)
 
 UAVOBJSRCFILENAMES =
+UAVOBJSRCFILENAMES += statusgrounddrive
+UAVOBJSRCFILENAMES += pidstatus
+UAVOBJSRCFILENAMES += statusvtolland
 UAVOBJSRCFILENAMES += vtolselftuningstats
 UAVOBJSRCFILENAMES += accessorydesired
 UAVOBJSRCFILENAMES += actuatorcommand
@@ -62,6 +65,7 @@ UAVOBJSRCFILENAMES += gpstime
 UAVOBJSRCFILENAMES += gpsvelocitysensor
 UAVOBJSRCFILENAMES += gpssettings
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
+UAVOBJSRCFILENAMES += groundpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += homelocation

flight/targets/boards/simposix/firmware/simposix.cpp

@@ -31,6 +31,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+extern "C" {
 #include "inc/openpilot.h"
 #include <systemmod.h>
 #include <uavobjectsinit.h>
@@ -74,6 +75,7 @@ static void initTask(void *parameters);
 
 /* Prototype of generated InitModules() function */
 extern void InitModules(void);
+}
 
 /**
  * OpenPilot Main function:

flight/uavtalk/inc/uavtalk.h

@@ -57,8 +57,8 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
 int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
 int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
 int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
-UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
+UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length);
-UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
+UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
 int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
 int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle);
 void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset);

flight/uavtalk/uavtalk.c

@@ -53,7 +53,15 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
 static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
 static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
 static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
-
+// UavTalk Process FSM functions
+static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_OBJID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_TIMESTAMP(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
+static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
 /**
  * Initialize the UAVTalk library
  * \param[in] connection UAVTalkConnection to be used
@@ -350,10 +358,12 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
 /**
  * Process an byte from the telemetry stream.
  * \param[in] connectionHandle UAVTalkConnection to be used
- * \param[in] rxbyte Received byte
+ * \param[in] rxbuffer Received buffer
+ * \param[in/out] Length in bytes of received buffer
+ * \param[in/out] position Next item to be read inside rxbuffer
  * \return UAVTalkRxState
  */
-UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
+UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
 {
     UAVTalkConnectionData *connection;
 
@@ -361,231 +371,83 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
 
     UAVTalkInputProcessor *iproc = &connection->iproc;
 
-    ++connection->stats.rxBytes;
-
     if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
         iproc->state = UAVTALK_STATE_SYNC;
     }
 
-    if (iproc->rxPacketLength < 0xffff) {
+    uint8_t processedBytes = (*position);
-        // update packet byte count
+    uint8_t count = 0;
-        iproc->rxPacketLength++;
-    }
 
-    // Receive state machine
+    // stop processing as soon as a complete packet is received, error is encountered or buffer is processed entirely
-    switch (iproc->state) {
+    while ((count = length - (*position)) > 0
-    case UAVTALK_STATE_SYNC:
+           && iproc->state != UAVTALK_STATE_COMPLETE
-
+           && iproc->state != UAVTALK_STATE_ERROR) {
-        if (rxbyte != UAVTALK_SYNC_VAL) {
+        // Receive state machine
-            connection->stats.rxSyncErrors++;
+        if (iproc->state == UAVTALK_STATE_SYNC &&
+            !UAVTalkProcess_SYNC(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        // Initialize and update the CRC
+        if (iproc->state == UAVTALK_STATE_TYPE &&
-        iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
+            !UAVTalkProcess_TYPE(connection, iproc, rxbuffer, length, position)) {
-
-        iproc->rxPacketLength = 1;
-        iproc->rxCount = 0;
-
-        iproc->type    = 0;
-        iproc->state   = UAVTALK_STATE_TYPE;
-        break;
-
-    case UAVTALK_STATE_TYPE:
-
-        if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
-            connection->stats.rxErrors++;
-            iproc->state = UAVTALK_STATE_SYNC;
             break;
         }
 
-        // update the CRC
+        if (iproc->state == UAVTALK_STATE_SIZE &&
-        iproc->cs    = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+            !UAVTalkProcess_SIZE(connection, iproc, rxbuffer, length, position)) {
-
-        iproc->type  = rxbyte;
-
-        iproc->packet_size = 0;
-        iproc->state = UAVTALK_STATE_SIZE;
-        break;
-
-    case UAVTALK_STATE_SIZE:
-
-        // update the CRC
-        iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
-        if (iproc->rxCount == 0) {
-            iproc->packet_size += rxbyte;
-            iproc->rxCount++;
-            break;
-        }
-        iproc->packet_size += rxbyte << 8;
-        iproc->rxCount      = 0;
-
-        if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
-            // incorrect packet size
-            connection->stats.rxErrors++;
-            iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        iproc->objId = 0;
+        if (iproc->state == UAVTALK_STATE_OBJID &&
-        iproc->state = UAVTALK_STATE_OBJID;
+            !UAVTalkProcess_OBJID(connection, iproc, rxbuffer, length, position)) {
-        break;
-
-    case UAVTALK_STATE_OBJID:
-
-        // update the CRC
-        iproc->cs     = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
-        iproc->objId += rxbyte << (8 * (iproc->rxCount++));
-        if (iproc->rxCount < 4) {
-            break;
-        }
-        iproc->rxCount = 0;
-
-        iproc->instId  = 0;
-        iproc->state   = UAVTALK_STATE_INSTID;
-        break;
-
-    case UAVTALK_STATE_INSTID:
-
-        // update the CRC
-        iproc->cs      = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
-        iproc->instId += rxbyte << (8 * (iproc->rxCount++));
-        if (iproc->rxCount < 2) {
-            break;
-        }
-        iproc->rxCount = 0;
-
-        UAVObjHandle obj = UAVObjGetByID(iproc->objId);
-
-        // Determine data length
-        if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
-            iproc->length = 0;
-            iproc->timestampLength = 0;
-        } else {
-            iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
-            if (obj) {
-                iproc->length = UAVObjGetNumBytes(obj);
-            } else {
-                iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
-            }
-        }
-
-        // Check length
-        if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
-            // packet error - exceeded payload max length
-            connection->stats.rxErrors++;
-            iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        // Check the lengths match
+        if (iproc->state == UAVTALK_STATE_INSTID &&
-        if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
+            !UAVTalkProcess_INSTID(connection, iproc, rxbuffer, length, position)) {
-            // packet error - mismatched packet size
-            connection->stats.rxErrors++;
-            iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        // Determine next state
+        if (iproc->state == UAVTALK_STATE_TIMESTAMP &&
-        if (iproc->type & UAVTALK_TIMESTAMPED) {
+            !UAVTalkProcess_TIMESTAMP(connection, iproc, rxbuffer, length, position)) {
-            // If there is a timestamp get it
-            iproc->timestamp = 0;
-            iproc->state     = UAVTALK_STATE_TIMESTAMP;
-        } else {
-            // If there is a payload get it, otherwise receive checksum
-            if (iproc->length > 0) {
-                iproc->state = UAVTALK_STATE_DATA;
-            } else {
-                iproc->state = UAVTALK_STATE_CS;
-            }
-        }
-        break;
-
-    case UAVTALK_STATE_TIMESTAMP:
-
-        // update the CRC
-        iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
-        iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
-        if (iproc->rxCount < 2) {
-            break;
-        }
-        iproc->rxCount = 0;
-
-        // If there is a payload get it, otherwise receive checksum
-        if (iproc->length > 0) {
-            iproc->state = UAVTALK_STATE_DATA;
-        } else {
-            iproc->state = UAVTALK_STATE_CS;
-        }
-        break;
-
-    case UAVTALK_STATE_DATA:
-
-        // update the CRC
-        iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
-        connection->rxBuffer[iproc->rxCount++] = rxbyte;
-        if (iproc->rxCount < iproc->length) {
-            break;
-        }
-        iproc->rxCount = 0;
-
-        iproc->state   = UAVTALK_STATE_CS;
-        break;
-
-    case UAVTALK_STATE_CS:
-
-        // Check the CRC byte
-        if (rxbyte != iproc->cs) {
-            // packet error - faulty CRC
-            UAVT_DEBUGLOG_PRINTF("BAD CRC");
-            connection->stats.rxCrcErrors++;
-            connection->stats.rxErrors++;
-            iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
+        if (iproc->state == UAVTALK_STATE_DATA &&
-            // packet error - mismatched packet size
+            !UAVTalkProcess_DATA(connection, iproc, rxbuffer, length, position)) {
-            connection->stats.rxErrors++;
-            iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        connection->stats.rxObjects++;
+        if (iproc->state == UAVTALK_STATE_CS &&
-        connection->stats.rxObjectBytes += iproc->length;
+            !UAVTalkProcess_CS(connection, iproc, rxbuffer, length, position)) {
-
+            break;
-        iproc->state = UAVTALK_STATE_COMPLETE;
+        }
-        break;
-
-    default:
-
-        iproc->state = UAVTALK_STATE_ERROR;
-        break;
     }
 
     // Done
+    processedBytes = (*position) - processedBytes;
+    connection->stats.rxBytes += processedBytes;
     return iproc->state;
 }
 
 /**
- * Process an byte from the telemetry stream.
+ * Process a buffer from the telemetry stream.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] rxbyte Received byte
+ * \param[in] rxbuffer Received buffer
+ * \param[in] count bytes inside rxbuffer
  * \return UAVTalkRxState
  */
-UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
+UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length)
 {
-    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte);
+    uint8_t position     = 0;
+    UAVTalkRxState state = UAVTALK_STATE_ERROR;
 
-    if (state == UAVTALK_STATE_COMPLETE) {
+    while (position < length) {
-        UAVTalkReceiveObject(connectionHandle);
+        state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbuffer, length, &position);
+        if (state == UAVTALK_STATE_COMPLETE) {
+            UAVTalkReceiveObject(connectionHandle);
+        }
     }
-
     return state;
 }
 
@@ -1000,6 +862,236 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
     return 0;
 }
 
+/*
+ * Functions that implements the UAVTalk Process FSM. return false to break out of current cycle
+ */
+
+static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
+{
+    uint8_t rxbyte = rxbuffer[(*position)++];
+
+    if (rxbyte != UAVTALK_SYNC_VAL) {
+        connection->stats.rxSyncErrors++;
+        return false;
+    }
+
+    // Initialize and update the CRC
+    iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
+
+    iproc->rxPacketLength = 1;
+    iproc->rxCount = 0;
+
+    iproc->type    = 0;
+    iproc->state   = UAVTALK_STATE_TYPE;
+    return true;
+}
+
+static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
+{
+    uint8_t rxbyte = rxbuffer[(*position)++];
+
+    if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
+        connection->stats.rxErrors++;
+        iproc->state = UAVTALK_STATE_SYNC;
+        return false;
+    }
+
+    // update the CRC
+    iproc->cs    = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+
+    iproc->type  = rxbyte;
+    iproc->rxPacketLength++;
+    iproc->packet_size = 0;
+    iproc->state = UAVTALK_STATE_SIZE;
+    return true;
+}
+
+static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
+{
+    while (iproc->rxCount < 2 && length > (*position)) {
+        uint8_t rxbyte = rxbuffer[(*position)++];
+        // update the CRC
+        iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+        iproc->packet_size += rxbyte << 8 * iproc->rxCount;
+        iproc->rxCount++;
+    }
+
+    if (iproc->rxCount < 2) {
+        return false;;
+    }
+
+    iproc->rxCount = 0;
+
+    if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
+        // incorrect packet size
+        connection->stats.rxErrors++;
+        iproc->state = UAVTALK_STATE_ERROR;
+        return false;
+    }
+    iproc->rxPacketLength += 2;
+    iproc->objId = 0;
+    iproc->state = UAVTALK_STATE_OBJID;
+    return true;
+}
+
+static bool UAVTalkProcess_OBJID(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
+{
+    while (iproc->rxCount < 4 && length > (*position)) {
+        uint8_t rxbyte = rxbuffer[(*position)++];
+        iproc->cs     = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+        iproc->objId += rxbyte << (8 * (iproc->rxCount++));
+    }
+
+    if (iproc->rxCount < 4) {
+        return false;
+    }
+    iproc->rxCount = 0;
+    iproc->rxPacketLength += 4;
+    iproc->instId  = 0;
+    iproc->state   = UAVTALK_STATE_INSTID;
+    return true;
+}
+
+static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
+{
+    while (iproc->rxCount < 2 && length > (*position)) {
+        uint8_t rxbyte = rxbuffer[(*position)++];
+        iproc->cs      = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+        iproc->instId += rxbyte << (8 * (iproc->rxCount++));
+    }
+
+    if (iproc->rxCount < 2) {
+        return false;
+    }
+    iproc->rxPacketLength += 2;
+    iproc->rxCount = 0;
+
+    UAVObjHandle obj = UAVObjGetByID(iproc->objId);
+
+    // Determine data length
+    if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
+        iproc->length = 0;
+        iproc->timestampLength = 0;
+    } else {
+        iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
+        if (obj) {
+            iproc->length = UAVObjGetNumBytes(obj);
+        } else {
+            iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
+        }
+    }
+
+    // Check length
+    if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
+        // packet error - exceeded payload max length
+        connection->stats.rxErrors++;
+        iproc->state = UAVTALK_STATE_ERROR;
+        return false;
+    }
+
+    // Check the lengths match
+    if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
+        // packet error - mismatched packet size
+        connection->stats.rxErrors++;
+        iproc->state = UAVTALK_STATE_ERROR;
+        return false;
+    }
+
+    // Determine next state
+    if (iproc->type & UAVTALK_TIMESTAMPED) {
+        // If there is a timestamp get it
+        iproc->timestamp = 0;
+        iproc->state     = UAVTALK_STATE_TIMESTAMP;
+    } else {
+        // If there is a payload get it, otherwise receive checksum
+        if (iproc->length > 0) {
+            iproc->state = UAVTALK_STATE_DATA;
+        } else {
+            iproc->state = UAVTALK_STATE_CS;
+        }
+    }
+    return true;
+}
+
+static bool UAVTalkProcess_TIMESTAMP(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
+{
+    while (iproc->rxCount < 2 && length > (*position)) {
+        uint8_t rxbyte = rxbuffer[(*position)++];
+        iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+        iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
+    }
+
+    if (iproc->rxCount < 2) {
+        return false;;
+    }
+
+    iproc->rxCount = 0;
+    iproc->rxPacketLength += 2;
+    // If there is a payload get it, otherwise receive checksum
+    if (iproc->length > 0) {
+        iproc->state = UAVTALK_STATE_DATA;
+    } else {
+        iproc->state = UAVTALK_STATE_CS;
+    }
+    return true;
+}
+
+static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
+{
+    uint8_t toCopy = iproc->length - iproc->rxCount;
+
+    if (toCopy > length - (*position)) {
+        toCopy = length - (*position);
+    }
+
+    memcpy(&connection->rxBuffer[iproc->rxCount], &rxbuffer[(*position)], toCopy);
+    (*position)    += toCopy;
+
+    // update the CRC
+    iproc->cs       = PIOS_CRC_updateCRC(iproc->cs, &connection->rxBuffer[iproc->rxCount], toCopy);
+    iproc->rxCount += toCopy;
+
+    iproc->rxPacketLength += toCopy;
+
+    if (iproc->rxCount < iproc->length) {
+        return false;
+    }
+
+    iproc->rxCount = 0;
+    iproc->state   = UAVTALK_STATE_CS;
+    return true;
+}
+
+static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
+{
+    // Check the CRC byte
+    uint8_t rxbyte = rxbuffer[(*position)++];
+
+    if (rxbyte != iproc->cs) {
+        // packet error - faulty CRC
+        UAVT_DEBUGLOG_PRINTF("BAD CRC");
+        connection->stats.rxCrcErrors++;
+        connection->stats.rxErrors++;
+        iproc->state = UAVTALK_STATE_ERROR;
+        return false;;
+    }
+    iproc->rxPacketLength++;
+
+    if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
+        // packet error - mismatched packet size
+        connection->stats.rxErrors++;
+        iproc->state = UAVTALK_STATE_ERROR;
+        return false;;
+    }
+
+    connection->stats.rxObjects++;
+    connection->stats.rxObjectBytes += iproc->length;
+
+    iproc->state = UAVTALK_STATE_COMPLETE;
+    return true;
+}
+
+
 /**
  * @}
  * @}

ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp

@@ -138,7 +138,7 @@ void MagicWaypointGadgetWidget::positionSelected(double north, double east)
 
     pathDesired.End[PathDesired::END_NORTH] = north * scale;
     pathDesired.End[PathDesired::END_EAST]  = east * scale;
-    pathDesired.Mode = PathDesired::MODE_FLYENDPOINT;
+    pathDesired.Mode = PathDesired::MODE_GOTOENDPOINT;
     getPathDesired()->setData(pathDesired);
 }
 

ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp

@@ -86,18 +86,17 @@ void modelMapProxy::selectedWPChanged(QList<WayPointItem *> list)
 modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type)
 {
     switch (type) {
-    case MapDataDelegate::MODE_FLYENDPOINT:
+    case MapDataDelegate::MODE_GOTOENDPOINT:
-    case MapDataDelegate::MODE_FLYVECTOR:
+    case MapDataDelegate::MODE_FOLLOWVECTOR:
-    case MapDataDelegate::MODE_DRIVEENDPOINT:
+    case MapDataDelegate::MODE_VELOCITY:
-    case MapDataDelegate::MODE_DRIVEVECTOR:
+    case MapDataDelegate::MODE_LAND:
+    case MapDataDelegate::MODE_BRAKE:
         return OVERLAY_LINE;
 
-    case MapDataDelegate::MODE_FLYCIRCLERIGHT:
+    case MapDataDelegate::MODE_CIRCLERIGHT:
-    case MapDataDelegate::MODE_DRIVECIRCLERIGHT:
         return OVERLAY_CIRCLE_RIGHT;
 
-    case MapDataDelegate::MODE_FLYCIRCLELEFT:
+    case MapDataDelegate::MODE_CIRCLELEFT:
-    case MapDataDelegate::MODE_DRIVECIRCLELEFT:
     default:
         return OVERLAY_CIRCLE_LEFT;
     }

ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp

@@ -104,15 +104,13 @@ void opmap_edit_waypoint_dialog::setupModeWidgets()
     MapDataDelegate::ModeOptions mode = (MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt();
 
     switch (mode) {
-    case MapDataDelegate::MODE_FLYENDPOINT:
+    case MapDataDelegate::MODE_GOTOENDPOINT:
-    case MapDataDelegate::MODE_FLYVECTOR:
+    case MapDataDelegate::MODE_FOLLOWVECTOR:
-    case MapDataDelegate::MODE_FLYCIRCLERIGHT:
+    case MapDataDelegate::MODE_CIRCLERIGHT:
-    case MapDataDelegate::MODE_FLYCIRCLELEFT:
+    case MapDataDelegate::MODE_CIRCLELEFT:
-    case MapDataDelegate::MODE_DRIVEENDPOINT:
-    case MapDataDelegate::MODE_DRIVEVECTOR:
-    case MapDataDelegate::MODE_DRIVECIRCLELEFT:
-    case MapDataDelegate::MODE_DRIVECIRCLERIGHT:
     case MapDataDelegate::MODE_DISARMALARM:
+    case MapDataDelegate::MODE_LAND:
+    case MapDataDelegate::MODE_BRAKE:
         ui->modeParam1->setVisible(false);
         ui->modeParam2->setVisible(false);
         ui->modeParam3->setVisible(false);
@@ -122,6 +120,16 @@ void opmap_edit_waypoint_dialog::setupModeWidgets()
         ui->dsb_modeParam3->setVisible(false);
         ui->dsb_modeParam4->setVisible(false);
         break;
+    case MapDataDelegate::MODE_VELOCITY:
+        ui->modeParam1->setVisible(true);
+        ui->modeParam2->setVisible(true);
+        ui->modeParam3->setVisible(true);
+        ui->modeParam4->setVisible(false);
+        ui->dsb_modeParam1->setVisible(true);
+        ui->dsb_modeParam2->setVisible(true);
+        ui->dsb_modeParam3->setVisible(true);
+        ui->dsb_modeParam4->setVisible(false);
+        break;
     case MapDataDelegate::MODE_FIXEDATTITUDE:
         ui->modeParam1->setText("pitch");
         ui->modeParam2->setText("roll");

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp

@@ -106,19 +106,17 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa
 {
     switch (type) {
     case flightDataModel::MODE:
-        combo->addItem("Fly Direct", MODE_FLYENDPOINT);
+        combo->addItem("Goto Endpoint", MODE_GOTOENDPOINT);
-        combo->addItem("Fly Vector", MODE_FLYVECTOR);
+        combo->addItem("Follow Vector", MODE_FOLLOWVECTOR);
-        combo->addItem("Fly Circle Right", MODE_FLYCIRCLERIGHT);
+        combo->addItem("Circle Right", MODE_CIRCLERIGHT);
-        combo->addItem("Fly Circle Left", MODE_FLYCIRCLELEFT);
+        combo->addItem("Circle Left", MODE_CIRCLELEFT);
-
-        combo->addItem("Drive Direct", MODE_DRIVEENDPOINT);
-        combo->addItem("Drive Vector", MODE_DRIVEVECTOR);
-        combo->addItem("Drive Circle Right", MODE_DRIVECIRCLELEFT);
-        combo->addItem("Drive Circle Left", MODE_DRIVECIRCLERIGHT);
-
         combo->addItem("Fixed Attitude", MODE_FIXEDATTITUDE);
         combo->addItem("Set Accessory", MODE_SETACCESSORY);
         combo->addItem("Disarm Alarm", MODE_DISARMALARM);
+        combo->addItem("Land", MODE_LAND);
+        combo->addItem("Brake", MODE_BRAKE);
+        combo->addItem("Velocity", MODE_VELOCITY);
+
         break;
     case flightDataModel::CONDITION:
         combo->addItem("None", ENDCONDITION_NONE);

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h

@@ -36,9 +36,10 @@ class MapDataDelegate : public QItemDelegate {
     Q_OBJECT
 
 public:
-    typedef enum { MODE_FLYENDPOINT   = 0, MODE_FLYVECTOR = 1, MODE_FLYCIRCLERIGHT = 2, MODE_FLYCIRCLELEFT = 3,
+    typedef enum { MODE_GOTOENDPOINT  = 0, MODE_FOLLOWVECTOR = 1, MODE_CIRCLERIGHT = 2, MODE_CIRCLELEFT = 3,
-                   MODE_DRIVEENDPOINT = 4, MODE_DRIVEVECTOR = 5, MODE_DRIVECIRCLELEFT = 6, MODE_DRIVECIRCLERIGHT = 7,
+                   MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10, MODE_LAND = 11,
-                   MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10 } ModeOptions;
+                   MODE_BRAKE = 12, MODE_VELOCITY = 13 } ModeOptions;
+
     typedef enum { ENDCONDITION_NONE = 0, ENDCONDITION_TIMEOUT = 1, ENDCONDITION_DISTANCETOTARGET = 2,
                    ENDCONDITION_LEGREMAINING = 3, ENDCONDITION_BELOWERROR = 4, ENDCONDITION_ABOVEALTITUDE = 5,
                    ENDCONDITION_ABOVESPEED = 6, ENDCONDITION_POINTINGTOWARDSNEXT = 7, ENDCONDITION_PYTHONSCRIPT = 8,

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -27,6 +27,9 @@ OTHER_FILES += UAVObjects.pluginspec
 
 # Add in all of the synthetic/generated uavobject files
 HEADERS += \
+    $$UAVOBJECT_SYNTHETICS/statusgrounddrive.h \
+    $$UAVOBJECT_SYNTHETICS/pidstatus.h \
+    $$UAVOBJECT_SYNTHETICS/statusvtolland.h \
     $$UAVOBJECT_SYNTHETICS/vtolselftuningstats.h \
     $$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \
     $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
@@ -96,6 +99,7 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \
     $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \
     $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
+    $$UAVOBJECT_SYNTHETICS/groundpathfollowersettings.h \
     $$UAVOBJECT_SYNTHETICS/ratedesired.h \
     $$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
     $$UAVOBJECT_SYNTHETICS/i2cstats.h \
@@ -133,6 +137,9 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/perfcounter.h
 
 SOURCES += \
+    $$UAVOBJECT_SYNTHETICS/statusgrounddrive.cpp \
+    $$UAVOBJECT_SYNTHETICS/pidstatus.cpp \
+    $$UAVOBJECT_SYNTHETICS/statusvtolland.cpp \
     $$UAVOBJECT_SYNTHETICS/vtolselftuningstats.cpp \
     $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
     $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
@@ -202,6 +209,7 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \
     $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/groundpathfollowersettings.cpp \
     $$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
     $$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
     $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \

make/common-defs.mk

@@ -164,7 +164,6 @@ ifeq ($(USE_CXX), YES)
     CPPFLAGS += -DPIOS_ENABLE_CXX
 endif
 
-
 # Compiler flags to generate dependency files
 CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
 

shared/uavobjectdefinition/altitudeholdsettings.xml

@@ -2,7 +2,7 @@
     <object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
         <description>Settings for the @ref AltitudeHold module</description>
 	<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
-	<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" />
+	<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.3,0.3,2.0" />
 	<field name="CutThrustWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
         <field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
         <field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />

shared/uavobjectdefinition/flightmodesettings.xml

@@ -83,34 +83,35 @@
                limits="\
                        %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
                        %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
-                       %0903NE:POI:PathPlanner:AutoCruise:Land;\
+                       %0903NE:POI:PathPlanner:AutoCruise;\
                        \
                        %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
                        %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
-                       %0903NE:POI:PathPlanner:AutoCruise:Land;\
+                       %0903NE:POI:PathPlanner:AutoCruise;\
                        \
                        %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
                        %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
-                       %0903NE:POI:PathPlanner:AutoCruise:Land;\
+                       %0903NE:POI:PathPlanner:AutoCruise;\
                        \
                        %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
                        %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
-                       %0903NE:POI:PathPlanner:AutoCruise:Land;\
+                       %0903NE:POI:PathPlanner:AutoCruise;\
                        \
                        %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
                        %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
-                       %0903NE:POI:PathPlanner:AutoCruise:Land;\
+                       %0903NE:POI:PathPlanner:AutoCruise;\
                        \
                        %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
                        %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
-                       %0903NE:POI:PathPlanner:AutoCruise:Land;"/>
+                       %0903NE:POI:PathPlanner: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="DisableSanityChecks" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
         <field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
-        <field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.4"/>
+        <field name="ReturnToBaseNextCommand" units="" type="enum" elements="1" options="Hold,Land" defaultvalue="Hold"/>
+        <field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.6"/>
         <field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
 	<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
         <access gcs="readwrite" flight="readwrite"/>

shared/uavobjectdefinition/groundpathfollowersettings.xml

@@ -0,0 +1,30 @@
+<xml>
+    <object name="GroundPathFollowerSettings" singleinstance="true" settings="true" category="Control">
+        <description>Settings for the @ref GroundPathFollowerModule</description>
+
+        <!-- these coefficients control desired movement in airspace -->
+        <field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="2"/>
+                <!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
+        <field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="0"/>
+                <!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
+        <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
+        <field name="VelocityFeedForward" units="s" type="float" elements="1" defaultvalue="0.1"/>
+                <!-- how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response -->
+        
+	<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.2"/>
+                <!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
+        <!-- these coefficients control actual control outputs -->
+
+        <field name="SpeedPI" units="deg / (m/s)" type="float" elements="4" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.1, .1, 0.001, 0.8"/>
+                <!-- coefficients for desired pitch
+                     in relation to speed error IASerror -->
+        <field name="ThrustLimit" units="" type="float" elements="2" elementnames="Min,SlowForward, Max" defaultvalue="-0.3,0.15,0.3" />
+                <!-- minimum and maximum allowed thrust and setpoint for cruise speed -->
+        <field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
+
+        <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>

shared/uavobjectdefinition/nedaccel.xml

@@ -6,7 +6,7 @@
         <field name="Down" units="m/s^2" type="float" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="periodic" period="10001"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
         <logging updatemode="manual" period="0"/>
     </object>
 </xml>

shared/uavobjectdefinition/pathaction.xml

@@ -2,9 +2,10 @@
     <object name="PathAction" singleinstance="false" settings="false" category="Navigation">
         <description>A waypoint command the pathplanner is to use at a certain waypoint</description>
 
-        <field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
+	    <!-- ensure the following Mode options are exactly the same as in pathdesired mode -->
-            DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
+        <field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
-            FixedAttitude, SetAccessory, DisarmAlarm" default="Endpoint" />
+            FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
+            
         <field name="ModeParameters" units="" type="float" elements="4" default="0"/>
         <field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
             DistanceToTarget,LegRemaining,BelowError,AboveAltitude,AboveSpeed,PointingTowardsNext,

shared/uavobjectdefinition/pathdesired.xml

@@ -3,16 +3,14 @@
         <description>The endpoint or path the craft is trying to achieve.  Can come from @ref ManualControl or @ref PathPlanner </description>
 
 	<field name="Start" units="m" type="float" elementnames="North,East,Down" default="0"/>
-    <field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
+    	<field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
 	<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
 	<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
 
-	<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
+	<!-- ensure the following Mode options are exactly the same as in pathaction mode -->
-		DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
+    	<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
-		FixedAttitude,
+            FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
-		SetAccessory,
+
-		Land,
-		DisarmAlarm,Brake" default="0"/>
 	<!-- Endpoint mode - move directly towards endpoint regardless of position -->
 	<!-- Straight Mode - move across linear path through Start towards the waypoint end, adjusting velocity - continue straight -->
 	<!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)-->

shared/uavobjectdefinition/pathsummary.xml

@@ -11,6 +11,8 @@
 	<field name="decelrate" units="m/s2" type="float" elements="1"/>
 	<field name="brakeRateActualDesiredRatio" units="" type="float" elements="1"/>
 	<field name="velocityIntoHold" units="m/s" type="float" elements="1"/>
+    	<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
+            FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>

shared/uavobjectdefinition/pidstatus.xml

@@ -0,0 +1,18 @@
+<xml>
+    <object name="PIDStatus" singleinstance="true" settings="false" category="Navigation">
+    <description>Status of a PID loop for debugging</description>
+	<field name="setpoint" units="m" type="float" elements="1"/>
+	<field name="actual" units="m" type="float" elements="1"/>
+	<field name="error" units="m" type="float" elements="1"/>
+	<field name="ulow" units="m" type="float" elements="1"/>
+	<field name="uhigh" units="m" type="float" elements="1"/>
+	<field name="command" units="m" type="float" elements="1"/>
+	<field name="P" units="m" type="float" elements="1"/>
+	<field name="I" units="m" type="float" elements="1"/>
+	<field name="D" units="m" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+	<logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/statusgrounddrive.xml

@@ -0,0 +1,14 @@
+<xml>
+    <object name="StatusGroundDrive" singleinstance="true" settings="false" category="Navigation">
+    <description>Status of a Ground drive sequence</description>
+	<field name="ControlState" units="" type="enum" elements="1" options="Inactive,OnTrack,TurnAroundRight,TurnAroundLeft,Brake" default="0"/>
+    <field name="NECommand" units="" type="float" elementnames="North,East" defaultvalue="0.0, 0.0"/>
+    <field name="State" units="" type="float" elementnames="Yaw,Velocity,Thrust" defaultvalue="0.0"/>
+    <field name="BodyCommand" units="" type="float" elementnames="Forward,Right" defaultvalue="0.0, 0.0"/>
+    <field name="ControlCommand" units="" type="float" elementnames="Speed,Course" defaultvalue="0.0, 0.0"/>
+    <access gcs="readwrite" flight="readwrite"/>
+    <telemetrygcs acked="false" updatemode="manual" period="0"/>
+    <telemetryflight acked="false" updatemode="onchange" period="100"/>
+	<logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/statusvtolland.xml

@@ -0,0 +1,19 @@
+<xml>
+    <object name="StatusVtolLand" singleinstance="true" settings="false" category="Navigation">
+    <description>Status of a Vtol landing sequence</description>
+	<field name="State" units="" type="enum" elements="1" options="Inactive,InitAltHold,WtgForDescentRate,AtDescentRate,WtgForGroundEffect,
+		GroundEffect,ThrustDown,ThrustOff,Disarmed, Abort" default="0"/>
+	<field name="AltitudeAtState" units="m" type="float" elements="10" default="0"/>
+	<field name="StateExitReason" units="" type="enum" elements="10" options="None,DescentRateOk,OnGround,BounceVelocity,BounceAccel,LowDescentRate,ZeroThrust,PositionError,Timeout" default="0"/>
+	<field name="targetDescentRate" units="m" type="float" elements="1"/>
+	<field name="averageDescentRate" units="m" type="float" elements="1"/>
+	<field name="averageDescentThrust" units="m" type="float" elements="1"/>
+	<field name="calculatedNeutralThrust" units="m" type="float" elements="1"/>
+	<field name="AltitudeState" units="" type="enum" elements="1" options="High,Low" default="0"/>
+    	<field name="WtgForGroundEffect" units="" type="float" elementnames="BounceVelocity,BounceAccel" defaultvalue="0.0, 0.0"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="onchange" period="100"/>
+	<logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/vtolpathfollowersettings.xml

@@ -8,7 +8,7 @@
         <field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
         <field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
         <field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="8.0, 0.5, 0.0, 15"/>
-        <field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1, 0.01, 0.0, 1.0"/>
+        <field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.3, 0.3, 0.0, 1.0"/>
         <field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
         <field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="2"/>
         <field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="manual"/>
@@ -23,6 +23,7 @@
         <field name="BrakeMaxPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
         <field name="BrakeHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="12.0, 0.0, 0.03, 15"/>
         <field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
+        <field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>