OP-1848 altvario improvements

1. Avoid neutral thrust calcs in altvario typically used in forward flight
2. Capture stick state for debugging into altitudeholdstatus

flight/libraries/pid/pidcontroldown.cpp

@@ -59,7 +59,7 @@ extern "C" {
 PIDControlDown::PIDControlDown()
     : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(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();
 }

flight/libraries/pid/pidcontroldown.h

@@ -58,6 +58,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();
@@ -77,7 +85,6 @@ private:
     float mPositionState;
     float mMinThrust;
     float mMaxThrust;
-    uint8_t mActive;
 
     struct NeutralThrustEstimation {
         uint32_t count;
@@ -90,6 +97,8 @@ private:
         bool     have_correction;
     };
     struct NeutralThrustEstimation neutralThrustEst;
+    bool mActive;
+    bool mAllowNeutralThrustCalc;
 };
 
 #endif // PIDCONTROLDOWN_H

flight/modules/PathFollower/vtolflycontroller.cpp

@@ -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/Stabilization/altitudeloop.cpp

@@ -141,17 +141,20 @@ static void altitudeHoldTask(void)
         controlDown.UpdatePositionState(positionStateDown);
         controlDown.ControlPosition();
         altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
         thrustDemand = controlDown.GetDownCommand();
     }
     break;
 
     case ALTITUDEVARIO:
         altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
         thrustDemand = controlDown.GetDownCommand();
         break;
 
     case DIRECT:
         altitudeHoldStatus.VelocityDesired = 0.0f;
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
         break;
     }
 
@@ -202,6 +205,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 
     // 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();
 }
 
 

shared/uavobjectdefinition/altitudeholdstatus.xml

@@ -2,6 +2,7 @@
     <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"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>