Merge branch 'abeck/OP-1848r-altvario' into next

flight/libraries/pid/pidcontroldown.cpp

@@ -45,7 +45,6 @@ extern "C" {
 #include <vtolselftuningstats.h>
 }
 
-#include "pathfollowerfsm.h"
 #include "pidcontroldown.h"
 
 #define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
@@ -59,17 +58,17 @@ extern "C" {
 
 PIDControlDown::PIDControlDown()
     : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
-    mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
+    mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
-    mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
+    mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true)
 {
     Deactivate();
 }
 
 PIDControlDown::~PIDControlDown() {}
 
-void PIDControlDown::Initialize(PathFollowerFSM *fsm)
+void PIDControlDown::Initialize(PIDControlDownCallback *callback)
 {
-    mFSM = fsm;
+    mCallback = callback;
 }
 
 void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
@@ -80,7 +79,6 @@ void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
 
 void PIDControlDown::Deactivate()
 {
-    // pid_zero(&PID);
     mActive = false;
 }
 
@@ -333,9 +331,9 @@ void PIDControlDown::UpdateVelocityState(float pv)
 {
     mVelocityState = pv;
 
-    if (mFSM) {
+    if (mCallback) {
         // The FSM controls the actual descent velocity and introduces step changes as required
-        float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget);
+        float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget);
         // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
         mVelocitySetpointCurrent = velocitySetpointDesired;
     } else {
@@ -354,8 +352,8 @@ float PIDControlDown::GetDownCommand(void)
     float ulow  = mMinThrust;
     float uhigh = mMaxThrust;
 
-    if (mFSM) {
+    if (mCallback) {
-        mFSM->BoundThrust(ulow, uhigh);
+        mCallback->BoundThrust(ulow, uhigh);
     }
     float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
     pidStatus.setpoint = mVelocitySetpointCurrent;

flight/libraries/pid/pidcontroldown.h

@@ -34,16 +34,20 @@ extern "C" {
 #include <pid.h>
 #include <stabilizationdesired.h>
 }
-#include "pathfollowerfsm.h"
+#include "pidcontroldowncallback.h"
 
 class PIDControlDown {
 public:
     PIDControlDown();
     ~PIDControlDown();
-    void Initialize(PathFollowerFSM *fsm);
+    void Initialize(PIDControlDownCallback *callback);
     void SetThrustLimits(float min_thrust, float max_thrust);
     void Deactivate();
     void Activate();
+    bool IsActive()
+    {
+        return mActive;
+    }
     void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
     void UpdateNeutralThrust(float neutral);
     void UpdateVelocitySetpoint(float setpoint);
@@ -58,6 +62,14 @@ public:
     void ControlPositionWithPath(struct path_status *progress);
     void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
     void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
+    void DisableNeutralThrustCalc()
+    {
+        mAllowNeutralThrustCalc = false;
+    }
+    void EnableNeutralThrustCalc()
+    {
+        mAllowNeutralThrustCalc = true;
+    }
 
 private:
     void setup_neutralThrustCalc();
@@ -69,7 +81,7 @@ private:
     float mVelocitySetpointCurrent;
     float mVelocityState;
     float mDownCommand;
-    PathFollowerFSM *mFSM;
+    PIDControlDownCallback *mCallback;
     float mNeutral;
     float mVelocityMax;
     struct pid PIDpos;
@@ -77,7 +89,6 @@ private:
     float mPositionState;
     float mMinThrust;
     float mMaxThrust;
-    uint8_t mActive;
 
     struct NeutralThrustEstimation {
         uint32_t count;
@@ -90,6 +101,8 @@ private:
         bool     have_correction;
     };
     struct NeutralThrustEstimation neutralThrustEst;
+    bool mActive;
+    bool mAllowNeutralThrustCalc;
 };
 
 #endif // PIDCONTROLDOWN_H

flight/libraries/pid/pidcontroldowncallback.h

@@ -0,0 +1,42 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PID Library
+ * @brief Thrust control callback pure virtual
+ * @{
+ *
+ * @file       pidcontroldowncallback.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for PathFollower FSMs
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PIDCONTROLDOWNCALLBACK_H
+#define PIDCONTROLDOWNCALLBACK_H
+
+class PIDControlDownCallback {
+public:
+    // PIDControlDownCalback() {};
+    virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0;
+    virtual float BoundVelocityDown(float velocity) = 0;
+    // virtual ~PIDControlDownCalback() = 0;
+};
+
+#endif // PIDCONTROLDOWNCALLBACK_H

flight/modules/PathFollower/inc/pathfollowerfsm.h

@@ -34,6 +34,7 @@
 extern "C" {
 #include <stabilizationdesired.h>
 }
+#include <pidcontroldowncallback.h>
 
 typedef enum {
     PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
@@ -42,7 +43,7 @@ typedef enum {
     PFFSM_STATE_ABORT // Abort on error
 } PathFollowerFSMState_T;
 
-class PathFollowerFSM {
+class PathFollowerFSM : public PIDControlDownCallback {
 public:
     // PathFollowerFSM() {};
     virtual void Inactive(void) {}

flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

@@ -126,7 +126,7 @@ void VtolAutoTakeoffController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
 

flight/modules/PathFollower/vtolbrakecontroller.cpp

@@ -132,7 +132,7 @@ void VtolBrakeController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta,
                                dT,
                                10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);

flight/modules/PathFollower/vtolflycontroller.cpp

@@ -127,7 +127,7 @@ void VtolFlyController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
@@ -136,7 +136,7 @@ void VtolFlyController::SettingsUpdated(void)
     controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
                                  vtolPathFollowerSettings->VerticalVelPID.Ki,
                                  vtolPathFollowerSettings->VerticalVelPID.Kd,
-                                 vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA
+                                 vtolPathFollowerSettings->VerticalVelPID.Beta,
                                  dT,
                                  vtolPathFollowerSettings->VerticalVelMax);
     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
@@ -145,6 +145,9 @@ void VtolFlyController::SettingsUpdated(void)
     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
     controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+    // disable neutral thrust calcs which should only be done in a hold mode.
+    controlDown.DisableNeutralThrustCalc();
 }
 
 /**

flight/modules/PathFollower/vtollandcontroller.cpp

@@ -120,7 +120,7 @@ void VtolLandController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
 

flight/modules/PathFollower/vtolvelocitycontroller.cpp

@@ -107,7 +107,7 @@ void VtolVelocityController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
 
@@ -181,7 +181,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)
     ManualControlCommandData manualControl;
     ManualControlCommandGet(&manualControl);
 
-    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
     stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 
     // default thrust mode to altvario

flight/modules/Stabilization/altitudeloop.cpp

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
  *
- * @file       altitudeloop.c
+ * @file       altitudeloop.cpp
  * @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
@@ -26,6 +26,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+extern "C" {
 #include <openpilot.h>
 
 #include <callbackinfo.h>
@@ -37,6 +38,12 @@
 #include <altitudeholdstatus.h>
 #include <velocitystate.h>
 #include <positionstate.h>
+#include <vtolselftuningstats.h>
+#include <stabilization.h>
+}
+
+#include <pidcontroldown.h>
+
 // Private constants
 
 
@@ -50,78 +57,134 @@
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
 #define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
 
-#define STACK_SIZE_BYTES  512
 // Private types
 
 // Private variables
 static DelayedCallbackInfo *altitudeHoldCBInfo;
+static PIDControlDown controlDown;
 static AltitudeHoldSettingsData altitudeHoldSettings;
-static struct pid pid0, pid1;
 static ThrustModeType thrustMode;
-static PiOSDeltatimeConfig timeval;
+static float thrustDemand = 0.0f;
-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 altitudeHoldTask(void);
 static void VelocityStateUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Setup mode and setpoint
+ *
+ * reinit: true when althold/vario mode selected over a previous alternate thrust mode
  */
 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
 {
     static bool newaltitude = true;
 
-    if (reinit) {
+    if (reinit || !controlDown.IsActive()) {
-        startThrust = setpoint;
+        controlDown.Activate();
-        pid_zero(&pid0);
-        pid_zero(&pid1);
         newaltitude = true;
+        // calculate a thrustDemand on reinit only
+        altitudeHoldTask();
     }
 
     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;
+        controlDown.UpdateVelocitySetpoint(0.0f);
-        thrustDemand   = 0.0f;
+        thrustDemand = 0.0f;
-        thrustMode     = DIRECT;
+        thrustMode   = DIRECT;
-        newaltitude    = true;
+        newaltitude  = true;
+        return thrustDemand;
     } 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);
+        controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
-        thrustMode     = ALTITUDEVARIO;
+        thrustMode  = ALTITUDEVARIO;
-        newaltitude    = true;
+        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);
+        controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
-        thrustMode     = ALTITUDEVARIO;
+        thrustMode  = ALTITUDEVARIO;
-        newaltitude    = true;
+        newaltitude = true;
     } else if (newaltitude == true) {
-        thrustSetpoint = posState.Down;
+        controlDown.UpdateVelocitySetpoint(0.0f);
-        thrustMode     = ALTITUDEHOLD;
+        PositionStateData posState;
-        newaltitude    = false;
+        PositionStateGet(&posState);
+        controlDown.UpdatePositionSetpoint(posState.Down);
+        thrustMode  = ALTITUDEHOLD;
+        newaltitude = false;
     }
 
+    thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
+
     return thrustDemand;
 }
 
+/**
+ * Disable the alt hold task loop velocity controller to save cpu cycles
+ */
+void stabilizationDisableAltitudeHold(void)
+{
+    controlDown.Deactivate();
+}
+
+
+/**
+ * Module thread, should not return.
+ */
+static void altitudeHoldTask(void)
+{
+    if (!controlDown.IsActive()) {
+        return;
+    }
+
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
+    float velocityStateDown;
+    VelocityStateDownGet(&velocityStateDown);
+    controlDown.UpdateVelocityState(velocityStateDown);
+
+    float local_thrustDemand = 0.0f;
+
+    switch (thrustMode) {
+    case ALTITUDEHOLD:
+    {
+        float positionStateDown;
+        PositionStateDownGet(&positionStateDown);
+        controlDown.UpdatePositionState(positionStateDown);
+        controlDown.ControlPosition();
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
+        local_thrustDemand = controlDown.GetDownCommand();
+    }
+    break;
+
+    case ALTITUDEVARIO:
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
+        local_thrustDemand = controlDown.GetDownCommand();
+        break;
+
+    case DIRECT:
+        altitudeHoldStatus.VelocityDesired = 0.0f;
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
+        break;
+    }
+
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
+    thrustDemand = local_thrustDemand;
+}
+
+static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+}
+
 /**
  * Initialise the module, called on startup
  */
@@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit()
     AltitudeHoldStatusInitialize();
     PositionStateInitialize();
     VelocityStateInitialize();
+    VtolSelfTuningStatsInitialize();
 
-    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+    AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
-    // Create object queue
+    VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
+    SettingsUpdatedCb(NULL);
 
     altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
-    AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
     VelocityStateConnectCallback(&VelocityStateUpdatedCb);
-
-    // Start main task
-    SettingsUpdatedCb(NULL);
 }
 
 
-/**
- * Module thread, should not return.
- */
-static void altitudeHoldTask(void)
-{
-    AltitudeHoldStatusData altitudeHoldStatus;
-
-    AltitudeHoldStatusGet(&altitudeHoldStatus);
-
-    // do the actual control loop(s)
-    float positionStateDown;
-    PositionStateDownGet(&positionStateDown);
-    float velocityStateDown;
-    VelocityStateDownGet(&velocityStateDown);
-
-    float dT;
-    dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
-    switch (thrustMode) {
-    case ALTITUDEHOLD:
-    {
-        // altitude control loop
-        // No scaling.
-        const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
-        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
-    }
-    break;
-    case ALTITUDEVARIO:
-        altitudeHoldStatus.VelocityDesired = thrustSetpoint;
-        break;
-    default:
-        altitudeHoldStatus.VelocityDesired = 0;
-        break;
-    }
-
-    AltitudeHoldStatusSet(&altitudeHoldStatus);
-
-    switch (thrustMode) {
-    case DIRECT:
-        thrustDemand = thrustSetpoint;
-        break;
-    default:
-    {
-        // velocity control loop
-        // No scaling.
-        const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
-        thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
-    }
-    break;
-    }
-}
-
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     AltitudeHoldSettingsGet(&altitudeHoldSettings);
-    pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
-    pid_zero(&pid0);
-    pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
-    pid_zero(&pid1);
-}
 
-static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+    controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
-{
+                                 altitudeHoldSettings.VerticalVelPID.Ki,
-    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+                                 altitudeHoldSettings.VerticalVelPID.Kd,
+                                 altitudeHoldSettings.VerticalVelPID.Beta,
+                                 (float)(UPDATE_EXPECTED),
+                                 altitudeHoldSettings.ThrustRate);
+
+    controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
+
+    // initialise limits on thrust but note the FSM can override.
+    controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
+
+    // disable neutral thrust calcs which should only be done in a hold mode.
+    controlDown.DisableNeutralThrustCalc();
 }
 
 

flight/modules/Stabilization/inc/altitudeloop.h

@@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
 
 void stabilizationAltitudeloopInit();
 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
+void stabilizationDisableAltitudeHold(void);
 
 #endif /* ALTITUDELOOP_H */

flight/modules/Stabilization/outerloop.c

@@ -103,7 +103,33 @@ static void stabilizationOuterloopTask()
     float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
     float *rateDesiredAxis = &rateDesired.Roll;
     int t;
-    float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+    float dT    = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+
+    bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
+    previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+#ifdef REVOLUTION
+    if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
+                   previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
+        // disable the altvario velocity control loop
+        stabilizationDisableAltitudeHold();
+    }
+#endif /* REVOLUTION */
+    switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
+#ifdef REVOLUTION
+    case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
+        break;
+    case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
+        break;
+#endif /* REVOLUTION */
+    case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+    case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
+    default:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+        break;
+    }
+
 
     float local_error[3];
     {
@@ -151,151 +177,134 @@ static void stabilizationOuterloopTask()
     }
 
 
-    for (t = 0; t < AXES; t++) {
+    for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
-        bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
+        reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
         previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
 
-        if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
+        if (reinit) {
-            if (reinit) {
+            stabSettings.outerPids[t].iAccumulator = 0;
-                stabSettings.outerPids[t].iAccumulator = 0;
+        }
-            }
+        switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
-            switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
+        case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
-            case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
+            rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
-                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] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[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),
-                                            -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
-                                            StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[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:
+        case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
-                // FIXME: local_error[] is rate - attitude for Weak Leveling
+        {
-                // The only ramifications are:
+            float stickinput[3];
-                // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
+            stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
-                // Changing Rate mode max rate currently requires a change to Kp
+            stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
-                // That would be changed to Attitude mode max angle affecting Kp
+            stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
-                // Also does not take dT into account
+            float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
-            {
+            // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
-                float stickinput[3];
+            rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-                stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+                                        -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
-                stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+                                        StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
-                stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+                                        );
-                float rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            // Compute the weighted average rate desired
-                float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
+            // Using max() rather than sqrt() for cpu speed;
-                weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
+            // - this makes the stick region into a square;
-
+            // - this is a feature!
-                // Compute desired rate as input biased towards leveling
+            // - hold a roll angle and add just pitch without the stick sensitivity changing
-                rateDesiredAxis[t] = rate_input + weak_leveling;
+            float magnitude = fabsf(stickinput[t]);
+            if (t < 2) {
+                magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
             }
-            break;
+
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
+            // modify magnitude to move the Att to Rate transition to the place
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
+            // specified by the user
-                // now test limits for pitch and/or roll
+            // we are looking for where the stick angle == transition angle
-                if (t == 1) { // pitch
+            // and the Att rate equals the Rate rate
-                    if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
+            // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
-                        // attitude exceeds pitch max.
+            // == Rate x StickAngle [Rate pulling up according to stick angle]
-                        // zero rate desired if also -ve
+            // * StickAngle [X Ratt proportion]
-                        if (rateDesiredAxis[t] < 0.0f) {
+            // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
-                            rateDesiredAxis[t] = 0.0f;
+            // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
-                        }
+            // and quadratic formula says that is 0.618033989f
-                    } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
+            // I tested 14.01 and came up with .61 without even remembering this number
-                        // attitude exceeds pitch max
+            // I thought that moving the P,I, and maxangle terms around would change this value
-                        // zero rate desired if also +ve
+            // and that I would have to take these into account, but varying
-                        if (rateDesiredAxis[t] > 0.0f) {
+            // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
-                            rateDesiredAxis[t] = 0.0f;
+            // 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 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 rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[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_DIRECTWITHLIMITS:
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
+            // now test limits for pitch and/or roll
+            if (t == 1) { // pitch
+                if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
+                    // attitude exceeds pitch max.
+                    // zero rate desired if also -ve
+                    if (rateDesiredAxis[t] < 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
                     }
-                } else if (t == 0) { // roll
+                } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
-                    if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
+                    // attitude exceeds pitch max
-                        // attitude exceeds roll max.
+                    // zero rate desired if also +ve
-                        // zero rate desired if also -ve
+                    if (rateDesiredAxis[t] > 0.0f) {
-                        if (rateDesiredAxis[t] < 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
-                            rateDesiredAxis[t] = 0.0f;
-                        }
-                    } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
-                        // attitude exceeds roll max
-                        // zero rate desired if also +ve
-                        if (rateDesiredAxis[t] > 0.0f) {
-                            rateDesiredAxis[t] = 0.0f;
-                        }
                     }
                 }
-                break;
+            } else if (t == 0) { // roll
+                if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
+                    // attitude exceeds roll max.
+                    // zero rate desired if also -ve
+                    if (rateDesiredAxis[t] < 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
+                    }
+                } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
+                    // attitude exceeds roll max
+                    // zero rate desired if also +ve
+                    if (rateDesiredAxis[t] > 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
+                    }
+                }
+            }
+            break;
 
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+        case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-            default:
+        default:
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t];
-                break;
+            break;
-            }
-        } else {
-            switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
-#ifdef REVOLUTION
-            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
-                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
-                break;
-            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
-                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
-                break;
-#endif /* REVOLUTION */
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-            default:
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
-                break;
-            }
         }
     }
 
@@ -314,7 +323,7 @@ static void stabilizationOuterloopTask()
         }
     }
 
-    // update cruisecontrol based on attitude
+// update cruisecontrol based on attitude
     cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
     stabSettings.monitor.rateupdates = 0;
 }

flight/modules/TxPID/txpid.c

@@ -55,6 +55,9 @@
 #include "accessorydesired.h"
 #include "manualcontrolcommand.h"
 #include "stabilizationsettings.h"
+#ifdef REVOLUTION
+#include "altitudeholdsettings.h"
+#endif
 #include "stabilizationbank.h"
 #include "stabilizationsettingsbank1.h"
 #include "stabilizationsettingsbank2.h"
@@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void)
     bool txPIDEnabled;
     HwSettingsOptionalModulesData optionalModules;
 
+#ifdef REVOLUTION
+    AltitudeHoldSettingsInitialize();
+#endif
+
     HwSettingsInitialize();
     HwSettingsOptionalModulesGet(&optionalModules);
 
@@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev)
     }
     StabilizationSettingsData stab;
     StabilizationSettingsGet(&stab);
+#ifdef REVOLUTION
+    AltitudeHoldSettingsData altitude;
+    AltitudeHoldSettingsGet(&altitude);
+#endif
     AccessoryDesiredData accessory;
 
-    uint8_t needsUpdateBank = 0;
+    uint8_t needsUpdateBank     = 0;
-    uint8_t needsUpdateStab = 0;
+    uint8_t needsUpdateStab     = 0;
+#ifdef REVOLUTION
+    uint8_t needsUpdateAltitude = 0;
+#endif
 
     // Loop through every enabled instance
     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
                 needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
                 break;
+#ifdef REVOLUTION
+            case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
+                needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
+                break;
+#endif
             default:
                 PIOS_Assert(0);
             }
@@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev)
     if (needsUpdateStab) {
         StabilizationSettingsSet(&stab);
     }
+#ifdef REVOLUTION
+    if (needsUpdateAltitude) {
+        AltitudeHoldSettingsSet(&altitude);
+    }
+#endif
     if (needsUpdateBank) {
         switch (inst.BankNumber) {
         case 0:

flight/targets/boards/discoveryf4bare/firmware/Makefile

@@ -24,6 +24,9 @@ endif
 include ../board-info.mk
 include $(ROOT_DIR)/make/firmware-defs.mk
 
+# REVO C++ support 
+USE_CXX = YES
+
 # ARM DSP library
 USE_DSP_LIB ?= NO
 
@@ -70,7 +73,7 @@ ifndef TESTAPP
     ## Application Core
     SRC += ../pios_usb_board_data.c
     SRC += $(OPMODULEDIR)/System/systemmod.c
-    SRC += $(OPSYSTEM)/discoveryf4bare.c
+    CPPSRC += $(OPSYSTEM)/discoveryf4bare.cpp
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
     SRC += $(OPUAVTALK)/uavtalk.c
@@ -91,6 +94,7 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/insgps13state.c
     SRC += $(FLIGHTLIB)/auxmagsupport.c
     SRC += $(FLIGHTLIB)/lednotification.c    
+    CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
 
     ## UAVObjects
     include ./UAVObjects.inc

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

@@ -19,6 +19,8 @@
 # These are the UAVObjects supposed to be build as part of the OpenPilot target
 # (all architectures)
 UAVOBJSRCFILENAMES =
+UAVOBJSRCFILENAMES += statusgrounddrive
+UAVOBJSRCFILENAMES += pidstatus
 UAVOBJSRCFILENAMES += statusvtolland
 UAVOBJSRCFILENAMES += statusvtolautotakeoff
 UAVOBJSRCFILENAMES += vtolselftuningstats

flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp

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

flight/targets/boards/revoproto/firmware/Makefile

@@ -24,6 +24,9 @@ endif
 include ../board-info.mk
 include $(ROOT_DIR)/make/firmware-defs.mk
 
+# REVO C++ support 
+USE_CXX = YES
+
 # ARM DSP library
 USE_DSP_LIB ?= NO
 
@@ -65,7 +68,7 @@ ifndef TESTAPP
     ## Application Core
     SRC += ../pios_usb_board_data.c
     SRC += $(OPMODULEDIR)/System/systemmod.c
-    SRC += $(OPSYSTEM)/revolution.c
+    CPPSRC += $(OPSYSTEM)/revolution.cpp
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
     SRC += $(OPUAVTALK)/uavtalk.c
@@ -84,6 +87,8 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
     SRC += $(FLIGHTLIB)/auxmagsupport.c
+    CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
+
 
     ## UAVObjects
     include ./UAVObjects.inc

flight/targets/boards/revoproto/firmware/revolution.cpp

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

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

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>974</width>
-    <height>857</height>
+    <height>755</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -136,8 +136,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>939</width>
+            <width>950</width>
-            <height>776</height>
+            <height>775</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_3">
@@ -8244,8 +8244,8 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
+            <width>950</width>
-            <height>763</height>
+            <height>736</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -18236,8 +18236,8 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
+            <width>950</width>
-            <height>763</height>
+            <height>671</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_8" stretch="0,0,0,0,0,0">
@@ -24082,8 +24082,8 @@ font:bold;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
+            <width>950</width>
-            <height>763</height>
+            <height>671</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_18">
@@ -25209,8 +25209,7 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:AltitudePI</string>
+                     <string>fieldname:VerticalPosP</string>
-                     <string>element:Kp</string>
                      <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98,10</string>
@@ -25294,15 +25293,18 @@ border-radius: 5;</string>
                   </widget>
                  </item>
                  <item row="3" column="1">
-                  <widget class="QSlider" name="AltKdSlider">
+                  <widget class="QSlider" name="VelKiSlider">
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &amp;quot;throttle trim&amp;quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should attain its target velocity.  For neutral throttle estimation,  the altitude module assumes that when engaged the throttle thrust limit neutral setting is in the range required to hover. If the throttle required is a lot higher or lower, it needs to adjust this &amp;quot;throttle trim&amp;quot;. Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="maximum">
-                    <number>1000</number>
+                    <number>100</number>
+                   </property>
+                   <property name="singleStep">
+                    <number>1</number>
                    </property>
                    <property name="value">
-                    <number>50</number>
+                    <number>25</number>
                    </property>
                    <property name="orientation">
                     <enum>Qt::Horizontal</enum>
@@ -25313,17 +25315,17 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:VelocityPI</string>
+                     <string>fieldname:VerticalVelPID</string>
                      <string>element:Ki</string>
-                     <string>scale:0.00001</string>
+                     <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>
                    </property>
                   </widget>
                  </item>
-                 <item row="2" column="2">
+                 <item row="3" column="2">
-                  <widget class="QSpinBox" name="AltKi">
+                  <widget class="QSpinBox" name="VelKi">
                    <property name="sizePolicy">
                     <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
                      <horstretch>0</horstretch>
@@ -25355,13 +25357,13 @@ border-radius: 5;</string>
                     <number>100</number>
                    </property>
                    <property name="value">
-                    <number>51</number>
+                    <number>25</number>
                    </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:VelocityPI</string>
+                     <string>fieldname:VerticalVelPID</string>
-                     <string>element:Kp</string>
+                     <string>element:Ki</string>
                      <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98,10</string>
@@ -25369,8 +25371,68 @@ border-radius: 5;</string>
                    </property>
                   </widget>
                  </item>
-                 <item row="3" column="2">
+                 <item row="4" column="0">
-                  <widget class="QSpinBox" name="AltKd">
+                  <widget class="QLabel" name="label_25_kd">
+                   <property name="sizePolicy">
+                    <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+                     <horstretch>0</horstretch>
+                     <verstretch>0</verstretch>
+                    </sizepolicy>
+                   </property>
+                   <property name="minimumSize">
+                    <size>
+                     <width>58</width>
+                     <height>0</height>
+                    </size>
+                   </property>
+                   <property name="maximumSize">
+                    <size>
+                     <width>16777215</width>
+                     <height>16777215</height>
+                    </size>
+                   </property>
+                   <property name="styleSheet">
+                    <string notr="true"/>
+                   </property>
+                   <property name="text">
+                    <string>Velocity Derivative</string>
+                   </property>
+                   <property name="alignment">
+                    <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+                   </property>
+                  </widget>
+                 </item>
+                 <item row="4" column="1">
+                  <widget class="QSlider" name="VelKdSlider">
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Small abouts of Kd can reduce oscillations in the velocity controller.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
+                   <property name="maximum">
+                    <number>300</number>
+                   </property>
+                   <property name="value">
+                    <number>50</number>
+                   </property>
+                   <property name="orientation">
+                    <enum>Qt::Horizontal</enum>
+                   </property>
+                   <property name="tickPosition">
+                    <enum>QSlider::TicksBelow</enum>
+                   </property>
+                   <property name="objrelation" stdset="0">
+                    <stringlist>
+                     <string>objname:AltitudeHoldSettings</string>
+                     <string>fieldname:VerticalVelPID</string>
+                     <string>element:Kd</string>
+                     <string>scale:0.0001</string>
+                     <string>haslimits:yes</string>
+                     <string>buttongroup:98</string>
+                    </stringlist>
+                   </property>
+                  </widget>
+                 </item>
+                 <item row="4" column="2">
+                  <widget class="QSpinBox" name="VelKd">
                    <property name="sizePolicy">
                     <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
                      <horstretch>0</horstretch>
@@ -25399,7 +25461,7 @@ border-radius: 5;</string>
                     <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
                    </property>
                    <property name="maximum">
-                    <number>1000</number>
+                    <number>300</number>
                    </property>
                    <property name="value">
                     <number>51</number>
@@ -25407,9 +25469,172 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:VelocityPI</string>
+                     <string>fieldname:VerticalVelPID</string>
-                     <string>element:Ki</string>
+                     <string>element:Kd</string>
-                     <string>scale:0.00001</string>
+                     <string>scale:0.0001</string>
+                     <string>haslimits:yes</string>
+                     <string>buttongroup:98,10</string>
+                    </stringlist>
+                   </property>
+                  </widget>
+                 </item>
+                 <item row="5" column="0">
+                  <widget class="QLabel" name="label_25_beta">
+                   <property name="sizePolicy">
+                    <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+                     <horstretch>0</horstretch>
+                     <verstretch>0</verstretch>
+                    </sizepolicy>
+                   </property>
+                   <property name="minimumSize">
+                    <size>
+                     <width>58</width>
+                     <height>0</height>
+                    </size>
+                   </property>
+                   <property name="maximumSize">
+                    <size>
+                     <width>16777215</width>
+                     <height>16777215</height>
+                    </size>
+                   </property>
+                   <property name="styleSheet">
+                    <string notr="true"/>
+                   </property>
+                   <property name="text">
+                    <string>Velocity Beta</string>
+                   </property>
+                   <property name="alignment">
+                    <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+                   </property>
+                  </widget>
+                 </item>
+                 <item row="5" column="1">
+                  <widget class="QSlider" name="VelBetaSlider">
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The beta value applies a setpoint weighting that reduces the sensitivity to quick changes in the desired velocity.  Transitions from altitude hold to descent/climb can be made smooth applying a Beta value of around 80 to 90%.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
+                   <property name="minimum">
+                    <number>70</number>
+                   </property>
+                   <property name="maximum">
+                    <number>100</number>
+                   </property>
+                   <property name="value">
+                    <number>90</number>
+                   </property>
+                   <property name="sliderPosition">
+                    <number>90</number>
+                   </property>
+                   <property name="orientation">
+                    <enum>Qt::Horizontal</enum>
+                   </property>
+                   <property name="tickPosition">
+                    <enum>QSlider::TicksBelow</enum>
+                   </property>
+                   <property name="objrelation" stdset="0">
+                    <stringlist>
+                     <string>objname:AltitudeHoldSettings</string>
+                     <string>fieldname:VerticalVelPID</string>
+                     <string>element:Beta</string>
+                     <string>scale:0.01</string>
+                     <string>haslimits:yes</string>
+                     <string>buttongroup:98</string>
+                    </stringlist>
+                   </property>
+                  </widget>
+                 </item>
+                 <item row="5" column="2">
+                  <widget class="QSpinBox" name="VelBeta">
+                   <property name="sizePolicy">
+                    <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                     <horstretch>0</horstretch>
+                     <verstretch>0</verstretch>
+                    </sizepolicy>
+                   </property>
+                   <property name="minimumSize">
+                    <size>
+                     <width>50</width>
+                     <height>22</height>
+                    </size>
+                   </property>
+                   <property name="maximumSize">
+                    <size>
+                     <width>50</width>
+                     <height>22</height>
+                    </size>
+                   </property>
+                   <property name="focusPolicy">
+                    <enum>Qt::StrongFocus</enum>
+                   </property>
+                   <property name="styleSheet">
+                    <string notr="true"/>
+                   </property>
+                   <property name="alignment">
+                    <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+                   </property>
+                   <property name="minimum">
+                    <number>70</number>
+                   </property>
+                   <property name="maximum">
+                    <number>100</number>
+                   </property>
+                   <property name="value">
+                    <number>90</number>
+                   </property>
+                   <property name="objrelation" stdset="0">
+                    <stringlist>
+                     <string>objname:AltitudeHoldSettings</string>
+                     <string>fieldname:VerticalVelPID</string>
+                     <string>element:Beta</string>
+                     <string>scale:0.01</string>
+                     <string>haslimits:yes</string>
+                     <string>buttongroup:98,10</string>
+                    </stringlist>
+                   </property>
+                  </widget>
+                 </item>
+                 <item row="2" column="2">
+                  <widget class="QSpinBox" name="VelKp">
+                   <property name="sizePolicy">
+                    <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                     <horstretch>0</horstretch>
+                     <verstretch>0</verstretch>
+                    </sizepolicy>
+                   </property>
+                   <property name="minimumSize">
+                    <size>
+                     <width>50</width>
+                     <height>22</height>
+                    </size>
+                   </property>
+                   <property name="maximumSize">
+                    <size>
+                     <width>50</width>
+                     <height>22</height>
+                    </size>
+                   </property>
+                   <property name="focusPolicy">
+                    <enum>Qt::StrongFocus</enum>
+                   </property>
+                   <property name="styleSheet">
+                    <string notr="true"/>
+                   </property>
+                   <property name="alignment">
+                    <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+                   </property>
+                   <property name="maximum">
+                    <number>100</number>
+                   </property>
+                   <property name="value">
+                    <number>50</number>
+                   </property>
+                   <property name="objrelation" stdset="0">
+                    <stringlist>
+                     <string>objname:AltitudeHoldSettings</string>
+                     <string>fieldname:VerticalVelPID</string>
+                     <string>element:Kp</string>
+                     <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98,10</string>
                     </stringlist>
@@ -25419,7 +25644,7 @@ border-radius: 5;</string>
                  <item row="1" column="1">
                   <widget class="QSlider" name="AltKpSlider">
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should climb or descent to compensate a certain altitude difference.  Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="maximum">
                     <number>100</number>
@@ -25434,10 +25659,9 @@ border-radius: 5;</string>
                     <enum>QSlider::TicksBelow</enum>
                    </property>
                    <property name="objrelation" stdset="0">
-                    <stringlist>
+                    <stringlist notr="true">
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:AltitudePI</string>
+                     <string>fieldname:VerticalPosP</string>
-                     <string>element:Kp</string>
                      <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
@@ -25446,7 +25670,7 @@ border-radius: 5;</string>
                   </widget>
                  </item>
                  <item row="2" column="1">
-                  <widget class="QSlider" name="AltKiSlider">
+                  <widget class="QSlider" name="VelKpSlider">
                    <property name="toolTip">
                     <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How much the vehicle should throttle up or down to compensate or achieve a certain vertical speed. Higher values lead to more aggressive throttle changes and could lead to oscillations. This is the most likely candidate to change depending on the crafts engine thrust. Heavy craft with weak engines might require higher values.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
@@ -25465,7 +25689,7 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:VelocityPI</string>
+                     <string>fieldname:VerticalVelPID</string>
                      <string>element:Kp</string>
                      <string>scale:0.01</string>
                      <string>haslimits:yes</string>
@@ -27173,10 +27397,10 @@ Useful if you have accidentally changed some settings.</string>
   <tabstop>pushButton_7</tabstop>
   <tabstop>AltKpSlider</tabstop>
   <tabstop>AltKp</tabstop>
-  <tabstop>AltKiSlider</tabstop>
+  <tabstop>VelKpSlider</tabstop>
-  <tabstop>AltKi</tabstop>
+  <tabstop>VelKp</tabstop>
-  <tabstop>AltKdSlider</tabstop>
+  <tabstop>VelKiSlider</tabstop>
-  <tabstop>AltKd</tabstop>
+  <tabstop>VelKi</tabstop>
   <tabstop>pushButton_8</tabstop>
   <tabstop>AltThrExpSlider_2</tabstop>
   <tabstop>AltThrExp_2</tabstop>

make/apps-defs.mk

@@ -33,6 +33,10 @@ FLIGHTLIBINC	= $(FLIGHTLIB)/inc
 OPUAVOBJINC	= $(OPUAVOBJ)/inc
 OPUAVTALKINC	= $(OPUAVTALK)/inc
 
+## PID 
+PIDLIB		=$(FLIGHTLIB)/pid
+PIDLIBINC	=$(FLIGHTLIB)/pid
+
 ## Math
 MATHLIB		= $(FLIGHTLIB)/math
 MATHLIBINC	= $(FLIGHTLIB)/math
@@ -88,7 +92,10 @@ SRC += $(PIOSCOMMON)/pios_sensors.c
 SRC += $(FLIGHTLIB)/sanitycheck.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
 SRC += $(MATHLIB)/sin_lookup.c
+
+## PID library functions
 SRC += $(MATHLIB)/pid.c
+CPPSRC += $(PIDLIB)/pidcontroldown.cpp
 
 ## PIOS Hardware (Common)
 SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c
@@ -167,6 +174,7 @@ EXTRAINCDIRS += $(FLIGHTLIBINC)
 EXTRAINCDIRS += $(PIOSCOMMON)
 EXTRAINCDIRS += $(OPSYSTEMINC)
 EXTRAINCDIRS += $(MATHLIBINC)
+EXTRAINCDIRS += $(PIDLIBINC)
 EXTRAINCDIRS += $(OPUAVOBJINC)
 EXTRAINCDIRS += $(OPUAVTALKINC)
 EXTRAINCDIRS += $(OPUAVSYNTHDIR)

shared/uavobjectdefinition/altitudeholdsettings.xml

@@ -1,14 +1,15 @@
 <xml>
     <object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
-        <description>Settings for the @ref AltitudeHold module</description>
+    <description>Settings for the @ref AltitudeHold module</description>
-	<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
-	<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.3,0.3,2.0" />
 	<field name="CutThrustWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
-        <field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
+    <field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
-        <field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
+    <field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
-        <access gcs="readwrite" flight="readwrite"/>
+    <field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
-        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+    <field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
-        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+    <field name="VerticalVelPID" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.5, 0.45, 0.001, 0.95"/>
-        <logging updatemode="manual" period="0"/>
+    <access gcs="readwrite" flight="readwrite"/>
+    <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+    <telemetryflight acked="true" updatemode="onchange" period="0"/>
+    <logging updatemode="manual" period="0"/>
     </object>
 </xml>

shared/uavobjectdefinition/altitudeholdstatus.xml

@@ -2,6 +2,8 @@
     <object name="AltitudeHoldStatus" singleinstance="true" settings="false" category="Control">
         <description>Status Data from Altitude Hold Control Loops</description>
         <field name="VelocityDesired" units="m/s" type="float" elements="1"/>
+        <field name="State" units="" type="enum" elements="1" options="Direct,AltitudeVario,AltitudeHold" defaultvalue="Direct"/>
+        <field name="ThrustDemand" 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/txpidsettings.xml

@@ -20,7 +20,7 @@
 			Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp,
 			Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp,
 			Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo,
-			GyroTau,AcroPlusFactor"
+			GyroTau,AcroPlusFactor,Altitude Pos.Kp,Altitude Velocity.Kp,Altitude Velocity.Ki,Altitude Velocity.Kd,Altitude Velocity.Beta"
 		defaultvalue="Disabled"/>
         <field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>
         <field name="MaxPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>

shared/uavobjectdefinition/vtolpathfollowersettings.xml

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