OP-1848 altvario improvements

1. Disable velocity pid loop when altvario is not active
2. Add sanity boundf to ensure max min are inforced.

flight/libraries/pid/pidcontroldown.h

@@ -44,6 +44,10 @@ public:
     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);

flight/modules/Stabilization/altitudeloop.cpp

@@ -81,7 +81,7 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
 {
     static bool newaltitude = true;
 
-    if (reinit) {
+    if (reinit || !controlDown.IsActive()) {
         controlDown.Activate();
         newaltitude = true;
     }
@@ -97,6 +97,7 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
         thrustDemand = 0.0f;
         thrustMode   = DIRECT;
         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
@@ -116,23 +117,38 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
         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)
 {
-    AltitudeHoldStatusData altitudeHoldStatus;
+    if (!controlDown.IsActive()) {
+        return;
+    }
 
+    AltitudeHoldStatusData altitudeHoldStatus;
     AltitudeHoldStatusGet(&altitudeHoldStatus);
 
     float velocityStateDown;
     VelocityStateDownGet(&velocityStateDown);
     controlDown.UpdateVelocityState(velocityStateDown);
 
+    float local_thrustDemand = 0.0f;
+
     switch (thrustMode) {
     case ALTITUDEHOLD:
     {
@@ -142,14 +158,14 @@ static void altitudeHoldTask(void)
         controlDown.ControlPosition();
         altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
         altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
-        thrustDemand = controlDown.GetDownCommand();
+        local_thrustDemand = controlDown.GetDownCommand();
     }
     break;
 
     case ALTITUDEVARIO:
         altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
         altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
-        thrustDemand = controlDown.GetDownCommand();
+        local_thrustDemand = controlDown.GetDownCommand();
         break;
 
     case DIRECT:
@@ -159,6 +175,7 @@ static void altitudeHoldTask(void)
     }
 
     AltitudeHoldStatusSet(&altitudeHoldStatus);
+    thrustDemand = local_thrustDemand;
 }
 
 static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)

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

@@ -107,6 +107,11 @@ static void stabilizationOuterloopTask()
 
     bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
     previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+    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();
+    }
     switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
 #ifdef REVOLUTION
     case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:

shared/uavobjectdefinition/altitudeholdstatus.xml

@@ -3,6 +3,7 @@
         <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"/>