Merge branch 'corvuscorax/OP-1309_Stabilization-Refactoring' into corvuscorax/OP-1259_Cruise_Control_Tweaks

Conflicts:
	flight/modules/Stabilization/stabilization.c
	shared/uavobjectdefinition/stabilizationsettings.xml

flight/libraries/math/mathmisc.c

@@ -1,9 +1,13 @@
 /**
  ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Reuseable math functions
+ * @{
  *
- * @file       examplemodperiodic.c
+ * @file       mathmisc.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
- * @brief      Example module to be used as a template for actual modules.
+ * @brief      Reuseable math functions
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -23,9 +27,6 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-#ifndef EXAMPLEMODPERIODIC_H
-#define EXAMPLEMODPERIODIC_H
 
-int32_t ExampleModPeriodicInitialize();
+
-int32_t GuidanceInitialize(void);
+// space deliberately left empty, any non inline misc math functions can go here
-#endif // EXAMPLEMODPERIODIC_H

flight/libraries/math/mathmisc.h

@@ -0,0 +1,55 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Reuseable math functions
+ * @{
+ *
+ * @file       mathmisc.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Reuseable math functions
+ *
+ * @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 MATHMISC_H
+#define MATHMISC_H
+
+// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
+// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
+// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
+static inline float boundf(float val, float boundary1, float boundary2)
+{
+    if (boundary1 > boundary2) {
+        if (!(val >= boundary2)) {
+            return boundary2;
+        } else if (!(val <= boundary1)) {
+            return boundary1;
+        }
+    } else {
+        if (!(val >= boundary1)) {
+            return boundary1;
+        } else if (!(val <= boundary2)) {
+            return boundary2;
+        }
+    }
+    return val;
+}
+
+#endif /* MATHMISC_H */

flight/libraries/math/pid.c

@@ -30,11 +30,9 @@
 
 #include "openpilot.h"
 #include "pid.h"
+#include <mathmisc.h>
 #include <pios_math.h>
 
-// ! Private method
-static float bound(float val, float range);
-
 // ! Store the shared time constant for the derivative cutoff.
 static float deriv_tau   = 7.9577e-3f;
 
@@ -52,7 +50,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
 {
     // Scale up accumulator by 1000 while computing to avoid losing precision
     pid->iAccumulator += err * (pid->i * dT * 1000.0f);
-    pid->iAccumulator  = bound(pid->iAccumulator, pid->iLim * 1000.0f);
+    pid->iAccumulator  = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
 
     // Calculate DT1 term
     float diff  = (err - pid->lastErr);
@@ -84,7 +82,7 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
 
     // Scale up accumulator by 1000 while computing to avoid losing precision
     pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
-    pid->iAccumulator  = bound(pid->iAccumulator, pid->iLim * 1000.0f);
+    pid->iAccumulator  = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
 
     // Calculate DT1 term,
     float dterm = 0;
@@ -142,16 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
     pid->d    = d;
     pid->iLim = iLim;
 }
-
-/**
- * Bound input value between limits
- */
-static float bound(float val, float range)
-{
-    if (val < -range) {
-        val = -range;
-    } else if (val > range) {
-        val = range;
-    }
-    return val;
-}

flight/libraries/sanitycheck.c

@@ -46,7 +46,7 @@
 ****************************/
 
 // ! Check a stabilization mode switch position for safety
-static int32_t check_stabilization_settings(int index, bool multirotor);
+static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
 
 /**
  * Run a preflight check over the hardware configuration
@@ -98,34 +98,19 @@ int32_t configuration_check()
             }
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
+            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor, coptercontrol) : severity;
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
+            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor, coptercontrol) : severity;
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
-            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
+            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor, coptercontrol) : severity;
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
             if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
-            // TODO: put check equivalent to TASK_MONITOR_IsRunning
-            // here as soon as available for delayed callbacks
-            break;
-        case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
-            if (coptercontrol) {
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
-                severity = SYSTEMALARMS_ALARM_ERROR;
-            }
-            break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
@@ -199,7 +184,7 @@ int32_t configuration_check()
  * @param[in] index Which stabilization mode to check
  * @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
  */
-static int32_t check_stabilization_settings(int index, bool multirotor)
+static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
 {
     // Make sure the modes have identical sizes
     if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
@@ -224,18 +209,45 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
         return SYSTEMALARMS_ALARM_ERROR;
     }
 
-    // For multirotors verify that nothing is set to "none"
+    // For multirotors verify that roll/pitch/yaw are not set to "none"
+    // (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
     if (multirotor) {
-        for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
+        for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
-            if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
+            if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
                 return SYSTEMALARMS_ALARM_ERROR;
             }
         }
     }
 
+    // coptercontrol cannot do altitude holding
+    if (coptercontrol) {
+        if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
+            || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
+            ) {
+            return SYSTEMALARMS_ALARM_ERROR;
+        }
+    }
+
+    // check that thrust modes are only set to thrust axis
+    for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
+        if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
+            || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
+            ) {
+            return SYSTEMALARMS_ALARM_ERROR;
+        }
+    }
+    if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
+          || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
+          || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
+          || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
+          )) {
+        return SYSTEMALARMS_ALARM_ERROR;
+    }
+
     // Warning: This assumes that certain conditions in the XML file are met.  That
-    // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
+    // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
-    // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
+    // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
+    // (this is checked at compile time by static constraint manualcontrol.h)
 
     return SYSTEMALARMS_ALARM_OK;
 }

flight/modules/CameraStab/camerastab.c

@@ -79,7 +79,6 @@ static struct CameraStab_data {
 
 // Private functions
 static void attitudeUpdated(UAVObjEvent *ev);
-static float bound(float val, float limit);
 
 #ifdef USE_GIMBAL_FF
 static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab);
@@ -186,7 +185,8 @@ static void attitudeUpdated(UAVObjEvent *ev)
                     input_rate = accessory.AccessoryVal *
                                  cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i];
                     if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
-                        csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis,
+                        csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
+                                                -cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i],
                                                 cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
                     }
                     break;
@@ -229,7 +229,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
         // bounding for elevon mixing occurs on the unmixed output
         // to limit the range of the mixed output you must limit the range
         // of both the unmixed pitch and unmixed roll
-        float output = bound((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], 1.0f);
+        float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f);
 
         // set output channels
         switch (i) {
@@ -282,13 +282,6 @@ static void attitudeUpdated(UAVObjEvent *ev)
     }
 }
 
-float bound(float val, float limit)
-{
-    return (val > limit) ? limit :
-           (val < -limit) ? -limit :
-           val;
-}
-
 #ifdef USE_GIMBAL_FF
 void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab)
 {

flight/modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -85,7 +85,6 @@ static void updatePathVelocity();
 static uint8_t updateFixedDesiredAttitude();
 static void updateFixedAttitude();
 static void airspeedStateUpdatedCb(UAVObjEvent *ev);
-static float bound(float val, float min, float max);
 
 /**
  * Initialise the module, called on startup
@@ -277,9 +276,9 @@ static void updatePathVelocity()
     case PATHDESIRED_MODE_DRIVEVECTOR:
     default:
         groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
-                      bound(progress.fractional_progress, 0, 1);
+                      boundf(progress.fractional_progress, 0, 1);
         altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
-                           bound(progress.fractional_progress, 0, 1);
+                           boundf(progress.fractional_progress, 0, 1);
         break;
     }
     // make sure groundspeed is not zero
@@ -427,7 +426,7 @@ static uint8_t updateFixedDesiredAttitude()
 
     // Desired ground speed
     groundspeedDesired       = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
-    indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias,
+    indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias,
                                       fixedwingpathfollowerSettings.HorizontalVelMin,
                                       fixedwingpathfollowerSettings.HorizontalVelMax);
 
@@ -435,7 +434,7 @@ static uint8_t updateFixedDesiredAttitude()
     airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
 
     // Vertical speed error
-    descentspeedDesired = bound(
+    descentspeedDesired = boundf(
         velocityDesired.Down,
         -fixedwingpathfollowerSettings.VerticalVelMax,
         fixedwingpathfollowerSettings.VerticalVelMax);
@@ -473,14 +472,14 @@ static uint8_t updateFixedDesiredAttitude()
      */
     // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
     if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
-        powerIntegral = bound(powerIntegral + -descentspeedError * dT,
+        powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
                                -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
                                fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
                                ) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
     } else { powerIntegral = 0; }
 
     // Compute the cross feed from vertical speed to pitch, with saturation
-    float speedErrorToPowerCommandComponent = bound(
+    float speedErrorToPowerCommandComponent = boundf(
         (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
         -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
         fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
@@ -497,7 +496,7 @@ static uint8_t updateFixedDesiredAttitude()
     fixedwingpathfollowerStatus.Command.Power  = powerCommand;
 
     // set thrust
-    stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
+    stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
                                 fixedwingpathfollowerSettings.ThrustLimit.Min,
                                 fixedwingpathfollowerSettings.ThrustLimit.Max);
 
@@ -529,13 +528,13 @@ static uint8_t updateFixedDesiredAttitude()
 
     if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
         // Integrate with saturation
-        airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
+        airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
                                   -fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
                                   fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
     }
 
     // Compute the cross feed from vertical speed to pitch, with saturation
-    float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
+    float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
                                                         -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
                                                         fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
                                                         );
@@ -549,7 +548,7 @@ static uint8_t updateFixedDesiredAttitude()
     fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
     fixedwingpathfollowerStatus.Command.Speed  = pitchCommand;
 
-    stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
+    stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
                                fixedwingpathfollowerSettings.PitchLimit.Min,
                                fixedwingpathfollowerSettings.PitchLimit.Max);
 
@@ -606,7 +605,7 @@ static uint8_t updateFixedDesiredAttitude()
         courseError -= 360.0f;
     }
 
-    courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
+    courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
                             -fixedwingpathfollowerSettings.CoursePI.ILimit,
                             fixedwingpathfollowerSettings.CoursePI.ILimit);
     courseCommand  = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
@@ -616,7 +615,7 @@ static uint8_t updateFixedDesiredAttitude()
     fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
     fixedwingpathfollowerStatus.Command.Course  = courseCommand;
 
-    stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
+    stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral +
                               courseCommand,
                               fixedwingpathfollowerSettings.RollLimit.Min,
                               fixedwingpathfollowerSettings.RollLimit.Max);
@@ -633,7 +632,7 @@ static uint8_t updateFixedDesiredAttitude()
 
     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
+    stabDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 
     StabilizationDesiredSet(&stabDesired);
 
@@ -642,20 +641,6 @@ static uint8_t updateFixedDesiredAttitude()
     return result;
 }
 
-
-/**
- * Bound input value between limits
- */
-static float bound(float val, float min, float max)
-{
-    if (val < min) {
-        val = min;
-    } else if (val > max) {
-        val = max;
-    }
-    return val;
-}
-
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);

flight/modules/ManualControl/altitudehandler.c

@@ -1,133 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup ManualControl
- * @brief Interpretes the control input in ManualControlCommand
- * @{
- *
- * @file       altitudehandler.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- ******************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#include "inc/manualcontrol.h"
-#include <manualcontrolcommand.h>
-#include <stabilizationbank.h>
-#include <altitudeholddesired.h>
-#include <altitudeholdsettings.h>
-#include <positionstate.h>
-
-#if defined(REVOLUTION)
-// Private constants
-
-// Private types
-
-// Private functions
-
-/**
- * @brief Handler to control deprecated flight modes controlled by AltitudeHold module
- * @input: ManualControlCommand
- * @output: AltitudeHoldDesired
- */
-void altitudeHandler(bool newinit)
-{
-    const float DEADBAND      = 0.20f;
-    const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
-    const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
-
-    if (newinit) {
-        StabilizationBankInitialize();
-        AltitudeHoldDesiredInitialize();
-        AltitudeHoldSettingsInitialize();
-        PositionStateInitialize();
-    }
-
-
-    // this is the max speed in m/s at the extents of thrust
-    float thrustRate;
-    uint8_t thrustExp;
-
-    static uint8_t flightMode;
-    static bool newaltitude = true;
-
-    ManualControlCommandData cmd;
-    ManualControlCommandGet(&cmd);
-
-    FlightStatusFlightModeGet(&flightMode);
-
-    AltitudeHoldDesiredData altitudeHoldDesiredData;
-    AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
-
-    AltitudeHoldSettingsThrustExpGet(&thrustExp);
-    AltitudeHoldSettingsThrustRateGet(&thrustRate);
-
-    StabilizationBankData stabSettings;
-    StabilizationBankGet(&stabSettings);
-
-    PositionStateData posState;
-    PositionStateGet(&posState);
-
-    altitudeHoldDesiredData.Roll  = cmd.Roll * stabSettings.RollMax;
-    altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax;
-    altitudeHoldDesiredData.Yaw   = cmd.Yaw * stabSettings.ManualRate.Yaw;
-
-    if (newinit) {
-        newaltitude = true;
-    }
-
-    uint8_t cutOff;
-    AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff);
-    if (cutOff && cmd.Thrust < 0) {
-        // Cut thrust if desired
-        altitudeHoldDesiredData.SetPoint    = cmd.Thrust;
-        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST;
-        newaltitude = true;
-    } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > DEADBAND_HIGH) {
-        // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
-        // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
-        altitudeHoldDesiredData.SetPoint    = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
-        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
-        newaltitude = true;
-    } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) {
-        altitudeHoldDesiredData.SetPoint    = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate);
-        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
-        newaltitude = true;
-    } else if (newaltitude == true) {
-        altitudeHoldDesiredData.SetPoint    = posState.Down;
-        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
-        newaltitude = false;
-    }
-
-    AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
-}
-
-#else /* if defined(REVOLUTION) */
-void altitudeHandler(__attribute__((unused)) bool newinit)
-{
-    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
-}
-#endif // REVOLUTION
-
-
-/**
- * @}
- * @}
- */

flight/modules/ManualControl/inc/manualcontrol.h

@@ -61,13 +61,6 @@ void manualHandler(bool newinit);
  */
 void stabilizedHandler(bool newinit);
 
-/**
- * @brief Handler to control deprecated flight modes controlled by AltitudeHold module
- * @input: ManualControlCommand
- * @output: AltitudeHoldDesired
- */
-void altitudeHandler(bool newinit);
-
 /**
  * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
  * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
@@ -88,7 +81,7 @@ void pathPlannerHandler(bool newinit);
  */
 #define assumptions1 \
     ( \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
@@ -97,7 +90,7 @@ void pathPlannerHandler(bool newinit);
 
 #define assumptions3 \
     ( \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
@@ -106,7 +99,7 @@ void pathPlannerHandler(bool newinit);
 
 #define assumptions5 \
     ( \
-        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+        ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
         ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
@@ -119,9 +112,6 @@ void pathPlannerHandler(bool newinit);
         ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
         ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
         ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
-        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
-        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
-        ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
         ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
         ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
         ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \

flight/modules/ManualControl/manualcontrol.c

@@ -85,16 +85,6 @@ static const controlHandler handler_AUTOTUNE = {
 };
 
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
-// TODO: move the altitude handling into stabi
-static const controlHandler handler_ALTITUDE = {
-    .controlChain      = {
-        .Stabilization = true,
-        .PathFollower  = false,
-        .PathPlanner   = false,
-    },
-    .handler           = &altitudeHandler,
-};
-
 static const controlHandler handler_PATHFOLLOWER = {
     .controlChain      = {
         .Stabilization = true,
@@ -206,7 +196,6 @@ static void manualControlTask(void)
         handler = &handler_STABILIZED;
         break;
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
-    case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
     case FLIGHTSTATUS_FLIGHTMODE_LAND:
@@ -216,10 +205,6 @@ static void manualControlTask(void)
     case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
         handler = &handler_PATHPLANNER;
         break;
-    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
-        handler = &handler_ALTITUDE;
-        break;
 #endif
     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
         handler = &handler_AUTOTUNE;

flight/modules/ManualControl/stabilizedhandler.c

@@ -86,25 +86,25 @@ void stabilizedHandler(bool newinit)
     }
 
     stabilization.Roll =
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
-        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax :
         0; // this is an invalid mode
 
     stabilization.Pitch =
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
         0; // this is an invalid mode
@@ -120,13 +120,13 @@ void stabilizedHandler(bool newinit)
     } else {
         stabilization.StabilizationMode.Yaw = stab_settings[2];
         stabilization.Yaw =
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
-            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
             0; // this is an invalid mode

flight/modules/Stabilization/altitudeloop.c

@@ -1,8 +1,8 @@
 /**
  ******************************************************************************
  *
- * @file       guidance.c
+ * @file       altitudeloop.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  * @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.
@@ -26,89 +26,122 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-/**
- * Input object: ActiveWaypoint
- * Input object: PositionState
- * Input object: ManualControlCommand
- * Output object: AttitudeDesired
- *
- * This module will periodically update the value of the AttitudeDesired object.
- *
- * The module executes in its own thread in this example.
- *
- * 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 <altitudeloop.h>
 #include <CoordinateConversions.h>
 #include <altitudeholdsettings.h>
-#include <altitudeholddesired.h> // object that will be updated by the module
 #include <altitudeholdstatus.h>
-#include <flightstatus.h>
-#include <stabilizationdesired.h>
-#include <accelstate.h>
-#include <pios_constants.h>
 #include <velocitystate.h>
 #include <positionstate.h>
 // Private constants
 
+
+#ifdef REVOLUTION
+
+#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_MIN        1.0e-6f
+#define UPDATE_MAX        1.0f
+#define UPDATE_ALPHA      1.0e-2f
+
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
 #define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
 
-#define STACK_SIZE_BYTES       1024
+#define STACK_SIZE_BYTES  512
-#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
 // Private types
 
 // Private variables
 static DelayedCallbackInfo *altitudeHoldCBInfo;
 static AltitudeHoldSettingsData altitudeHoldSettings;
 static struct pid pid0, pid1;
+static ThrustModeType thrustMode;
+static PiOSDeltatimeConfig timeval;
+static float thrustSetpoint = 0.0f;
+static float thrustDemand   = 0.0f;
+static float startThrust    = 0.5f;
 
 
 // Private functions
 static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
+static void VelocityStateUpdatedCb(UAVObjEvent *ev);
 
 /**
- * Initialise the module, called on startup
+ * Setup mode and setpoint
- * \returns 0 on success or -1 if initialisation failed
  */
-int32_t AltitudeHoldStart()
+float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
 {
-    // Start main task
+    static bool newaltitude = true;
-    SettingsUpdatedCb(NULL);
-    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
 
-    return 0;
+    if (reinit) {
+        startThrust = setpoint;
+        pid_zero(&pid0);
+        pid_zero(&pid1);
+        newaltitude = true;
+    }
+
+    const float DEADBAND      = 0.20f;
+    const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
+    const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
+
+    // this is the max speed in m/s at the extents of thrust
+    float thrustRate;
+    uint8_t thrustExp;
+
+    AltitudeHoldSettingsThrustExpGet(&thrustExp);
+    AltitudeHoldSettingsThrustRateGet(&thrustRate);
+
+    PositionStateData posState;
+    PositionStateGet(&posState);
+
+    if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
+        // Cut thrust if desired
+        thrustSetpoint = 0.0f;
+        thrustDemand   = 0.0f;
+        thrustMode     = DIRECT;
+        newaltitude    = true;
+    } else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
+        // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
+        // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
+        thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
+        thrustMode     = ALTITUDEVARIO;
+        newaltitude    = true;
+    } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
+        thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
+        thrustMode     = ALTITUDEVARIO;
+        newaltitude    = true;
+    } else if (newaltitude == true) {
+        thrustSetpoint = posState.Down;
+        thrustMode     = ALTITUDEHOLD;
+        newaltitude    = false;
+    }
+
+    return thrustDemand;
 }
 
 /**
  * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
  */
-int32_t AltitudeHoldInitialize()
+void stabilizationAltitudeloopInit()
 {
     AltitudeHoldSettingsInitialize();
-    AltitudeHoldDesiredInitialize();
     AltitudeHoldStatusInitialize();
+    PositionStateInitialize();
+    VelocityStateInitialize();
 
+    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
     // Create object queue
 
     altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
     AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
+    VelocityStateConnectCallback(&VelocityStateUpdatedCb);
 
-    return 0;
+    // Start main task
+    SettingsUpdatedCb(NULL);
 }
-MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
 
 
 /**
@@ -116,44 +149,25 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
  */
 static void altitudeHoldTask(void)
 {
-    static float startThrust = 0.5f;
-
-    // make sure we run only when we are supposed to run
-    FlightStatusData flightStatus;
-
-    FlightStatusGet(&flightStatus);
-    switch (flightStatus.FlightMode) {
-    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
-        break;
-    default:
-        pid_zero(&pid0);
-        pid_zero(&pid1);
-        StabilizationDesiredThrustGet(&startThrust);
-        PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-        return;
-
-        break;
-    }
-
     AltitudeHoldStatusData altitudeHoldStatus;
+
     AltitudeHoldStatusGet(&altitudeHoldStatus);
 
     // do the actual control loop(s)
-    AltitudeHoldDesiredData altitudeHoldDesired;
-    AltitudeHoldDesiredGet(&altitudeHoldDesired);
     float positionStateDown;
     PositionStateDownGet(&positionStateDown);
     float velocityStateDown;
     VelocityStateDownGet(&velocityStateDown);
 
-    switch (altitudeHoldDesired.ControlMode) {
+    float dT;
-    case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
+    dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+    switch (thrustMode) {
+    case ALTITUDEHOLD:
         // altitude control loop
-        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
+        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
         break;
-    case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
+    case ALTITUDEVARIO:
-        altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
+        altitudeHoldStatus.VelocityDesired = thrustSetpoint;
         break;
     default:
         altitudeHoldStatus.VelocityDesired = 0;
@@ -162,37 +176,16 @@ static void altitudeHoldTask(void)
 
     AltitudeHoldStatusSet(&altitudeHoldStatus);
 
-    float thrust;
+    switch (thrustMode) {
-    switch (altitudeHoldDesired.ControlMode) {
+    case DIRECT:
-    case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
+        thrustDemand = thrustSetpoint;
-        thrust = altitudeHoldDesired.SetPoint;
         break;
     default:
         // velocity control loop
-        thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
+        thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
 
-        if (thrust >= 1.0f) {
-            thrust = 1.0f;
-        }
-        if (thrust <= 0.0f) {
-            thrust = 0.0f;
-        }
         break;
     }
-
-    StabilizationDesiredData stab;
-    StabilizationDesiredGet(&stab);
-    stab.Roll   = altitudeHoldDesired.Roll;
-    stab.Pitch  = altitudeHoldDesired.Pitch;
-    stab.Yaw    = altitudeHoldDesired.Yaw;
-    stab.Thrust = thrust;
-    stab.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stab.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-
-    StabilizationDesiredSet(&stab);
-
-    PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 }
 
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
@@ -203,3 +196,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
     pid_zero(&pid1);
 }
+
+static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+}
+
+
+#endif /* ifdef REVOLUTION */

flight/modules/Stabilization/cruisecontrol.c

@@ -0,0 +1,293 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief cruisecontrol mode
+ * @note This file implements the logic for a cruisecontrol
+ * @{
+ *
+ * @file       cruisecontrol.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <openpilot.h>
+#include <stabilization.h>
+#include <attitudestate.h>
+#include <sin_lookup.h>
+
+static float cruisecontrol_factor = 1.0f;
+
+
+static inline float CruiseControlLimitThrust(float thrust)
+{
+    // limit to user specified absolute max thrust
+    return boundf(thrust, stabSettings.cruiseControl.min_thrust, stabSettings.cruiseControl.max_thrust);
+}
+
+// assumes 1.0 <= factor <= 100.0
+// a factor of less than 1.0 could make it return a value less than stabSettings.cruiseControl.min_thrust
+// CP helis need to have min_thrust=-1
+//
+// multicopters need to have min_thrust=0.05 or so
+// values below that will not be subject to max / min limiting
+// that means thrust can be less than min
+// that means multicopter motors stop spinning at low stick
+static inline float CruiseControlFactorToThrust(float factor, float thrust)
+{
+    // don't touch thrust if it's less than min_thrust
+    // without that	 test, quadcopter props will spin up
+    // to min thrust even at zero throttle stick
+    // if Cruise Control is enabled on this flight switch position
+    if (thrust > stabSettings.cruiseControl.min_thrust) {
+        return CruiseControlLimitThrust(thrust * factor);
+    }
+    return thrust;
+}
+
+static float CruiseControlAngleToFactor(float angle)
+{
+    float factor;
+
+    // avoid singularity
+    if (angle > 89.999f && angle < 90.001f) {
+        factor = stabSettings.settings.CruiseControlMaxPowerFactor;
+    } else {
+        // the simple bank angle boost calculation that Cruise Control revolves around
+        factor = 1.0f / fabsf(cos_lookup_deg(angle));
+        // factor in the power trim, no effect at 1.0, linear effect increases with factor
+        factor = (factor - 1.0f) * stabSettings.cruiseControl.power_trim + 1.0f;
+        // limit to user specified max power multiplier
+        if (factor > stabSettings.settings.CruiseControlMaxPowerFactor) {
+            factor = stabSettings.settings.CruiseControlMaxPowerFactor;
+        }
+    }
+    return factor;
+}
+
+
+void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand)
+{
+    static float previous_angle;
+    static uint32_t previous_time   = 0;
+    static bool previous_time_valid = false;
+
+    // For multiple, speedy flips this mainly strives to address the
+    // fact that (due to thrust delay) thrust didn't average straight
+    // down, but at an angle.  For less speedy flips it acts like it
+    // used to.  It can be turned off by setting power delay to 0.
+
+    // It takes significant time for the motors of a multi-copter to
+    // spin up.  It takes significant time for the collective servo of
+    // a CP heli to move from one end to the other.  Both of those are
+    // modeled here as linear, i.e. twice as much change takes twice
+    // as long. Given a correctly configured maximum delay time this
+    // code calculates how far in advance to start the control
+    // transition so that half way through the physical transition it
+    // is just crossing the transition angle.
+    // Example: Rotation rate = 360.  Full stroke delay = 0.2
+    // Transition angle 90 degrees.  Start the transition 0.1 second
+    // before 90 degrees (36 degrees at 360 deg/sec) and it will be
+    // complete 0.1 seconds after 90 degrees.
+
+    // Note that this code only handles the transition to/from inverted
+    // thrust.  It doesn't handle the case where thrust is changed a
+    // lot in a small angle range when that range is close to 90 degrees.
+    // It doesn't handle the small constant "system delay" caused by the
+    // delay between reading sensors and actuators beginning to respond.
+    // It also assumes that the pilot is holding the throttle constant;
+    // when the pilot does change the throttle, the compensation is
+    // simply recalculated.
+
+    // This implementation of future thrust isn't perfect.  That would
+    // probably require an iterative procedure for solving a
+    // transcendental equation of the form linear(x) = 1/cos(x).  It's
+    // shortcomings generally don't hurt anything and work better than
+    // without it.  It is designed to work perfectly if the pilot is
+    // using full thrust during flips and it is only activated if 70% or
+    // greater thrust is used.
+
+    uint32_t time = PIOS_DELAY_GetuS();
+
+    // Get roll and pitch angles, calculate combined angle, and begin
+    // the general algorithm.
+    // Example: 45 degrees roll plus 45 degrees pitch = 60 degrees
+    // Do it every 8th iteration to save CPU.
+    if (time != previous_time || previous_time_valid == false) {
+        float angle, angle_unmodified;
+
+        // spherical right triangle
+        // 0.0 <= angle <= 180.0
+        angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitude->Roll)
+                                                 * cos_lookup_deg(attitude->Pitch)));
+
+        // Calculate rate as a combined (roll and pitch) bank angle
+        // change; in degrees per second.  Rate is calculated over the
+        // most recent 8 loops through stabilization.  We could have
+        // asked the gyros.  This is probably cheaper.
+        if (previous_time_valid) {
+            float rate;
+
+            // rate can be negative.
+            rate = (angle - previous_angle) / ((float)(time - previous_time) / 1000000.0f);
+
+            // Define "within range" to be those transitions that should
+            // be executing now.  Recall that each impulse transition is
+            // spread out over a range of time / angle.
+
+            // There is only one transition and the high power level for
+            // it is either:
+            // 1/fabsf(cos(angle)) * current thrust
+            // or max power factor * current thrust
+            // or full thrust
+            // You can cross the transition with angle either increasing
+            // or decreasing (rate positive or negative).
+
+            // Thrust is never boosted for negative values of
+            // thrustDemand (negative stick values)
+            //
+            // When the aircraft is upright, thrust is always boosted
+            // . for positive values of thrustDemand
+            // When the aircraft is inverted, thrust is sometimes
+            // . boosted or reversed (or combinations thereof) or zeroed
+            // . for positive values of thrustDemand
+            // It depends on the inverted power settings.
+            // Of course, you can set MaxPowerFactor to 1.0 to
+            // . effectively disable boost.
+            if (thrustDemand > 0.0f) {
+                // to enable the future thrust calculations, make sure
+                // there is a large enough transition that the result
+                // will be roughly on vs. off; without that, it can
+                // exaggerate the length of time the inverted to upright
+                // transition holds full throttle and reduce the length
+                // of time for full throttle when going upright to inverted.
+                if (thrustDemand > 0.7f) {
+                    float thrust;
+
+                    thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand);
+
+                    // determine if we are in range of the transition
+
+                    // given the thrust at max_angle and thrustDemand
+                    // (typically close to 1.0), change variable 'thrust' to
+                    // be the proportion of the largest thrust change possible
+                    // that occurs when going into inverted mode.
+                    // Example: 'thrust' is 0.8  A quad has min_thrust set
+                    // to 0.05  The difference is 0.75.  The largest possible
+                    // difference with this setup is 0.9 - 0.05 = 0.85, so
+                    // the proportion is 0.75/0.85
+                    // That is nearly a full throttle stroke.
+                    // the 'thrust' variable is non-negative here
+                    switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
+                    case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
+                        // normal multi-copter case, stroke is max to zero
+                        // technically max to constant min_thrust
+                        // can be used by CP
+                        thrust = (thrust - CruiseControlLimitThrust(0.0f)) / stabSettings.cruiseControl.thrust_difference;
+                        break;
+                    case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
+                        // reversed but not boosted
+                        // : CP heli case, stroke is max to -stick
+                        // : thrust = (thrust - CruiseControlLimitThrust(-thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
+                        // else it is both unreversed and unboosted
+                        // : simply turn off boost, stroke is max to +stick
+                        // : thrust = (thrust - CruiseControlLimitThrust(thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
+                        thrust = (thrust - CruiseControlLimitThrust(
+                                      (stabSettings.settings.CruiseControlInvertedThrustReversing
+                                       == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED)
+                                      ? -thrustDemand
+                                      : thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
+                        break;
+                    case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
+                        // if boosted and reversed
+                        if (stabSettings.settings.CruiseControlInvertedThrustReversing
+                            == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
+                            // CP heli case, stroke is max to min
+                            thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
+                        }
+                        // else it is boosted and unreversed so the throttle doesn't change
+                        else {
+                            // CP heli case, no transition, so stroke is zero
+                            thrust = 0.0f;
+                        }
+                        break;
+                    }
+
+                    // 'thrust' is now the proportion of max stroke
+                    // multiply this proportion of max stroke,
+                    // times the max stroke time, to get this stroke time
+                    // we only want half of this time before the transition
+                    // (and half after the transition)
+                    thrust *= stabSettings.cruiseControl.half_power_delay;
+                    // 'thrust' is now the length of time for this stroke
+                    // multiply that times angular rate to get the lead angle
+                    thrust *= fabsf(rate);
+                    // if the transition is within range we use it,
+                    // else we just use the current calculated thrust
+                    if ((float)stabSettings.settings.CruiseControlMaxAngle - thrust <= angle
+                        && angle <= (float)stabSettings.settings.CruiseControlMaxAngle + thrust) {
+                        // default to a little above max angle
+                        angle = (float)stabSettings.settings.CruiseControlMaxAngle + 0.01f;
+                        // if roll direction is downward
+                        // then thrust value is taken from below max angle
+                        // by the code that knows about the transition angle
+                        if (rate < 0.0f) {
+                            angle -= 0.02f;
+                        }
+                    }
+                } // if thrust > 0.7; else just use the angle we already calculated
+                cruisecontrol_factor = CruiseControlAngleToFactor(angle);
+            } else { // if thrust > 0 set factor from angle; else
+                cruisecontrol_factor = 1.0f;
+            }
+
+            if (angle >= (float)stabSettings.settings.CruiseControlMaxAngle) {
+                switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
+                case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
+                    cruisecontrol_factor = 0.0f;
+                    break;
+                case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
+                    cruisecontrol_factor = 1.0f;
+                    break;
+                case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
+                    // no change, leave factor >= 1.0 alone
+                    break;
+                }
+                if (stabSettings.settings.CruiseControlInvertedThrustReversing
+                    == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
+                    cruisecontrol_factor = -cruisecontrol_factor;
+                }
+            }
+        } // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone
+        previous_time  = time;
+        previous_time_valid = true;
+        previous_angle = angle_unmodified;
+    } // every 8th time
+}
+
+float cruisecontrol_apply_factor(float raw)
+{
+    if (stabSettings.settings.CruiseControlMaxPowerFactor > 0.0001f) {
+        raw = CruiseControlFactorToThrust(cruisecontrol_factor, raw);
+    }
+    return raw;
+}

flight/modules/Stabilization/inc/altitudeloop.h

@@ -0,0 +1,41 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief altitudeloop mode
+ * @note This file implements the logic for a altitudeloop
+ * @{
+ *
+ * @file       altitudeloop.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @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 ALTITUDELOOP_H
+#define ALTITUDELOOP_H
+
+typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
+
+void stabilizationAltitudeloopInit();
+float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
+
+#endif /* ALTITUDELOOP_H */

flight/modules/Stabilization/inc/cruisecontrol.h

@@ -0,0 +1,42 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief cruisecontrol mode
+ * @note This file implements the logic for a cruisecontrol
+ * @{
+ *
+ * @file       cruisecontrol.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @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 CRUISECONTROL_H
+#define CRUISECONTROL_H
+
+#include <openpilot.h>
+#include <attitudestate.h>
+
+void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand);
+float cruisecontrol_apply_factor(float raw);
+
+#endif /* CRUISECONTROL_H */

flight/modules/Stabilization/inc/innerloop.h

@@ -0,0 +1,38 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief innerloop mode
+ * @note This file implements the logic for a innerloop
+ * @{
+ *
+ * @file       innerloop.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @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 INNERLOOP_H
+#define INNERLOOP_H
+
+void stabilizationInnerloopInit();
+
+#endif /* INNERLOOP_H */

flight/modules/Stabilization/inc/outerloop.h

@@ -0,0 +1,38 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief outerloop mode
+ * @note This file implements the logic for a outerloop
+ * @{
+ *
+ * @file       outerloop.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @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 OUTERLOOP_H
+#define OUTERLOOP_H
+
+void stabilizationOuterloopInit();
+
+#endif /* OUTERLOOP_H */

flight/modules/Stabilization/inc/stabilization.h

@@ -33,10 +33,52 @@
 #ifndef STABILIZATION_H
 #define STABILIZATION_H
 
-enum { ROLL, PITCH, YAW, MAX_AXES };
+#include <openpilot.h>
+#include <pid.h>
+#include <stabilizationsettings.h>
+#include <stabilizationbank.h>
+
 
 int32_t StabilizationInitialize();
 
+typedef struct {
+    StabilizationSettingsData settings;
+    StabilizationBankData     stabBank;
+    float gyro_alpha;
+    struct {
+        float min_thrust;
+        float max_thrust;
+        float thrust_difference;
+        float power_trim;
+        float half_power_delay;
+        float max_power_factor_angle;
+    } cruiseControl;
+    struct {
+        int8_t gyroupdates;
+        int8_t rateupdates;
+    }     monitor;
+    float rattitude_mode_transition_stick_position;
+    struct pid innerPids[3], outerPids[3];
+} StabilizationData;
+
+
+extern StabilizationData stabSettings;
+
+#define AXES                4
+#define FAILSAFE_TIMEOUT_MS 30
+
+#ifndef PIOS_STABILIZATION_STACK_SIZE
+#define STACK_SIZE_BYTES    800
+#else
+#define STACK_SIZE_BYTES    PIOS_STABILIZATION_STACK_SIZE
+#endif
+
+// must be same as eventdispatcher to avoid needing additional mutexes
+#define CBTASK_PRIORITY     CALLBACK_TASK_FLIGHTCONTROL
+
+// outer loop only executes every 4th uavobject update to safe CPU
+#define OUTERLOOP_SKIPCOUNT 4
+
 #endif // STABILIZATION_H
 
 /**

flight/modules/Stabilization/innerloop.c

@@ -0,0 +1,289 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief Stabilization PID loops in an airframe type independent manner
+ * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
+ * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
+ * @{
+ *
+ * @file       innerloop.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <openpilot.h>
+#include <pios_struct_helper.h>
+#include <pid.h>
+#include <callbackinfo.h>
+#include <ratedesired.h>
+#include <actuatordesired.h>
+#include <gyrostate.h>
+#include <airspeedstate.h>
+#include <stabilizationstatus.h>
+#include <flightstatus.h>
+#include <manualcontrolcommand.h>
+#include <stabilizationbank.h>
+
+#include <stabilization.h>
+#include <relay_tuning.h>
+#include <virtualflybar.h>
+#include <cruisecontrol.h>
+
+// Private constants
+
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
+
+#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_MIN        1.0e-6f
+#define UPDATE_MAX        1.0f
+#define UPDATE_ALPHA      1.0e-2f
+
+// Private variables
+static DelayedCallbackInfo *callbackHandle;
+static float gyro_filtered[3] = { 0, 0, 0 };
+static float axis_lock_accum[3] = { 0, 0, 0 };
+static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
+static PiOSDeltatimeConfig timeval;
+static float speedScaleFactor = 1.0f;
+
+// Private functions
+static void stabilizationInnerloopTask();
+static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+#ifdef REVOLUTION
+static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+#endif
+
+void stabilizationInnerloopInit()
+{
+    RateDesiredInitialize();
+    ActuatorDesiredInitialize();
+    GyroStateInitialize();
+    StabilizationStatusInitialize();
+    FlightStatusInitialize();
+    ManualControlCommandInitialize();
+#ifdef REVOLUTION
+    AirspeedStateInitialize();
+    AirspeedStateConnectCallback(AirSpeedUpdatedCb);
+#endif
+    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+
+    callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
+    GyroStateConnectCallback(GyroStateUpdatedCb);
+
+    // schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
+    PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
+}
+
+
+/**
+ * WARNING! This callback executes with critical flight control priority every
+ * time a gyroscope update happens do NOT put any time consuming calculations
+ * in this loop unless they really have to execute with every gyro update
+ */
+static void stabilizationInnerloopTask()
+{
+    // watchdog and error handling
+    {
+#ifdef PIOS_INCLUDE_WDG
+        PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
+#endif
+        bool warn  = false;
+        bool error = false;
+        bool crit  = false;
+        // check if outer loop keeps executing
+        if (stabSettings.monitor.rateupdates > -64) {
+            stabSettings.monitor.rateupdates--;
+        }
+        if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
+            // warning if rate loop skipped more than 2 execution
+            warn = true;
+        }
+        if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
+            // error if rate loop skipped more than 4 executions
+            error = true;
+        }
+        // check if gyro keeps updating
+        if (stabSettings.monitor.gyroupdates < 1) {
+            // critical if gyro didn't update at all!
+            crit = true;
+        }
+        if (stabSettings.monitor.gyroupdates > 1) {
+            // warning if we missed a gyro update
+            warn = true;
+        }
+        if (stabSettings.monitor.gyroupdates > 3) {
+            // error if we missed 3 gyro updates
+            error = true;
+        }
+        stabSettings.monitor.gyroupdates = 0;
+
+        if (crit) {
+            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
+        } else if (error) {
+            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
+        } else if (warn) {
+            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
+        } else {
+            AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
+        }
+    }
+
+
+    RateDesiredData rateDesired;
+    ActuatorDesiredData actuator;
+    StabilizationStatusInnerLoopData enabled;
+    FlightStatusControlChainData cchain;
+
+    RateDesiredGet(&rateDesired);
+    ActuatorDesiredGet(&actuator);
+    StabilizationStatusInnerLoopGet(&enabled);
+    FlightStatusControlChainGet(&cchain);
+    float *rate = &rateDesired.Roll;
+    float *actuatorDesiredAxis = &actuator.Roll;
+    int t;
+    float dT;
+    dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+
+    for (t = 0; t < AXES; t++) {
+        bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
+        previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
+
+        if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
+            if (reinit) {
+                stabSettings.innerPids[t].iAccumulator = 0;
+            }
+            switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
+            case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
+                stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
+                break;
+            case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
+                rate[t] = boundf(rate[t],
+                                 -cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
+                                 cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
+                                 );
+                stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
+                break;
+            case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
+                if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
+                    // While getting strong commands act like rate mode
+                    axis_lock_accum[t] = 0;
+                } else {
+                    // For weaker commands or no command simply attitude lock (almost) on no gyro change
+                    axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
+                    axis_lock_accum[t]  = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
+                    rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
+                }
+            // IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
+            // keep order as it is, RATE must follow!
+            case STABILIZATIONSTATUS_INNERLOOP_RATE:
+                // limit rate to maximum configured limits (once here instead of 5 times in outer loop)
+                rate[t] = boundf(rate[t],
+                                 -cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
+                                 cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
+                                 );
+                actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
+                break;
+            case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
+            default:
+                actuatorDesiredAxis[t] = rate[t];
+                break;
+            }
+        } else {
+            switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
+            case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
+                actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
+                break;
+            case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
+            default:
+                actuatorDesiredAxis[t] = rate[t];
+                break;
+            }
+        }
+
+        actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
+    }
+
+    actuator.UpdateTime = dT * 1000;
+
+    if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+        ActuatorDesiredSet(&actuator);
+    } else {
+        // Force all axes to reinitialize when engaged
+        for (t = 0; t < AXES; t++) {
+            previous_mode[t] = 255;
+        }
+    }
+    {
+        uint8_t armed;
+        FlightStatusArmedGet(&armed);
+        float throttleDesired;
+        ManualControlCommandThrottleGet(&throttleDesired);
+        if (armed != FLIGHTSTATUS_ARMED_ARMED ||
+            ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
+            // Force all axes to reinitialize when engaged
+            for (t = 0; t < AXES; t++) {
+                previous_mode[t] = 255;
+            }
+        }
+    }
+    PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
+}
+
+
+static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    GyroStateData gyroState;
+
+    GyroStateGet(&gyroState);
+
+    gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
+    gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
+    gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
+
+    PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
+    stabSettings.monitor.gyroupdates++;
+}
+
+#ifdef REVOLUTION
+static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
+    AirspeedStateData airspeedState;
+
+    AirspeedStateGet(&airspeedState);
+    if (stabSettings.settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
+        // feature has been turned off
+        speedScaleFactor = 1.0f;
+    } else {
+        // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
+        speedScaleFactor = boundf((stabSettings.settings.ScaleToAirspeed * stabSettings.settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed),
+                                  stabSettings.settings.ScaleToAirspeedLimits.Min,
+                                  stabSettings.settings.ScaleToAirspeedLimits.Max);
+    }
+}
+#endif
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Stabilization/outerloop.c

@@ -0,0 +1,288 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief Stabilization PID loops in an airframe type independent manner
+ * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
+ * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
+ * @{
+ *
+ * @file       outerloop.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Attitude stabilization module.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <openpilot.h>
+#include <pios_struct_helper.h>
+#include <pid.h>
+#include <callbackinfo.h>
+#include <ratedesired.h>
+#include <stabilizationdesired.h>
+#include <attitudestate.h>
+#include <stabilizationstatus.h>
+#include <flightstatus.h>
+#include <manualcontrolcommand.h>
+#include <stabilizationbank.h>
+
+
+#include <stabilization.h>
+#include <cruisecontrol.h>
+#include <altitudeloop.h>
+#include <CoordinateConversions.h>
+
+// Private constants
+
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
+
+#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_MIN        1.0e-6f
+#define UPDATE_MAX        1.0f
+#define UPDATE_ALPHA      1.0e-2f
+
+// Private variables
+static DelayedCallbackInfo *callbackHandle;
+static AttitudeStateData attitude;
+
+static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
+static PiOSDeltatimeConfig timeval;
+
+// Private functions
+static void stabilizationOuterloopTask();
+static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+
+void stabilizationOuterloopInit()
+{
+    RateDesiredInitialize();
+    StabilizationDesiredInitialize();
+    AttitudeStateInitialize();
+    StabilizationStatusInitialize();
+    FlightStatusInitialize();
+    ManualControlCommandInitialize();
+
+    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+
+    callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
+    AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
+}
+
+
+/**
+ * WARNING! This callback executes with critical flight control priority every
+ * time a gyroscope update happens do NOT put any time consuming calculations
+ * in this loop unless they really have to execute with every gyro update
+ */
+static void stabilizationOuterloopTask()
+{
+    AttitudeStateData attitudeState;
+    RateDesiredData rateDesired;
+    StabilizationDesiredData stabilizationDesired;
+    StabilizationStatusOuterLoopData enabled;
+
+    AttitudeStateGet(&attitudeState);
+    StabilizationDesiredGet(&stabilizationDesired);
+    RateDesiredGet(&rateDesired);
+    StabilizationStatusOuterLoopGet(&enabled);
+    float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
+    float *rateDesiredAxis = &rateDesired.Roll;
+    int t;
+    float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+
+    float local_error[3];
+    {
+#if defined(PIOS_QUATERNION_STABILIZATION)
+        // Quaternion calculation of error in each axis.  Uses more memory.
+        float rpy_desired[3];
+        float q_desired[4];
+        float q_error[4];
+
+        rpy_desired[0] = stabilizationDesiredAxis[0];
+        rpy_desired[1] = stabilizationDesiredAxis[1];
+        rpy_desired[2] = stabilizationDesiredAxis[2];
+
+        RPY2Quaternion(rpy_desired, q_desired);
+        quat_inverse(q_desired);
+        quat_mult(q_desired, &attitudeState.q1, q_error);
+        quat_inverse(q_error);
+        Quaternion2RPY(q_error, local_error);
+
+#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
+        // Simpler algorithm for CC, less memory
+        local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
+        local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
+        local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw;
+
+        // find shortest way
+        float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
+        if (modulo < 0) {
+            local_error[2] = modulo + 180.0f;
+        } else {
+            local_error[2] = modulo - 180.0f;
+        }
+#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
+    }
+    for (t = 0; t < AXES; t++) {
+        bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
+        previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
+
+        if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
+            if (reinit) {
+                stabSettings.outerPids[t].iAccumulator = 0;
+            }
+            switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
+            case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
+                rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
+                break;
+            case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
+            {
+                float stickinput[3];
+                stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+                stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+                stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+                float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
+                // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
+                rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
+                                            -cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t],
+                                            cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]
+                                            );
+                // Compute the weighted average rate desired
+                // Using max() rather than sqrt() for cpu speed;
+                // - this makes the stick region into a square;
+                // - this is a feature!
+                // - hold a roll angle and add just pitch without the stick sensitivity changing
+                float magnitude = fabsf(stickinput[t]);
+                if (t < 2) {
+                    magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
+                }
+
+                // modify magnitude to move the Att to Rate transition to the place
+                // specified by the user
+                // we are looking for where the stick angle == transition angle
+                // and the Att rate equals the Rate rate
+                // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
+                // == Rate x StickAngle [Rate pulling up according to stick angle]
+                // * StickAngle [X Ratt proportion]
+                // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
+                // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
+                // and quadratic formula says that is 0.618033989f
+                // I tested 14.01 and came up with .61 without even remembering this number
+                // I thought that moving the P,I, and maxangle terms around would change this value
+                // and that I would have to take these into account, but varying
+                // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
+                // and varying maxangle from 4 to 120 didn't either.
+                // so for now I'm not taking these into account
+                // while working with this, it occurred to me that Attitude mode,
+                // set up with maxangle=190 would be similar to Ratt, and it is.
+                #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
+
+                // the following assumes the transition would otherwise be at 0.618033989f
+                // and THAT assumes that Att ramps up to max roll rate
+                // when a small number of degrees off of where it should be
+
+                // if below the transition angle (still in attitude mode)
+                // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
+                if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
+                    magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
+                } else {
+                    magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
+                                * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
+                                / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+                                + STICK_VALUE_AT_MODE_TRANSITION;
+                }
+                rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
+            }
+            break;
+            case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
+                // FIXME: local_error[] is rate - attitude for Weak Leveling
+                // The only ramifications are:
+                // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
+                // Changing Rate mode max rate currently requires a change to Kp
+                // That would be changed to Attitude mode max angle affecting Kp
+                // Also does not take dT into account
+            {
+                float rate_input    = cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabSettings.stabBank, stabSettings.stabBank.RollMax)[t];
+                float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
+                weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
+
+                // Compute desired rate as input biased towards leveling
+                rateDesiredAxis[t] = rate_input + weak_leveling;
+            }
+            break;
+            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+            default:
+                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
+                break;
+            }
+        } else {
+            switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
+#ifdef REVOLUTION
+            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
+                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
+                break;
+            case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY:
+                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
+                break;
+#endif /* REVOLUTION */
+            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+            default:
+                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
+                break;
+            }
+        }
+    }
+
+    RateDesiredSet(&rateDesired);
+    {
+        uint8_t armed;
+        FlightStatusArmedGet(&armed);
+        float throttleDesired;
+        ManualControlCommandThrottleGet(&throttleDesired);
+        if (armed != FLIGHTSTATUS_ARMED_ARMED ||
+            ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
+            // Force all axes to reinitialize when engaged
+            for (t = 0; t < AXES; t++) {
+                previous_mode[t] = 255;
+            }
+        }
+    }
+
+    // update cruisecontrol based on attitude
+    cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
+    stabSettings.monitor.rateupdates = 0;
+}
+
+
+static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    // to reduce CPU utilisation, outer loop is not executed every state update
+    static uint8_t cpusafer = 0;
+
+    if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) {
+        // this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
+        AttitudeStateGet(&attitude);
+        PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
+    }
+}
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Stabilization/relay_tuning.c

@@ -63,7 +63,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
 
     portTickType thisTime = xTaskGetTickCount();
 
-    static bool rateRelayRunning[MAX_AXES];
+    static bool rateRelayRunning[3];
 
     // This indicates the current estimate of the smoothed error.  So when it is high
     // we are waiting for it to go low.

flight/modules/Stabilization/stabilization.c

@@ -33,128 +33,55 @@
 
 #include <openpilot.h>
 #include <pios_struct_helper.h>
-#include "stabilization.h"
+#include <pid.h>
-#include "stabilizationsettings.h"
+#include <manualcontrolcommand.h>
-#include "stabilizationbank.h"
+#include <flightmodesettings.h>
-#include "stabilizationsettingsbank1.h"
+#include <stabilizationsettings.h>
-#include "stabilizationsettingsbank2.h"
+#include <stabilizationdesired.h>
-#include "stabilizationsettingsbank3.h"
+#include <stabilizationstatus.h>
-#include "actuatordesired.h"
+#include <stabilizationbank.h>
-#include "ratedesired.h"
+#include <stabilizationsettingsbank1.h>
-#include "relaytuning.h"
+#include <stabilizationsettingsbank2.h>
-#include "relaytuningsettings.h"
+#include <stabilizationsettingsbank3.h>
-#include "stabilizationdesired.h"
+#include <relaytuning.h>
-#include "attitudestate.h"
+#include <relaytuningsettings.h>
-#include "airspeedstate.h"
+#include <ratedesired.h>
-#include "gyrostate.h"
+#include <sin_lookup.h>
-#include "flightstatus.h"
+#include <stabilization.h>
-#include "manualcontrolsettings.h"
+#include <innerloop.h>
-#include "manualcontrolcommand.h"
+#include <outerloop.h>
-#include "flightmodesettings.h"
+#include <altitudeloop.h>
-#include "taskinfo.h"
 
-// Math libraries
-#include "CoordinateConversions.h"
-#include "pid.h"
-#include "sin_lookup.h"
 
-// Includes for various stabilization algorithms
+// Public variables
-#include "relay_tuning.h"
+StabilizationData stabSettings;
-#include "virtualflybar.h"
-
-// Includes for various stabilization algorithms
-#include "relay_tuning.h"
-
-// Private constants
-#define UPDATE_EXPECTED     (1.0f / 666.0f)
-#define UPDATE_MIN          1.0e-6f
-#define UPDATE_MAX          1.0f
-#define UPDATE_ALPHA        1.0e-2f
-
-#define MAX_QUEUE_SIZE      1
-
-#if defined(PIOS_STABILIZATION_STACK_SIZE)
-#define STACK_SIZE_BYTES    PIOS_STABILIZATION_STACK_SIZE
-#else
-#define STACK_SIZE_BYTES    860
-#endif
-
-#define TASK_PRIORITY       (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
-#define FAILSAFE_TIMEOUT_MS 30
-
-// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
-// The PID_RATE set is used by the attitude portion of Attitude mode
-enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
-enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
-enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
 
 // Private variables
-static xTaskHandle taskHandle;
+static int cur_flight_mode = -1;
-static StabilizationSettingsData settings;
-static xQueueHandle queue;
-float gyro_alpha = 0;
-float axis_lock_accum[3] = { 0, 0, 0 };
-uint8_t max_axis_lock     = 0;
-uint8_t max_axislock_rate = 0;
-float weak_leveling_kp    = 0;
-uint8_t weak_leveling_max = 0;
-bool lowThrottleZeroIntegral;
-float vbar_decay    = 0.991f;
-struct pid pids[PID_MAX];
-
-int cur_flight_mode = -1;
-
-static float rattitude_mode_transition_stick_position;
-
-static float   cruise_control_min_thrust;
-static float   cruise_control_max_thrust;
-static float   cruise_control_thrust_difference;
-static float   cruise_control_max_angle;
-static float   cruise_control_max_power_factor;
-static float   cruise_control_power_trim;
-static float   cruise_control_max_power_factor_angle;
-static float   cruise_control_half_power_delay;
-static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
-static uint8_t cruise_control_inverted_thrust_reversing;
-static uint8_t cruise_control_inverted_power_output;
 
 // Private functions
-static void stabilizationTask(void *parameters);
-static float bound(float val, float range);
-static void ZeroPids(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
 static void BankUpdatedCb(UAVObjEvent *ev);
 static void SettingsBankUpdatedCb(UAVObjEvent *ev);
-static float CruiseControlAngleToFactor(float angle);
+static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
-static float CruiseControlFactorToThrust(float factor, float stick_thrust);
+static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
-static float CruiseControlLimitThrust(float thrust);
 
 /**
  * Module initialization
  */
 int32_t StabilizationStart()
 {
-    // Initialize variables
-    // Create object queue
-    queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
-
-    // Listen for updates.
-    // AttitudeStateConnectQueue(queue);
-    GyroStateConnectQueue(queue);
-
     StabilizationSettingsConnectCallback(SettingsUpdatedCb);
-    SettingsUpdatedCb(StabilizationSettingsHandle());
+    ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
-
     StabilizationBankConnectCallback(BankUpdatedCb);
-
     StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
     StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
     StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
+    StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
+    SettingsUpdatedCb(StabilizationSettingsHandle());
+    StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
+    FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
 
-
-    // Start main task
-    xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
 #ifdef PIOS_INCLUDE_WDG
     PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
 #endif
@@ -167,875 +94,189 @@ int32_t StabilizationStart()
 int32_t StabilizationInitialize()
 {
     // Initialize variables
-    ManualControlCommandInitialize();
-    ManualControlSettingsInitialize();
-    FlightStatusInitialize();
     StabilizationDesiredInitialize();
     StabilizationSettingsInitialize();
+    StabilizationStatusInitialize();
     StabilizationBankInitialize();
     StabilizationSettingsBank1Initialize();
     StabilizationSettingsBank2Initialize();
     StabilizationSettingsBank3Initialize();
-    ActuatorDesiredInitialize();
-#ifdef DIAG_RATEDESIRED
     RateDesiredInitialize();
-#endif
+    ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
-#ifdef REVOLUTION
-    AirspeedStateInitialize();
-#endif
     // Code required for relay tuning
     sin_lookup_initalize();
     RelayTuningSettingsInitialize();
     RelayTuningInitialize();
 
+    stabilizationOuterloopInit();
+    stabilizationInnerloopInit();
+#ifdef REVOLUTION
+    stabilizationAltitudeloopInit();
+#endif
+    pid_zero(&stabSettings.outerPids[0]);
+    pid_zero(&stabSettings.outerPids[1]);
+    pid_zero(&stabSettings.outerPids[2]);
+    pid_zero(&stabSettings.innerPids[0]);
+    pid_zero(&stabSettings.innerPids[1]);
+    pid_zero(&stabSettings.innerPids[2]);
     return 0;
 }
 
 MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
 
-/**
+static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
- * Module task
- */
-static void stabilizationTask(__attribute__((unused)) void *parameters)
 {
-    UAVObjEvent ev;
+    StabilizationStatusData status;
-    PiOSDeltatimeConfig timeval;
+    StabilizationDesiredStabilizationModeData mode;
+    int t;
 
-    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+    StabilizationDesiredStabilizationModeGet(&mode);
-
+    for (t = 0; t < AXES; t++) {
-    ActuatorDesiredData actuatorDesired;
+        switch (cast_struct_to_array(mode, mode.Roll)[t]) {
-    StabilizationDesiredData stabDesired;
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
-    float throttleDesired;
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
-    RateDesiredData rateDesired;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
-    AttitudeStateData attitudeState;
+            break;
-    GyroStateData gyroStateData;
-    FlightStatusData flightStatus;
-    StabilizationBankData stabBank;
-
-
-#ifdef REVOLUTION
-    AirspeedStateData airspeedState;
-#endif
-
-    SettingsUpdatedCb((UAVObjEvent *)NULL);
-
-    // Main task loop
-    ZeroPids();
-    while (1) {
-        float dT;
-
-#ifdef PIOS_INCLUDE_WDG
-        PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
-#endif
-
-        // Wait until the Gyro object is updated, if a timeout then go to failsafe
-        if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) {
-            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
-            continue;
-        }
-
-        dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
-        FlightStatusGet(&flightStatus);
-        StabilizationDesiredGet(&stabDesired);
-        ManualControlCommandThrottleGet(&throttleDesired);
-        AttitudeStateGet(&attitudeState);
-        GyroStateGet(&gyroStateData);
-        StabilizationBankGet(&stabBank);
-#ifdef DIAG_RATEDESIRED
-        RateDesiredGet(&rateDesired);
-#endif
-        uint8_t flight_mode_switch_position;
-        ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
-
-        if (cur_flight_mode != flight_mode_switch_position) {
-            cur_flight_mode = flight_mode_switch_position;
-            SettingsBankUpdatedCb(NULL);
-        }
-
-#ifdef REVOLUTION
-        float speedScaleFactor;
-        // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
-        AirspeedStateGet(&airspeedState);
-        if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
-            // feature has been turned off
-            speedScaleFactor = 1.0f;
-        } else {
-            // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
-            speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
-            if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) {
-                speedScaleFactor = settings.ScaleToAirspeedLimits.Min;
-            }
-            if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) {
-                speedScaleFactor = settings.ScaleToAirspeedLimits.Max;
-            }
-        }
-#else
-        const float speedScaleFactor = 1.0f;
-#endif
-
-#if defined(PIOS_QUATERNION_STABILIZATION)
-        // Quaternion calculation of error in each axis.  Uses more memory.
-        float rpy_desired[3];
-        float q_desired[4];
-        float q_error[4];
-        float local_error[3];
-
-        // Essentially zero errors for anything in rate or none
-        if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
-            rpy_desired[0] = stabDesired.Roll;
-        } else {
-            rpy_desired[0] = attitudeState.Roll;
-        }
-
-        if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
-            rpy_desired[1] = stabDesired.Pitch;
-        } else {
-            rpy_desired[1] = attitudeState.Pitch;
-        }
-
-        if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
-            rpy_desired[2] = stabDesired.Yaw;
-        } else {
-            rpy_desired[2] = attitudeState.Yaw;
-        }
-
-        RPY2Quaternion(rpy_desired, q_desired);
-        quat_inverse(q_desired);
-        quat_mult(q_desired, &attitudeState.q1, q_error);
-        quat_inverse(q_error);
-        Quaternion2RPY(q_error, local_error);
-
-#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
-        // Simpler algorithm for CC, less memory
-        float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
-                                 stabDesired.Pitch - attitudeState.Pitch,
-                                 stabDesired.Yaw - attitudeState.Yaw };
-        // find shortest way
-        float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
-        if (modulo < 0) {
-            local_error[2] = modulo + 180.0f;
-        } else {
-            local_error[2] = modulo - 180.0f;
-        }
-#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
-
-        float gyro_filtered[3];
-        gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha);
-        gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
-        gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha);
-
-        float *stabDesiredAxis     = &stabDesired.Roll;
-        float *actuatorDesiredAxis = &actuatorDesired.Roll;
-        float *rateDesiredAxis     = &rateDesired.Roll;
-
-        ActuatorDesiredGet(&actuatorDesired);
-
-        // A flag to track which stabilization mode each axis is in
-        static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 };
-        bool error = false;
-
-        // Run the selected stabilization algorithm on each axis:
-        for (uint8_t i = 0; i < MAX_AXES; i++) {
-            // Check whether this axis mode needs to be reinitialized
-            bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]);
-            previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i];
-
-            // Apply the selected control law
-            switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) {
         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
-                if (reinit) {
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
-                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
-                }
-
-                // Store to rate desired variable for storing to UAVO
-                rateDesiredAxis[i] =
-                    bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
-
-                // Compute the inner loop
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
             break;
-
         case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
-                if (reinit) {
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
-                    pids[PID_ROLL + i].iAccumulator = 0;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
-                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
-                }
-
-                // Compute the outer loop
-                rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
-                rateDesiredAxis[i] = bound(rateDesiredAxis[i],
-                                           cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
-
-                // Compute the inner loop
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
             break;
-
-            case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
-                // A parameterization from Attitude mode at center stick
-                // - to Rate mode at full stick
-                // This is done by parameterizing to use the rotation rate that Attitude mode
-                // - would use at center stick to using the rotation rate that Rate mode
-                // - would use at full stick in a weighted average sort of way.
-            {
-                if (reinit) {
-                    pids[PID_ROLL + i].iAccumulator = 0;
-                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
-                }
-
-                // Compute what Rate mode would give for this stick angle's rate
-                // Save Rate's rate in a temp for later merging with Attitude's rate
-                float rateDesiredAxisRate;
-                rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
-                                      * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
-
-                // Compute what Attitude mode would give for this stick angle's rate
-
-                // stabDesired for this mode is [-1.0f,+1.0f]
-                // - multiply by Attitude mode max angle to get desired angle
-                // - subtract off the actual angle to get the angle error
-                // This is what local_error[] holds for Attitude mode
-                float attitude_error = stabDesiredAxis[i]
-                                       * cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i]
-                                       - cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
-
-                // Compute the outer loop just like Attitude mode does
-                float rateDesiredAxisAttitude;
-                rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
-                rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
-                                                cast_struct_to_array(stabBank.ManualRate,
-                                                                     stabBank.ManualRate.Roll)[i]);
-
-                // Compute the weighted average rate desired
-                // Using max() rather than sqrt() for cpu speed;
-                // - this makes the stick region into a square;
-                // - this is a feature!
-                // - hold a roll angle and add just pitch without the stick sensitivity changing
-                // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
-                float magnitude;
-                magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
-
-                // modify magnitude to move the Att to Rate transition to the place
-                // specified by the user
-                // we are looking for where the stick angle == transition angle
-                // and the Att rate equals the Rate rate
-                // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
-                // == Rate x StickAngle [Rate pulling up according to stick angle]
-                // * StickAngle [X Ratt proportion]
-                // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
-                // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
-                // and quadratic formula says that is 0.618033989f
-                // I tested 14.01 and came up with .61 without even remembering this number
-                // I thought that moving the P,I, and maxangle terms around would change this value
-                // and that I would have to take these into account, but varying
-                // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
-                // and varying maxangle from 4 to 120 didn't either.
-                // so for now I'm not taking these into account
-                // while working with this, it occurred to me that Attitude mode,
-                // set up with maxangle=190 would be similar to Ratt, and it is.
-                #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
-
-                // the following assumes the transition would otherwise be at 0.618033989f
-                // and THAT assumes that Att ramps up to max roll rate
-                // when a small number of degrees off of where it should be
-
-                // if below the transition angle (still in attitude mode)
-                // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
-                if (magnitude <= rattitude_mode_transition_stick_position) {
-                    magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
-                } else {
-                    magnitude = (magnitude - rattitude_mode_transition_stick_position)
-                                * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
-                                / (1.0f - rattitude_mode_transition_stick_position)
-                                + STICK_VALUE_AT_MODE_TRANSITION;
-                }
-                rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
-                                     + magnitude * rateDesiredAxisRate;
-
-                // Compute the inner loop for the averaged rate
-                // actuatorDesiredAxis[i] is the weighted average
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
-                                                            rateDesiredAxis[i], gyro_filtered[i], dT);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
-                break;
-            }
-
-            case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
-                // Store for debugging output
-                rateDesiredAxis[i] = stabDesiredAxis[i];
-
-                // Run a virtual flybar stabilization algorithm on this axis
-                stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
-
-                break;
-
-            case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
-                // FIXME: local_error[] is rate - attitude for Weak Leveling
-                // The only ramifications are:
-                // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
-                // Changing Rate mode max rate currently requires a change to Kp
-                // That would be changed to Attitude mode max angle affecting Kp
-                // Also does not take dT into account
-            {
-                if (reinit) {
-                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
-                }
-
-                float weak_leveling = local_error[i] * weak_leveling_kp;
-                weak_leveling = bound(weak_leveling, weak_leveling_max);
-
-                // Compute desired rate as input biased towards leveling
-                rateDesiredAxis[i]     = stabDesiredAxis[i] + weak_leveling;
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
-                break;
-            }
-
         case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
-                if (reinit) {
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
-                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
-                }
+            break;
-
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
-                if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
-                    // While getting strong commands act like rate mode
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
-                    rateDesiredAxis[i] = stabDesiredAxis[i];
+            break;
-                    axis_lock_accum[i] = 0;
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
-                } else {
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
-                    // For weaker commands or no command simply attitude lock (almost) on no gyro change
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
-                    axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
+            break;
-                    axis_lock_accum[i]  = bound(axis_lock_accum[i], max_axis_lock);
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
-                    rateDesiredAxis[i]  = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
-                }
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
-
-                rateDesiredAxis[i]     = bound(rateDesiredAxis[i],
-                                               cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
-
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
             break;
-
         case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
-                // Store to rate desired variable for storing to UAVO
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
-                rateDesiredAxis[i] = bound(stabDesiredAxis[i],
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
-                                           cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
-
-                // Run the relay controller which also estimates the oscillation parameters
-                stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
             break;
-
         case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
-                if (reinit) {
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
-                    pids[PID_ROLL + i].iAccumulator = 0;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
-                }
-
-                // Compute the outer loop like attitude mode
-                rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
-                rateDesiredAxis[i] = bound(rateDesiredAxis[i],
-                                           cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
-
-                // Run the relay controller which also estimates the oscillation parameters
-                stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
-                actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
-
             break;
-
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
-            case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
-                actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
             break;
-            default:
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY:
-                error = true;
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
+            break;
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
+            cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
+            cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
             break;
         }
     }
-
+    StabilizationStatusSet(&status);
-        if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) {
-            stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
-        }
-
-#ifdef DIAG_RATEDESIRED
-        RateDesiredSet(&rateDesired);
-#endif
-
-        // Save dT
-        actuatorDesired.UpdateTime = dT * 1000;
-        actuatorDesired.Thrust     = stabDesired.Thrust;
-
-        ///////////////////////////////////////////////////////////////////////
-        // Cruise Control
-        // modify thrust according to 1/cos(bank angle)
-        // to maintain same altitude with changing bank angle
-        // but without manually adjusting thrust
-        // do it here and all the various flight modes (e.g. Altitude Hold) can use it
-        ///////////////////////////////////////////////////////////////////////
-
-        // Detect if the flight mode switch has changed.  If it has, there
-        // could be a time gap.  E.g. enabled, then disabled for 30 seconds
-        // then enabled again.  Previous_angle will also be invalid because
-        // of the time spent with Cruise Control off.
-        static bool previous_time_valid; // initially false
-        static uint8_t previous_flight_mode_switch_position = 250;
-        if (flight_mode_switch_position != previous_flight_mode_switch_position) {
-            previous_flight_mode_switch_position = flight_mode_switch_position;
-            // Force calculations on this pass (usually every 8th pass),
-            // but ignore rate calculations (uses time, previous_time, angle,
-            // previous_angle)
-            previous_time_valid = false;
-        }
-
-        if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
-            && cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
-            && cruise_control_max_power_factor > 0.0001f) {
-            static float factor;
-            static float previous_angle;
-            static uint32_t previous_time;
-            static uint8_t calc_count;
-            uint32_t time;
-
-            // For multiple, speedy flips this mainly strives to address the
-            // fact that (due to thrust delay) thrust didn't average straight
-            // down, but at an angle.  For less speedy flips it acts like it
-            // used to.  It can be turned off by setting power delay to 0.
-
-            // It takes significant time for the motors of a multi-copter to
-            // spin up.  It takes significant time for the collective servo of
-            // a CP heli to move from one end to the other.  Both of those are
-            // modeled here as linear, i.e. twice as much change takes twice
-            // as long. Given a correctly configured maximum delay time this
-            // code calculates how far in advance to start the control
-            // transition so that half way through the physical transition it
-            // is just crossing the transition angle.
-            // Example: Rotation rate = 360.  Full stroke delay = 0.2
-            // Transition angle 90 degrees.  Start the transition 0.1 second
-            // before 90 degrees (36 degrees at 360 deg/sec) and it will be
-            // complete 0.1 seconds after 90 degrees.
-
-            // Note that this code only handles the transition to/from inverted
-            // thrust.  It doesn't handle the case where thrust is changed a
-            // lot in a small angle range when that range is close to 90 degrees.
-            // It doesn't handle the small constant "system delay" caused by the
-            // delay between reading sensors and actuators beginning to respond.
-            // It also assumes that the pilot is holding the throttle constant;
-            // when the pilot does change the throttle, the compensation is
-            // simply recalculated.
-
-            // This implementation of future thrust isn't perfect.  That would
-            // probably require an iterative procedure for solving a
-            // transcendental equation of the form linear(x) = 1/cos(x).  It's
-            // shortcomings generally don't hurt anything and work better than
-            // without it.  It is designed to work perfectly if the pilot is
-            // using full thrust during flips and it is only activated if 70% or
-            // greater thrust is used.
-
-            time = PIOS_DELAY_GetuS();
-
-            // Get roll and pitch angles, calculate combined angle, and begin
-            // the general algorithm.
-            // Example: 45 degrees roll plus 45 degrees pitch = 60 degrees
-            // Do it every 8th iteration to save CPU.
-            if ((time != previous_time && calc_count++ >= 8) || previous_time_valid == false) {
-                float angle, angle_unmodified;
-
-                calc_count = 0;
-
-                // spherical right triangle
-                // 0.0 <= angle <= 180.0
-                angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll)
-                                                   * cos_lookup_deg(attitudeState.Pitch)));
-
-                // Calculate rate as a combined (roll and pitch) bank angle
-                // change; in degrees per second.  Rate is calculated over the
-                // most recent 8 loops through stabilization.  We could have
-                // asked the gyros.  This is probably cheaper.
-                if (previous_time_valid) {
-                    float rate;
-
-                    // rate can be negative.
-                    rate = (angle - previous_angle) / ((float) (time - previous_time) / 1000000.0f);
-
-                    // Define "within range" to be those transitions that should
-                    // be executing now.  Recall that each impulse transition is
-                    // spread out over a range of time / angle.
-
-                    // There is only one transition and the high power level for
-                    // it is either:
-                    // 1/fabsf(cos(angle)) * current thrust
-                    // or max power factor * current thrust
-                    // or full thrust
-                    // You can cross the transition with angle either increasing
-                    // or decreasing (rate positive or negative).
-
-                    // Thrust is never boosted for negative values of
-                    // actuatorDesired.Thrust (negative stick values)
-                    //
-                    // When the aircraft is upright, thrust is always boosted
-                    // . for positive values of actuatorDesired.Thrust
-                    // When the aircraft is inverted, thrust is sometimes
-                    // . boosted or reversed (or combinations thereof) or zeroed
-                    // . for positive values of actuatorDesired.Thrust
-                    // It depends on the inverted power settings.
-                    // Of course, you can set MaxPowerFactor to 1.0 to
-                    // . effectively disable boost.
-                    if (actuatorDesired.Thrust > 0.0f) {
-                        // to enable the future thrust calculations, make sure
-                        // there is a large enough transition that the result
-                        // will be roughly on vs. off; without that, it can
-                        // exaggerate the length of time the inverted to upright
-                        // transition holds full throttle and reduce the length
-                        // of time for full throttle when going upright to inverted.
-                        if (actuatorDesired.Thrust > 0.7f) {
-                            float thrust;
-
-                            thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust);
-
-                            // determine if we are in range of the transition
-
-                            // given the thrust at max_angle and actuatorDesired.Thrust
-                            // (typically close to 1.0), change variable 'thrust' to
-                            // be the proportion of the largest thrust change possible
-                            // that occurs when going into inverted mode.
-                            // Example: 'thrust' is 0.8  A quad has min_thrust set
-                            // to 0.05  The difference is 0.75.  The largest possible
-                            // difference with this setup is 0.9 - 0.05 = 0.85, so
-                            // the proportion is 0.75/0.85
-                            // That is nearly a full throttle stroke.
-                            // the 'thrust' variable is non-negative here
-                            switch (cruise_control_inverted_power_output) {
-                            case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
-                                // normal multi-copter case, stroke is max to zero
-                                // technically max to constant min_thrust
-                                // can be used by CP
-                                thrust = (thrust - CruiseControlLimitThrust(0.0f)) / cruise_control_thrust_difference;
-                                break;
-                            case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
-                                // reversed but not boosted
-                                // : CP heli case, stroke is max to -stick
-                                // : thrust = (thrust - CruiseControlLimitThrust(-actuatorDesired.Thrust)) / cruise_control_thrust_difference;
-                                // else it is both unreversed and unboosted
-                                // : simply turn off boost, stroke is max to +stick
-                                // : thrust = (thrust - CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference;
-                                thrust = (thrust - CruiseControlLimitThrust(
-                                    (cruise_control_inverted_thrust_reversing
-                                    == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED)
-                                    ? -actuatorDesired.Thrust
-                                    : actuatorDesired.Thrust)) / cruise_control_thrust_difference;
-                                break;
-                            case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
-                                // if boosted and reversed
-                                if (cruise_control_inverted_thrust_reversing
-                                    == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
-                                    // CP heli case, stroke is max to min
-                                    thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust)) / cruise_control_thrust_difference;
-                                }
-                                // else it is boosted and unreversed so the throttle doesn't change
-                                else {
-                                    // CP heli case, no transition, so stroke is zero
-                                    thrust = 0.0f;
-                                }
-                                break;
-                            }
-
-                            // 'thrust' is now the proportion of max stroke
-                            // multiply this proportion of max stroke,
-                            // times the max stroke time, to get this stroke time
-                            // we only want half of this time before the transition
-                            // (and half after the transition)
-                            thrust *= cruise_control_half_power_delay;
-                            // 'thrust' is now the length of time for this stroke
-                            // multiply that times angular rate to get the lead angle
-                            thrust *= fabsf(rate);
-                            // if the transition is within range we use it,
-                            // else we just use the current calculated thrust
-                            if (cruise_control_max_angle - thrust <= angle
-                                && angle <= cruise_control_max_angle + thrust) {
-                                // default to a little above max angle
-                                angle = cruise_control_max_angle + 0.01f;
-                                // if roll direction is downward
-                                // then thrust value is taken from below max angle
-                                // by the code that knows about the transition angle
-                                if (rate < 0.0f) {
-                                    angle -= 0.02f;
-                                }
-                            }
-                        } // if thrust > 0.7; else just use the angle we already calculated
-                        factor = CruiseControlAngleToFactor(angle);
-                    } else { // if thrust > 0 set factor from angle; else
-                        factor = 1.0f;
-                    }
-
-                    if (angle >= cruise_control_max_angle) {
-                        switch (cruise_control_inverted_power_output) {
-                        case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
-                            factor = 0.0f;
-                            break;
-                        case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
-                            factor = 1.0f;
-                            break;
-                        case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
-                            // no change, leave factor >= 1.0 alone
-                            break;
-                        }
-                        if (cruise_control_inverted_thrust_reversing
-                            == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
-                            factor = -factor;
-                        }
-                    }
-                } // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone
-                previous_time       = time;
-                previous_time_valid = true;
-                previous_angle      = angle_unmodified;
-            } // every 8th time
-
-            // don't touch thrust if it's less than min_thrust
-            // without that	 test, quadcopter props will spin up
-            // to min thrust even at zero throttle stick
-            actuatorDesired.Thrust = CruiseControlFactorToThrust(factor, actuatorDesired.Thrust);
-        } // if Cruise Control is enabled on this flight switch position
-
-        if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
-            ActuatorDesiredSet(&actuatorDesired);
-        } else {
-            // Force all axes to reinitialize when engaged
-            for (uint8_t i = 0; i < MAX_AXES; i++) {
-                previous_mode[i] = 255;
-            }
-        }
-
-        if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
-            (lowThrottleZeroIntegral && throttleDesired < 0)) {
-            // Force all axes to reinitialize when engaged
-            for (uint8_t i = 0; i < MAX_AXES; i++) {
-                previous_mode[i] = 255;
-            }
-        }
-
-        // Clear or set alarms.  Done like this to prevent toggline each cycle
-        // and hammering system alarms
-        if (error) {
-            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
-        } else {
-            AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
-        }
-    }
 }
 
-
+static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-/**
- * Clear the accumulators and derivatives for all the axes
- */
-static void ZeroPids(void)
 {
-    for (uint32_t i = 0; i < PID_MAX; i++) {
+    uint8_t fm;
-        pid_zero(&pids[i]);
-    }
 
+    ManualControlCommandFlightModeSwitchPositionGet(&fm);
 
-    for (uint8_t i = 0; i < 3; i++) {
+    if (fm == cur_flight_mode) {
-        axis_lock_accum[i] = 0.0f;
+        return;
     }
+    cur_flight_mode = fm;
+    SettingsBankUpdatedCb(NULL);
 }
 
-
-/**
- * Bound input value between limits
- */
-static float bound(float val, float range)
-{
-    if (val < -range) {
-        return -range;
-    } else if (val > range) {
-        return range;
-    }
-    return val;
-}
-
-
 static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
         return;
     }
-    if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
+    if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
-                 (settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
+                 (stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
-                 (settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
+                 (stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
-                 settings.FlightModeMap[cur_flight_mode] > 2)) {
+                 stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
         return;
     }
 
-    StabilizationBankData bank;
 
-    switch (settings.FlightModeMap[cur_flight_mode]) {
+    switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
     case 0:
-        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
+        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
         break;
 
     case 1:
-        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
+        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
         break;
 
     case 2:
-        StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
+        StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
         break;
     }
-    StabilizationBankSet(&bank);
+    StabilizationBankSet(&stabSettings.stabBank);
 }
 
 static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    StabilizationBankData bank;
+    StabilizationBankGet(&stabSettings.stabBank);
-
-    StabilizationBankGet(&bank);
-
-// this code will be needed if any other modules alter stabilizationbank
-/*
-    StabilizationBankData curBank;
-    if(flight_mode < 0) return;
-
-    switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
-    {
-    case 0:
-        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
-        if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
-        {
-            StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
-        }
-        break;
-
-    case 1:
-        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
-        if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
-        {
-            StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
-        }
-        break;
-
-    case 2:
-        StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
-        if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
-        {
-            StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
-        }
-        break;
-
-    default:
-        return; //invalid bank number
-    }
- */
-
 
     // Set the roll rate PID constants
-    pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
+    pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
-                  bank.RollRatePID.Ki,
+                  stabSettings.stabBank.RollRatePID.Ki,
-                  bank.RollRatePID.Kd,
+                  stabSettings.stabBank.RollRatePID.Kd,
-                  bank.RollRatePID.ILimit);
+                  stabSettings.stabBank.RollRatePID.ILimit);
 
     // Set the pitch rate PID constants
-    pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp,
+    pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
-                  bank.PitchRatePID.Ki,
+                  stabSettings.stabBank.PitchRatePID.Ki,
-                  bank.PitchRatePID.Kd,
+                  stabSettings.stabBank.PitchRatePID.Kd,
-                  bank.PitchRatePID.ILimit);
+                  stabSettings.stabBank.PitchRatePID.ILimit);
 
     // Set the yaw rate PID constants
-    pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp,
+    pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
-                  bank.YawRatePID.Ki,
+                  stabSettings.stabBank.YawRatePID.Ki,
-                  bank.YawRatePID.Kd,
+                  stabSettings.stabBank.YawRatePID.Kd,
-                  bank.YawRatePID.ILimit);
+                  stabSettings.stabBank.YawRatePID.ILimit);
 
     // Set the roll attitude PI constants
-    pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
+    pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
-                  bank.RollPI.Ki,
+                  stabSettings.stabBank.RollPI.Ki,
                   0,
-                  bank.RollPI.ILimit);
+                  stabSettings.stabBank.RollPI.ILimit);
 
     // Set the pitch attitude PI constants
-    pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
+    pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
-                  bank.PitchPI.Ki,
+                  stabSettings.stabBank.PitchPI.Ki,
                   0,
-                  bank.PitchPI.ILimit);
+                  stabSettings.stabBank.PitchPI.ILimit);
 
     // Set the yaw attitude PI constants
-    pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
+    pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
-                  bank.YawPI.Ki,
+                  stabSettings.stabBank.YawPI.Ki,
                   0,
-                  bank.YawPI.ILimit);
+                  stabSettings.stabBank.YawPI.ILimit);
-}
-
-
-static float CruiseControlAngleToFactor(float angle)
-{
-    float factor;
-    // avoid singularity
-    if (angle > 89.999f && angle < 90.001f) {
-        factor = cruise_control_max_power_factor;
-    } else {
-        // the simple bank angle boost calculation that Cruise Control revolves around
-        factor = 1.0f / fabsf(cos_lookup_deg(angle));
-        // factor in the power trim, no effect at 1.0, linear effect increases with factor
-        factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
-        // limit to user specified max power multiplier
-        if (factor > cruise_control_max_power_factor) {
-            factor = cruise_control_max_power_factor;
-        }
-    }
-    return (factor);
-}
-
-
-// assumes 1.0 <= factor <= 100.0
-// a factor of less than 1.0 could make it return a value less than cruise_control_min_thrust
-// CP helis need to have min_thrust=-1
-//
-// multicopters need to have min_thrust=0.05 or so
-// values below that will not be subject to max / min limiting
-// that means thrust can be less than min
-// that means multicopter motors stop spinning at low stick
-static float CruiseControlFactorToThrust(float factor, float thrust)
-{
-    // don't touch if below min_thrust so we don't limit to min of min_thrust
-    // e.g. multicopter motors always spin
-    if (thrust > cruise_control_min_thrust) {
-        thrust = CruiseControlLimitThrust(thrust * factor);
-    }
-    return (thrust);
-}
-
-
-static float CruiseControlLimitThrust(float thrust)
-{
-    // limit to user specified absolute max thrust
-    if (thrust > cruise_control_max_thrust) {
-        thrust = cruise_control_max_thrust;
-    } else if (thrust < cruise_control_min_thrust) {
-        thrust = cruise_control_min_thrust;
-    }
-    return (thrust);
 }
 
 
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    StabilizationSettingsGet(&settings);
+    // needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
+    StabilizationSettingsGet(&stabSettings.settings);
 
     // Set up the derivative term
-    pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
+    pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
-
-    // Maximum deviation to accumulate for axis lock
-    max_axis_lock     = settings.MaxAxisLock;
-    max_axislock_rate = settings.MaxAxisLockRate;
-
-    // Settings for weak leveling
-    weak_leveling_kp  = settings.WeakLevelingKp;
-    weak_leveling_max = settings.MaxWeakLevelingRate;
-
-    // Whether to zero the PID integrals while thrust is low
-    lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
 
     // The dT has some jitter iteration to iteration that we don't want to
     // make thie result unpredictable.  Still, it's nicer to specify the constant
@@ -1043,42 +284,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
     // calculation
     const float fakeDt = 0.0025f;
-    if (settings.GyroTau < 0.0001f) {
+    if (stabSettings.settings.GyroTau < 0.0001f) {
-        gyro_alpha = 0; // not trusting this to resolve to 0
+        stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
     } else {
-        gyro_alpha = expf(-fakeDt / settings.GyroTau);
+        stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
     }
 
-    // Compute time constant for vbar decay term based on a tau
-    vbar_decay = expf(-fakeDt / settings.VbarTau);
-
     // force flight mode update
     cur_flight_mode = -1;
 
     // Rattitude stick angle where the attitude to rate transition happens
-    if (settings.RattitudeModeTransition < (uint8_t)10) {
+    if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
-        rattitude_mode_transition_stick_position = 10.0f / 100.0f;
+        stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
     } else {
-        rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;
+        stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
     }
 
-    cruise_control_min_thrust                 = (float)settings.CruiseControlMinThrust / 100.0f;
+    stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
-    cruise_control_max_thrust                 = (float)settings.CruiseControlMaxThrust / 100.0f;
+    stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
-    cruise_control_thrust_difference          = cruise_control_max_thrust - cruise_control_min_thrust;
+    stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
 
-    cruise_control_max_angle                  = (float) settings.CruiseControlMaxAngle;
+    stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
-    cruise_control_max_power_factor           = settings.CruiseControlMaxPowerFactor;
+    stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
-    cruise_control_power_trim                 = settings.CruiseControlPowerTrim / 100.0f;
+    stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
-    cruise_control_half_power_delay           = settings.CruiseControlPowerDelayComp / 2.0f;
-    cruise_control_max_power_factor_angle     = RAD2DEG(acosf(1.0f / settings.CruiseControlMaxPowerFactor));
-
-    cruise_control_inverted_thrust_reversing  = settings.CruiseControlInvertedThrustReversing;
-    cruise_control_inverted_power_output      = settings.CruiseControlInvertedPowerOutput;
-
-    memcpy(
-        cruise_control_flight_mode_switch_pos_enable,
-        settings.CruiseControlFlightModeSwitchPosEnable,
-        sizeof(cruise_control_flight_mode_switch_pos_enable));
 }
 
 /**

flight/modules/Stabilization/virtualflybar.c

@@ -37,12 +37,9 @@
 #include "stabilizationsettings.h"
 
 // ! Private variables
-static float vbar_integral[MAX_AXES];
+static float vbar_integral[3];
 static float vbar_decay = 0.991f;
 
-// ! Private methods
-static float bound(float val, float range);
-
 int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
 {
     float gyro_gain = 1.0f;
@@ -54,7 +51,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
 
     // Track the angle of the virtual flybar which includes a slow decay
     vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
-    vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
+    vbar_integral[axis] = boundf(vbar_integral[axis], -settings->VbarMaxAngle, settings->VbarMaxAngle);
 
     // Command signal can indicate how much to disregard the gyro feedback (fast flips)
     if (settings->VbarGyroSuppress > 0) {
@@ -64,15 +61,15 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
 
     // Get the settings for the correct axis
     switch (axis) {
-    case ROLL:
+    case 0:
         kp = settings->VbarRollPI.Kp;
         ki = settings->VbarRollPI.Ki;
         break;
-    case PITCH:
+    case 1:
         kp = settings->VbarPitchPI.Kp;
         ki = settings->VbarPitchPI.Ki;;
         break;
-    case YAW:
+    case 2:
         kp = settings->VbarYawPI.Kp;
         ki = settings->VbarYawPI.Ki;
         break;
@@ -105,16 +102,3 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
 
     return 0;
 }
-
-/**
- * Bound input value between limits
- */
-static float bound(float val, float range)
-{
-    if (val < -range) {
-        val = -range;
-    } else if (val > range) {
-        val = range;
-    }
-    return val;
-}

flight/modules/StateEstimation/stateestimation.c

@@ -155,6 +155,10 @@ static stateFilter cfmFilter;
 static stateFilter ekf13iFilter;
 static stateFilter ekf13Filter;
 
+// this is a hack to provide a computational shortcut for faster gyro state progression
+static float gyroRaw[3];
+static float gyroDelta[3];
+
 // preconfigured filter chains selectable via revoSettings.FusionAlgorithm
 static const filterPipeline *cfQueue = &(filterPipeline) {
     .filter = &magFilter,
@@ -415,6 +419,11 @@ static void StateEstimationCb(void)
 
         // 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, mag, x, y, z);
         FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
@@ -453,7 +462,12 @@ static void StateEstimationCb(void)
     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);
+        // 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);
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
@@ -540,6 +554,14 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
 
     if (ev->obj == GyroSensorHandle()) {
         updatedSensors |= SENSORUPDATES_gyro;
+        // shortcut - update GyroState right away
+        GyroSensorData s;
+        GyroStateData t;
+        GyroSensorGet(&s);
+        t.x = s.x + gyroDelta[0];
+        t.y = s.y + gyroDelta[1];
+        t.z = s.z + gyroDelta[2];
+        GyroStateSet(&t);
     }
 
     if (ev->obj == AccelSensorHandle()) {

flight/modules/VtolPathFollower/vtolpathfollower.c

@@ -101,7 +101,6 @@ static void updatePathVelocity();
 static void updateEndpointVelocity();
 static void updateFixedAttitude(float *attitude);
 static void updateVtolDesiredAttitude(bool yaw_attitude);
-static float bound(float val, float min, float max);
 static bool vtolpathfollower_enabled;
 static void accessoryUpdated(UAVObjEvent *ev);
 
@@ -394,7 +393,7 @@ static void updatePathVelocity()
         break;
     case PATHDESIRED_MODE_FLYENDPOINT:
     case PATHDESIRED_MODE_DRIVEENDPOINT:
-        groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
+        groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
         if (progress.fractional_progress > 1) {
             groundspeed = 0;
         }
@@ -403,7 +402,7 @@ static void updatePathVelocity()
     case PATHDESIRED_MODE_DRIVEVECTOR:
     default:
         groundspeed = pathDesired.StartingVelocity
-                      + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
+                      + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
         if (progress.fractional_progress > 1) {
             groundspeed = 0;
         }
@@ -427,14 +426,14 @@ static void updatePathVelocity()
     velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
     velocityDesired.East  += progress.correction_direction[1] * error_speed * scale;
 
-    float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1);
+    float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
 
     float downError = altitudeSetpoint - positionState.Down;
-    downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
+    downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
                              -vtolpathfollowerSettings.VerticalPosPI.ILimit,
                              vtolpathfollowerSettings.VerticalPosPI.ILimit);
     downCommand     = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
-    velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
+    velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
 
     // update pathstatus
     pathStatus.error     = progress.error;
@@ -472,13 +471,13 @@ void updateEndpointVelocity()
 
     // Compute desired north command
     northError = pathDesired.End.North - positionState.North;
-    northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
+    northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
                               -vtolpathfollowerSettings.HorizontalPosPI.ILimit,
                               vtolpathfollowerSettings.HorizontalPosPI.ILimit);
     northCommand     = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
 
     eastError = pathDesired.End.East - positionState.East;
-    eastPosIntegral  = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
+    eastPosIntegral  = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
                               -vtolpathfollowerSettings.HorizontalPosPI.ILimit,
                               vtolpathfollowerSettings.HorizontalPosPI.ILimit);
     eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
@@ -494,11 +493,11 @@ void updateEndpointVelocity()
     velocityDesired.East  = eastCommand * scale;
 
     downError = pathDesired.End.Down - positionState.Down;
-    downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
+    downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
                              -vtolpathfollowerSettings.VerticalPosPI.ILimit,
                              vtolpathfollowerSettings.VerticalPosPI.ILimit);
     downCommand     = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
-    velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
+    velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
 
     VelocityDesiredSet(&velocityDesired);
 }
@@ -595,7 +594,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
 
     // Compute desired north command
     northError = velocityDesired.North - northVel;
-    northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
+    northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
                               -vtolpathfollowerSettings.HorizontalVelPID.ILimit,
                               vtolpathfollowerSettings.HorizontalVelPID.ILimit);
     northCommand     = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
@@ -604,7 +603,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
 
     // Compute desired east command
     eastError = velocityDesired.East - eastVel;
-    eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
+    eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
                              -vtolpathfollowerSettings.HorizontalVelPID.ILimit,
                              vtolpathfollowerSettings.HorizontalVelPID.ILimit);
     eastCommand     = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
@@ -615,20 +614,20 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
     downError = velocityDesired.Down - downVel;
     // Must flip this sign
     downError = -downError;
-    downVelIntegral    = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
+    downVelIntegral    = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
                                 -vtolpathfollowerSettings.VerticalVelPID.ILimit,
                                 vtolpathfollowerSettings.VerticalVelPID.ILimit);
     downCommand        = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
                           - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
 
-    stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1);
+    stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1);
 
     // 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
-    stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
+    stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
                                -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
                                -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
-    stabDesired.Roll  = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
+    stabDesired.Roll  = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
                                eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
                                -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
 
@@ -692,19 +691,6 @@ static void updateNedAccel()
     NedAccelSet(&accelData);
 }
 
-/**
- * Bound input value between limits
- */
-static float bound(float val, float min, float max)
-{
-    if (val < min) {
-        val = min;
-    } else if (val > max) {
-        val = max;
-    }
-    return val;
-}
-
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -87,6 +87,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
     SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
     SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
+    SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c
     SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
     SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
     SRC += $(OPUAVSYNTHDIR)/actuatordesired.c

flight/targets/boards/coptercontrol/firmware/inc/openpilot.h

@@ -39,6 +39,7 @@
 #include <uavtalk.h>
 
 #include "alarms.h"
+#include <mathmisc.h>
 
 /* Global Functions */
 void OpenPilotInit(void);

flight/targets/boards/coptercontrol/firmware/inc/pios_config.h

@@ -160,7 +160,7 @@
 #define PIOS_ACTUATOR_STACK_SIZE        820
 #define PIOS_MANUAL_STACK_SIZE          635
 #define PIOS_RECEIVER_STACK_SIZE        620
-#define PIOS_STABILIZATION_STACK_SIZE   780
+#define PIOS_STABILIZATION_STACK_SIZE   400
 
 #ifdef DIAG_TASKS
 #define PIOS_SYSTEM_STACK_SIZE          740

flight/targets/boards/oplinkmini/firmware/inc/openpilot.h

@@ -40,6 +40,7 @@
 #include <uavtalk.h>
 
 #include "alarms.h"
+#include <mathmisc.h>
 
 /* Global Functions */
 void OpenPilotInit(void);

flight/targets/boards/osd/firmware/inc/openpilot.h

@@ -39,6 +39,7 @@
 #include <uavtalk.h>
 
 #include "alarms.h"
+#include <mathmisc.h>
 
 /* Global Functions */
 void OpenPilotInit(void);

flight/targets/boards/revolution/firmware/Makefile

@@ -33,7 +33,7 @@ MODULES += Sensors
 MODULES += StateEstimation # use instead of Attitude
 MODULES += Altitude/revolution
 MODULES += Airspeed
-MODULES += AltitudeHold
+#MODULES += AltitudeHold # now integrated in Stabilization
 MODULES += Stabilization
 MODULES += VtolPathFollower
 MODULES += ManualControl

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

@@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
 UAVOBJSRCFILENAMES += stabilizationsettingsbank1
 UAVOBJSRCFILENAMES += stabilizationsettingsbank2
 UAVOBJSRCFILENAMES += stabilizationsettingsbank3
+UAVOBJSRCFILENAMES += stabilizationstatus
 UAVOBJSRCFILENAMES += stabilizationbank
 UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
@@ -105,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += oplinksettings
 UAVOBJSRCFILENAMES += oplinkstatus
 UAVOBJSRCFILENAMES += altitudefiltersettings
-UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive

flight/targets/boards/revolution/firmware/inc/openpilot.h

@@ -40,6 +40,7 @@
 #include <uavtalk.h>
 
 #include "alarms.h"
+#include <mathmisc.h>
 
 /* Global Functions */
 void OpenPilotInit(void);

flight/targets/boards/revolution/firmware/inc/pios_config.h

@@ -144,7 +144,7 @@
 #define PIOS_GPS_SETS_HOMELOCATION
 
 /* Stabilization options */
-/* #define PIOS_QUATERNION_STABILIZATION */
+#define PIOS_QUATERNION_STABILIZATION
 
 /* Performance counters */
 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692

flight/targets/boards/revoproto/firmware/Makefile

@@ -33,7 +33,7 @@ MODULES += Sensors
 MODULES += StateEstimation # use instead of Attitude
 MODULES += Altitude/revolution
 MODULES += Airspeed
-MODULES += AltitudeHold
+#MODULES += AltitudeHold # now integrated in Stabilization
 MODULES += Stabilization
 MODULES += VtolPathFollower
 MODULES += ManualControl

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

@@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
 UAVOBJSRCFILENAMES += stabilizationsettingsbank1
 UAVOBJSRCFILENAMES += stabilizationsettingsbank2
 UAVOBJSRCFILENAMES += stabilizationsettingsbank3
+UAVOBJSRCFILENAMES += stabilizationstatus
 UAVOBJSRCFILENAMES += stabilizationbank
 UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
@@ -105,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += oplinksettings
 UAVOBJSRCFILENAMES += oplinkstatus
 UAVOBJSRCFILENAMES += altitudefiltersettings
-UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive

flight/targets/boards/revoproto/firmware/inc/openpilot.h

@@ -40,6 +40,7 @@
 #include <uavtalk.h>
 
 #include "alarms.h"
+#include <mathmisc.h>
 
 /* Global Functions */
 void OpenPilotInit(void);

flight/targets/boards/simposix/firmware/Makefile

@@ -42,7 +42,7 @@ MODULES += FirmwareIAP
 MODULES += StateEstimation
 #MODULES += Sensors/simulated/Sensors
 MODULES += Airspeed
-MODULES += AltitudeHold
+#MODULES += AltitudeHold # now integrated in Stabilization
 #MODULES += OveroSync
 
 # Paths
@@ -96,6 +96,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
 
 SRC += $(MATHLIB)/sin_lookup.c
 SRC += $(MATHLIB)/pid.c
+SRC += $(MATHLIB)/mathmisc.c
 
 SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
 SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c

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

@@ -86,6 +86,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
 UAVOBJSRCFILENAMES += stabilizationsettingsbank1
 UAVOBJSRCFILENAMES += stabilizationsettingsbank2
 UAVOBJSRCFILENAMES += stabilizationsettingsbank3
+UAVOBJSRCFILENAMES += stabilizationstatus
 UAVOBJSRCFILENAMES += stabilizationbank
 UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
@@ -107,7 +108,6 @@ UAVOBJSRCFILENAMES += camerastabsettings
 UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudefiltersettings
 UAVOBJSRCFILENAMES += revosettings
-UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += ekfconfiguration
 UAVOBJSRCFILENAMES += ekfstatevariance

flight/targets/boards/simposix/firmware/inc/openpilot.h

@@ -40,6 +40,7 @@
 #include <uavtalk.h>
 
 #include "alarms.h"
+#include <mathmisc.h>
 
 /* Global Functions */
 void OpenPilotInit(void);

ground/openpilotgcs/src/plugins/config/configinputwidget.cpp

@@ -152,6 +152,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
     addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true);
     addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true);
     addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true);
+    addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Thrust, "Thrust", 1, true);
+    addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Thrust, "Thrust", 1, true);
+    addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Thrust, "Thrust", 1, true);
 
     addWidgetBinding("FlightModeSettings", "Arming", ui->armControl);
     addWidgetBinding("FlightModeSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
@@ -1324,32 +1327,26 @@ void ConfigInputWidget::updatePositionSlider()
     default:
     case 6:
         ui->fmsModePos6->setEnabled(true);
-        ui->cc_box_5->setEnabled(true);
         ui->pidBankSs1_5->setEnabled(true);
     // pass through
     case 5:
         ui->fmsModePos5->setEnabled(true);
-        ui->cc_box_4->setEnabled(true);
         ui->pidBankSs1_4->setEnabled(true);
     // pass through
     case 4:
         ui->fmsModePos4->setEnabled(true);
-        ui->cc_box_3->setEnabled(true);
         ui->pidBankSs1_3->setEnabled(true);
     // pass through
     case 3:
         ui->fmsModePos3->setEnabled(true);
-        ui->cc_box_2->setEnabled(true);
         ui->pidBankSs1_2->setEnabled(true);
     // pass through
     case 2:
         ui->fmsModePos2->setEnabled(true);
-        ui->cc_box_1->setEnabled(true);
         ui->pidBankSs1_1->setEnabled(true);
     // pass through
     case 1:
         ui->fmsModePos1->setEnabled(true);
-        ui->cc_box_0->setEnabled(true);
         ui->pidBankSs1_0->setEnabled(true);
     // pass through
     case 0:
@@ -1359,32 +1356,26 @@ void ConfigInputWidget::updatePositionSlider()
     switch (manualSettingsDataPriv.FlightModeNumber) {
     case 0:
         ui->fmsModePos1->setEnabled(false);
-        ui->cc_box_0->setEnabled(false);
         ui->pidBankSs1_0->setEnabled(false);
     // pass through
     case 1:
         ui->fmsModePos2->setEnabled(false);
-        ui->cc_box_1->setEnabled(false);
         ui->pidBankSs1_1->setEnabled(false);
     // pass through
     case 2:
         ui->fmsModePos3->setEnabled(false);
-        ui->cc_box_2->setEnabled(false);
         ui->pidBankSs1_2->setEnabled(false);
     // pass through
     case 3:
         ui->fmsModePos4->setEnabled(false);
-        ui->cc_box_3->setEnabled(false);
         ui->pidBankSs1_3->setEnabled(false);
     // pass through
     case 4:
         ui->fmsModePos5->setEnabled(false);
-        ui->cc_box_4->setEnabled(false);
         ui->pidBankSs1_4->setEnabled(false);
     // pass through
     case 5:
         ui->fmsModePos6->setEnabled(false);
-        ui->cc_box_5->setEnabled(false);
         ui->pidBankSs1_5->setEnabled(false);
     // pass through
     case 6:

ground/openpilotgcs/src/plugins/config/input.ui

@@ -17,7 +17,7 @@
    <item>
     <widget class="QTabWidget" name="tabWidget">
      <property name="currentIndex">
-      <number>0</number>
+      <number>1</number>
      </property>
      <widget class="QWidget" name="RCInput">
       <attribute name="title">
@@ -27,16 +27,7 @@
        <property name="spacing">
         <number>0</number>
        </property>
-       <property name="leftMargin">
+       <property name="margin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
         <number>0</number>
        </property>
        <item>
@@ -117,20 +108,11 @@
             <x>0</x>
             <y>0</y>
             <width>774</width>
-            <height>748</height>
+            <height>753</height>
            </rect>
           </property>
           <layout class="QGridLayout" name="gridLayout">
-           <property name="leftMargin">
+           <property name="margin">
-            <number>12</number>
-           </property>
-           <property name="topMargin">
-            <number>12</number>
-           </property>
-           <property name="rightMargin">
-            <number>12</number>
-           </property>
-           <property name="bottomMargin">
             <number>12</number>
            </property>
            <property name="verticalSpacing">
@@ -161,16 +143,7 @@
                 </property>
                 <widget class="QWidget" name="advancedPage">
                  <layout class="QVBoxLayout" name="verticalLayout_3">
-                  <property name="leftMargin">
+                  <property name="margin">
-                   <number>12</number>
-                  </property>
-                  <property name="topMargin">
-                   <number>12</number>
-                  </property>
-                  <property name="rightMargin">
-                   <number>12</number>
-                  </property>
-                  <property name="bottomMargin">
                    <number>12</number>
                   </property>
                   <item>
@@ -269,16 +242,7 @@
                 </widget>
                 <widget class="QWidget" name="wizard">
                  <layout class="QVBoxLayout" name="verticalLayout_5">
-                  <property name="leftMargin">
+                  <property name="margin">
-                   <number>12</number>
-                  </property>
-                  <property name="topMargin">
-                   <number>12</number>
-                  </property>
-                  <property name="rightMargin">
-                   <number>12</number>
-                  </property>
-                  <property name="bottomMargin">
                    <number>12</number>
                   </property>
                   <item>
@@ -457,16 +421,7 @@
        <string>Flight Mode Switch Settings</string>
       </attribute>
       <layout class="QVBoxLayout" name="verticalLayout_8">
-       <property name="leftMargin">
+       <property name="margin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
         <number>0</number>
        </property>
        <item>
@@ -546,21 +501,12 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>768</width>
+            <width>774</width>
-            <height>742</height>
+            <height>753</height>
            </rect>
           </property>
           <layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0,0">
-           <property name="leftMargin">
+           <property name="margin">
-            <number>12</number>
-           </property>
-           <property name="topMargin">
-            <number>12</number>
-           </property>
-           <property name="rightMargin">
-            <number>12</number>
-           </property>
-           <property name="bottomMargin">
             <number>12</number>
            </property>
            <item row="3" column="0">
@@ -587,7 +533,7 @@
              <property name="title">
               <string>Stabilization Modes Configuration</string>
              </property>
-             <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0">
+             <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0,0,0">
               <property name="leftMargin">
                <number>9</number>
               </property>
@@ -626,6 +572,51 @@
                 </property>
                </spacer>
               </item>
+              <item row="1" column="10">
+               <spacer name="horizontalSpacer_15">
+                <property name="orientation">
+                 <enum>Qt::Horizontal</enum>
+                </property>
+                <property name="sizeType">
+                 <enum>QSizePolicy::Fixed</enum>
+                </property>
+                <property name="sizeHint" stdset="0">
+                 <size>
+                  <width>10</width>
+                  <height>20</height>
+                 </size>
+                </property>
+               </spacer>
+              </item>
+              <item row="0" column="11">
+               <widget class="QLabel" name="label_11">
+                <property name="minimumSize">
+                 <size>
+                  <width>120</width>
+                  <height>20</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>20</height>
+                 </size>
+                </property>
+                <property name="styleSheet">
+                 <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
+color: rgb(255, 255, 255);
+border-radius: 5;
+font: bold 12px;
+margin:1px;</string>
+                </property>
+                <property name="text">
+                 <string>Thrust</string>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignCenter</set>
+                </property>
+               </widget>
+              </item>
               <item row="0" column="9">
                <widget class="QLabel" name="label_10">
                 <property name="minimumSize">
@@ -684,7 +675,7 @@ margin:1px;</string>
                 </property>
                </widget>
               </item>
-              <item row="1" column="10">
+              <item row="1" column="12">
                <spacer name="horizontalSpacer_11">
                 <property name="orientation">
                  <enum>Qt::Horizontal</enum>
@@ -792,16 +783,7 @@ margin:1px;</string>
                  <enum>QFrame::Raised</enum>
                 </property>
                 <layout class="QVBoxLayout" name="verticalLayout_10">
-                 <property name="leftMargin">
+                 <property name="margin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
                   <number>1</number>
                  </property>
                  <item>
@@ -843,16 +825,7 @@ margin:1px;</string>
                  <enum>QFrame::Raised</enum>
                 </property>
                 <layout class="QVBoxLayout" name="verticalLayout_14">
-                 <property name="leftMargin">
+                 <property name="margin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
                   <number>1</number>
                  </property>
                  <item>
@@ -894,16 +867,7 @@ margin:1px;</string>
                  <enum>QFrame::Raised</enum>
                 </property>
                 <layout class="QVBoxLayout" name="verticalLayout_15">
-                 <property name="leftMargin">
+                 <property name="margin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
                   <number>1</number>
                  </property>
                  <item>
@@ -936,6 +900,48 @@ margin:1px;</string>
                 </layout>
                </widget>
               </item>
+              <item row="1" column="11">
+               <widget class="QFrame" name="frame_7">
+                <property name="frameShape">
+                 <enum>QFrame::NoFrame</enum>
+                </property>
+                <property name="frameShadow">
+                 <enum>QFrame::Raised</enum>
+                </property>
+                <layout class="QVBoxLayout" name="verticalLayout_16">
+                 <property name="margin">
+                  <number>1</number>
+                 </property>
+                 <item>
+                  <widget class="QComboBox" name="fmsSsPos1Thrust">
+                   <property name="minimumSize">
+                    <size>
+                     <width>0</width>
+                     <height>0</height>
+                    </size>
+                   </property>
+                   <property name="focusPolicy">
+                    <enum>Qt::StrongFocus</enum>
+                   </property>
+                  </widget>
+                 </item>
+                 <item>
+                  <widget class="QComboBox" name="fmsSsPos2Thrust">
+                   <property name="focusPolicy">
+                    <enum>Qt::StrongFocus</enum>
+                   </property>
+                  </widget>
+                 </item>
+                 <item>
+                  <widget class="QComboBox" name="fmsSsPos3Thrust">
+                   <property name="focusPolicy">
+                    <enum>Qt::StrongFocus</enum>
+                   </property>
+                  </widget>
+                 </item>
+                </layout>
+               </widget>
+              </item>
               <item row="1" column="2">
                <spacer name="horizontalSpacer_14">
                 <property name="orientation">
@@ -973,23 +979,14 @@ margin:1px;</string>
               <string>Flight Mode Switch Positions</string>
              </property>
              <layout class="QGridLayout" name="gridLayout_4">
-              <property name="leftMargin">
+              <property name="margin">
-               <number>9</number>
-              </property>
-              <property name="topMargin">
-               <number>9</number>
-              </property>
-              <property name="rightMargin">
-               <number>9</number>
-              </property>
-              <property name="bottomMargin">
                <number>9</number>
               </property>
               <property name="horizontalSpacing">
                <number>6</number>
               </property>
               <item row="0" column="6">
-               <widget class="QLabel" name="label_11">
+               <widget class="QLabel" name="label_111">
                 <property name="minimumSize">
                  <size>
                   <width>80</width>
@@ -1078,7 +1075,7 @@ channel value for each flight mode.</string>
               <item row="2" column="10">
                <widget class="QLabel" name="label_15">
                 <property name="sizePolicy">
-                 <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+                 <sizepolicy hsizetype="Ignored" vsizetype="Preferred">
                   <horstretch>0</horstretch>
                   <verstretch>0</verstretch>
                  </sizepolicy>
@@ -1090,7 +1087,7 @@ channel value for each flight mode.</string>
                  </font>
                 </property>
                 <property name="text">
-                 <string>Avoid &quot;Manual&quot; for multirotors!</string>
+                 <string>Avoid &quot;Manual&quot; for multirotors! Never select &quot;Altitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</string>
                 </property>
                 <property name="alignment">
                  <set>Qt::AlignCenter</set>
@@ -1116,22 +1113,6 @@ channel value for each flight mode.</string>
                 </property>
                </spacer>
               </item>
-              <item row="1" column="5">
-               <spacer name="horizontalSpacer_15">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeType">
-                 <enum>QSizePolicy::Fixed</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>10</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
               <item row="1" column="7">
                <spacer name="horizontalSpacer_16">
                 <property name="orientation">
@@ -1164,35 +1145,6 @@ channel value for each flight mode.</string>
                 </property>
                </spacer>
               </item>
-              <item row="0" column="4">
-               <widget class="QLabel" name="label_12">
-                <property name="minimumSize">
-                 <size>
-                  <width>105</width>
-                  <height>20</height>
-                 </size>
-                </property>
-                <property name="maximumSize">
-                 <size>
-                  <width>16777215</width>
-                  <height>20</height>
-                 </size>
-                </property>
-                <property name="styleSheet">
-                 <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
-color: rgb(255, 255, 255);
-border-radius: 5;
-font: bold 12px;
-margin:1px;</string>
-                </property>
-                <property name="text">
-                 <string>Cruise Control</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignCenter</set>
-                </property>
-               </widget>
-              </item>
               <item row="0" column="8" rowspan="3">
                <widget class="Line" name="line">
                 <property name="orientation">
@@ -1229,294 +1181,6 @@ margin:1px;</string>
                 </property>
                </widget>
               </item>
-              <item row="1" column="4" rowspan="2">
-               <widget class="QFrame" name="frame_4">
-                <property name="minimumSize">
-                 <size>
-                  <width>30</width>
-                  <height>0</height>
-                 </size>
-                </property>
-                <property name="maximumSize">
-                 <size>
-                  <width>16777215</width>
-                  <height>16777215</height>
-                 </size>
-                </property>
-                <layout class="QVBoxLayout" name="verticalLayout_4">
-                 <property name="spacing">
-                  <number>6</number>
-                 </property>
-                 <property name="leftMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
-                  <number>1</number>
-                 </property>
-                 <item alignment="Qt::AlignHCenter">
-                  <widget class="QCheckBox" name="cc_box_0">
-                   <property name="sizePolicy">
-                    <sizepolicy hsizetype="Fixed" vsizetype="Minimum">
-                     <horstretch>0</horstretch>
-                     <verstretch>0</verstretch>
-                    </sizepolicy>
-                   </property>
-                   <property name="minimumSize">
-                    <size>
-                     <width>0</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="maximumSize">
-                    <size>
-                     <width>16777215</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="font">
-                    <font>
-                     <weight>75</weight>
-                     <bold>true</bold>
-                    </font>
-                   </property>
-                   <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                   </property>
-                   <property name="layoutDirection">
-                    <enum>Qt::RightToLeft</enum>
-                   </property>
-                   <property name="text">
-                    <string/>
-                   </property>
-                   <property name="objrelation" stdset="0">
-                    <stringlist>
-                     <string>objname:StabilizationSettings</string>
-                     <string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
-                     <string>index:0</string>
-                     <string>haslimits:no</string>
-                     <string>scale:1</string>
-                    </stringlist>
-                   </property>
-                  </widget>
-                 </item>
-                 <item alignment="Qt::AlignHCenter">
-                  <widget class="QCheckBox" name="cc_box_1">
-                   <property name="minimumSize">
-                    <size>
-                     <width>0</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="maximumSize">
-                    <size>
-                     <width>16777215</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="font">
-                    <font>
-                     <weight>75</weight>
-                     <bold>true</bold>
-                    </font>
-                   </property>
-                   <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                   </property>
-                   <property name="layoutDirection">
-                    <enum>Qt::RightToLeft</enum>
-                   </property>
-                   <property name="text">
-                    <string/>
-                   </property>
-                   <property name="objrelation" stdset="0">
-                    <stringlist>
-                     <string>objname:StabilizationSettings</string>
-                     <string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
-                     <string>index:1</string>
-                     <string>haslimits:no</string>
-                     <string>scale:1</string>
-                    </stringlist>
-                   </property>
-                  </widget>
-                 </item>
-                 <item alignment="Qt::AlignHCenter">
-                  <widget class="QCheckBox" name="cc_box_2">
-                   <property name="minimumSize">
-                    <size>
-                     <width>0</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="maximumSize">
-                    <size>
-                     <width>16777215</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="font">
-                    <font>
-                     <weight>75</weight>
-                     <bold>true</bold>
-                    </font>
-                   </property>
-                   <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                   </property>
-                   <property name="layoutDirection">
-                    <enum>Qt::RightToLeft</enum>
-                   </property>
-                   <property name="text">
-                    <string/>
-                   </property>
-                   <property name="objrelation" stdset="0">
-                    <stringlist>
-                     <string>objname:StabilizationSettings</string>
-                     <string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
-                     <string>index:2</string>
-                     <string>haslimits:no</string>
-                     <string>scale:1</string>
-                    </stringlist>
-                   </property>
-                  </widget>
-                 </item>
-                 <item alignment="Qt::AlignHCenter">
-                  <widget class="QCheckBox" name="cc_box_3">
-                   <property name="enabled">
-                    <bool>false</bool>
-                   </property>
-                   <property name="minimumSize">
-                    <size>
-                     <width>0</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="maximumSize">
-                    <size>
-                     <width>16777215</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="font">
-                    <font>
-                     <weight>75</weight>
-                     <bold>true</bold>
-                    </font>
-                   </property>
-                   <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                   </property>
-                   <property name="layoutDirection">
-                    <enum>Qt::RightToLeft</enum>
-                   </property>
-                   <property name="text">
-                    <string/>
-                   </property>
-                   <property name="objrelation" stdset="0">
-                    <stringlist>
-                     <string>objname:StabilizationSettings</string>
-                     <string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
-                     <string>index:3</string>
-                     <string>haslimits:no</string>
-                     <string>scale:1</string>
-                    </stringlist>
-                   </property>
-                  </widget>
-                 </item>
-                 <item alignment="Qt::AlignHCenter">
-                  <widget class="QCheckBox" name="cc_box_4">
-                   <property name="enabled">
-                    <bool>false</bool>
-                   </property>
-                   <property name="minimumSize">
-                    <size>
-                     <width>0</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="maximumSize">
-                    <size>
-                     <width>16777215</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="font">
-                    <font>
-                     <weight>75</weight>
-                     <bold>true</bold>
-                    </font>
-                   </property>
-                   <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                   </property>
-                   <property name="layoutDirection">
-                    <enum>Qt::RightToLeft</enum>
-                   </property>
-                   <property name="text">
-                    <string/>
-                   </property>
-                   <property name="objrelation" stdset="0">
-                    <stringlist>
-                     <string>objname:StabilizationSettings</string>
-                     <string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
-                     <string>index:4</string>
-                     <string>haslimits:no</string>
-                     <string>scale:1</string>
-                    </stringlist>
-                   </property>
-                  </widget>
-                 </item>
-                 <item alignment="Qt::AlignHCenter">
-                  <widget class="QCheckBox" name="cc_box_5">
-                   <property name="enabled">
-                    <bool>false</bool>
-                   </property>
-                   <property name="minimumSize">
-                    <size>
-                     <width>0</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="maximumSize">
-                    <size>
-                     <width>16777215</width>
-                     <height>20</height>
-                    </size>
-                   </property>
-                   <property name="font">
-                    <font>
-                     <weight>75</weight>
-                     <bold>true</bold>
-                    </font>
-                   </property>
-                   <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                   </property>
-                   <property name="layoutDirection">
-                    <enum>Qt::RightToLeft</enum>
-                   </property>
-                   <property name="text">
-                    <string/>
-                   </property>
-                   <property name="objrelation" stdset="0">
-                    <stringlist>
-                     <string>objname:StabilizationSettings</string>
-                     <string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
-                     <string>index:5</string>
-                     <string>haslimits:no</string>
-                     <string>scale:1</string>
-                    </stringlist>
-                   </property>
-                  </widget>
-                 </item>
-                </layout>
-               </widget>
-              </item>
               <item row="1" column="0" rowspan="2">
                <widget class="QFrame" name="frame">
                 <property name="minimumSize">
@@ -1538,21 +1202,12 @@ margin:1px;</string>
                  <enum>QFrame::Raised</enum>
                 </property>
                 <layout class="QGridLayout" name="gridLayout_8">
-                 <property name="leftMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
-                  <number>1</number>
-                 </property>
                  <property name="horizontalSpacing">
                   <number>10</number>
                  </property>
+                 <property name="margin">
+                  <number>1</number>
+                 </property>
                  <item row="4" column="1">
                   <widget class="QLabel" name="label_pos5">
                    <property name="sizePolicy">
@@ -1770,16 +1425,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                  </size>
                 </property>
                 <layout class="QGridLayout" name="gridLayout_6">
-                 <property name="leftMargin">
+                 <property name="margin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
                   <number>1</number>
                  </property>
                  <item row="5" column="1">
@@ -1867,7 +1513,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                </widget>
               </item>
               <item row="1" column="6" rowspan="2">
-               <widget class="QFrame" name="frame_7">
+               <widget class="QFrame" name="frame_8">
                 <property name="minimumSize">
                  <size>
                   <width>75</width>
@@ -1881,16 +1527,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                  </size>
                 </property>
                 <layout class="QVBoxLayout" name="verticalLayout_9">
-                 <property name="leftMargin">
+                 <property name="margin">
-                  <number>1</number>
-                 </property>
-                 <property name="topMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="rightMargin">
-                  <number>1</number>
-                 </property>
-                 <property name="bottomMargin">
                   <number>1</number>
                  </property>
                  <item>
@@ -2075,16 +1712,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
        <string>Arming Settings</string>
       </attribute>
       <layout class="QVBoxLayout" name="verticalLayout_12">
-       <property name="leftMargin">
+       <property name="margin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
         <number>0</number>
        </property>
        <item>
@@ -2164,21 +1792,12 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>768</width>
+            <width>774</width>
-            <height>742</height>
+            <height>753</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_2">
-           <property name="leftMargin">
+           <property name="margin">
-            <number>12</number>
-           </property>
-           <property name="topMargin">
-            <number>12</number>
-           </property>
-           <property name="rightMargin">
-            <number>12</number>
-           </property>
-           <property name="bottomMargin">
             <number>12</number>
            </property>
            <item>

ground/openpilotgcs/src/plugins/config/stabilization.ui

@@ -8444,7 +8444,7 @@ border-radius: 5;</string>
                        <string notr="true"/>
                       </property>
                       <property name="text">
-                       <string>Max rate attitude (deg/s)</string>
+                       <string>Max rate limit (all modes) (deg/s)</string>
                       </property>
                       <property name="alignment">
                        <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>

ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp

@@ -318,18 +318,21 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration()
     data.Stabilization1Settings[0] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
     data.Stabilization1Settings[1] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
     data.Stabilization1Settings[2] = FlightModeSettings::STABILIZATION1SETTINGS_AXISLOCK;
+    data.Stabilization1Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
     data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
     data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
     data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE;
+    data.Stabilization2Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
     data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
     data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
     data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
+    data.Stabilization3Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
     data2.FlightModeNumber = 3;
     data.FlightModePosition[0]     = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1;
     data.FlightModePosition[1]     = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2;
     data.FlightModePosition[2]     = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3;
-    data.FlightModePosition[3]     = FlightModeSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD;
+    data.FlightModePosition[3]     = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
-    data.FlightModePosition[4]     = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD;
+    data.FlightModePosition[4]     = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
     data.FlightModePosition[5]     = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
     modeSettings->setData(data);
     addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2"));

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

@@ -35,7 +35,6 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/airspeedstate.h \
     $$UAVOBJECT_SYNTHETICS/attitudestate.h \
     $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
-    $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
     $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
     $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
     $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
@@ -62,6 +61,7 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/overosyncstats.h \
     $$UAVOBJECT_SYNTHETICS/overosyncsettings.h \
     $$UAVOBJECT_SYNTHETICS/systemsettings.h \
+    $$UAVOBJECT_SYNTHETICS/stabilizationstatus.h \
     $$UAVOBJECT_SYNTHETICS/stabilizationsettings.h \
     $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.h \
     $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.h \
@@ -136,7 +136,6 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
-    $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
     $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \
@@ -163,6 +162,7 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/overosyncstats.cpp \
     $$UAVOBJECT_SYNTHETICS/overosyncsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/systemsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/stabilizationstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/stabilizationsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.cpp \
     $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.cpp \

make/apps-defs.mk

@@ -106,6 +106,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
 SRC += $(MATHLIB)/sin_lookup.c
 SRC += $(MATHLIB)/pid.c
+SRC += $(MATHLIB)/mathmisc.c
 SRC += $(FLIGHTLIB)/printf-stdarg.c
 
 ## Modules

shared/uavobjectdefinition/altitudeholddesired.xml

@@ -1,14 +0,0 @@
-<xml>
-    <object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
-        <description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
-        <field name="SetPoint" units="" type="float" elements="1"/>
-        <field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Thrust" />
-	<field name="Roll" units="deg" type="float" elements="1"/>
-        <field name="Pitch" units="deg" type="float" elements="1"/>
-        <field name="Yaw" units="deg/s" 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/callbackinfo.xml

@@ -6,6 +6,8 @@
 			<elementname>EventDispatcher</elementname>
 			<elementname>StateEstimation</elementname>
 			<elementname>AltitudeHold</elementname>
+			<elementname>Stabilization0</elementname>
+			<elementname>Stabilization1</elementname>
 			<elementname>PathPlanner0</elementname>
 			<elementname>PathPlanner1</elementname>
 			<elementname>ManualControl</elementname>
@@ -16,6 +18,8 @@
 			<elementname>EventDispatcher</elementname>
 			<elementname>StateEstimation</elementname>
 			<elementname>AltitudeHold</elementname>
+			<elementname>Stabilization0</elementname>
+			<elementname>Stabilization1</elementname>
 			<elementname>PathPlanner0</elementname>
 			<elementname>PathPlanner1</elementname>
 			<elementname>ManualControl</elementname>
@@ -30,6 +34,8 @@
 			<elementname>EventDispatcher</elementname>
 			<elementname>StateEstimation</elementname>
 			<elementname>AltitudeHold</elementname>
+			<elementname>Stabilization0</elementname>
+			<elementname>Stabilization1</elementname>
 			<elementname>PathPlanner0</elementname>
 			<elementname>PathPlanner1</elementname>
 			<elementname>ManualControl</elementname>

shared/uavobjectdefinition/flightmodesettings.xml

@@ -6,20 +6,38 @@
 
 	<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
         <field name="Stabilization1Settings" units="" type="enum"
-		elementnames="Roll,Pitch,Yaw"
+		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
-		defaultvalue="Attitude,Attitude,AxisLock"
+		defaultvalue="Attitude,Attitude,AxisLock,Manual"
-		limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			/>
         <field name="Stabilization2Settings" units="" type="enum"
-		elementnames="Roll,Pitch,Yaw"
+		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
-		defaultvalue="Attitude,Attitude,Rate"
+		defaultvalue="Attitude,Attitude,Rate,Manual"
-		limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			/>
         <field name="Stabilization3Settings" units="" type="enum"
-		elementnames="Roll,Pitch,Yaw"
+		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
+		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
-		defaultvalue="Rate,Rate,Rate"
+		defaultvalue="Rate,Rate,Rate,Manual"
-		limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
+		limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
+			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
+			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
+			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
+			/>
 
 	<!-- Note these options values should be identical to those defined in FlightMode -->
         <!-- Currently only some modes are enabled for UI using limits attribute per board. Update when more modes will be operational -->
@@ -27,32 +45,32 @@
                units=""
                type="enum"
                elements="6"
-               options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
+               options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"
-               defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,AltitudeVario,Manual"
+               defaultvalue="Stabilized1,Stabilized2,Stabilized3,Manual,Manual,Manual"
                limits="\
-                       %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
+                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
+                       %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
+                       %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
 
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>

shared/uavobjectdefinition/flightstatus.xml

@@ -4,7 +4,7 @@
         <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
 
 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
 
 	<field name="ControlChain" units="bool" type="enum" options="false,true">
 		<elementnames>

shared/uavobjectdefinition/ratedesired.xml

@@ -4,6 +4,7 @@
         <field name="Roll" units="deg/s" type="float" elements="1"/>
         <field name="Pitch" units="deg/s" type="float" elements="1"/>
         <field name="Yaw" units="deg/s" type="float" elements="1"/>
+        <field name="Thrust" units="%" type="float" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>

shared/uavobjectdefinition/stabilizationdesired.xml

@@ -5,9 +5,8 @@
         <field name="Pitch" units="degrees" type="float" elements="1"/>
         <field name="Yaw" units="degrees" type="float" elements="1"/>
         <field name="Thrust" units="%" type="float" elements="1"/>
-	<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
+	<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
-        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"/>
+        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"/>
-        <field name="ThrustStabilizationMode" units="" type="enum" elements="1" options="None"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>

shared/uavobjectdefinition/stabilizationsettings.xml

@@ -22,6 +22,7 @@
 	<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
 	<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
 
+	<field name="AxisLockKp" units="" type="float" elements="1" defaultvalue="2.5"/>
 	<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="30"/>
 	<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
 

shared/uavobjectdefinition/stabilizationsettingsbank1.xml

@@ -6,7 +6,7 @@
 	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
-	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

shared/uavobjectdefinition/stabilizationsettingsbank2.xml

@@ -6,7 +6,7 @@
 	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
-	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

shared/uavobjectdefinition/stabilizationsettingsbank3.xml

@@ -6,7 +6,7 @@
 	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
-	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

shared/uavobjectdefinition/stabilizationstatus.xml

@@ -0,0 +1,28 @@
+<xml>
+    <object name="StabilizationStatus" singleinstance="true" settings="false" category="Control">
+        <description>Contains status information to control submodules for stabilization.</description>
+
+
+	<field name="OuterLoop" units="" type="enum" options="Direct,Attitude,Rattitude,Weakleveling,Altitude,VerticalVelocity">
+		<elementnames>
+			<elementname>Roll</elementname>
+			<elementname>Pitch</elementname>
+			<elementname>Yaw</elementname>
+			<elementname>Thrust</elementname>
+		</elementnames>
+	</field>
+	<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,RelayTuning,AxisLock,Rate,CruiseControl">
+		<elementnames>
+			<elementname>Roll</elementname>
+			<elementname>Pitch</elementname>
+			<elementname>Yaw</elementname>
+			<elementname>Thrust</elementname>
+		</elementnames>
+	</field>
+
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="5000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>