OP-1848 ready for review

AltVario:
1. Uses new PID scheme
2. Runs at the outer loop rate to save CPU cycles

flight/libraries/pid/pidcontroldowncallback.h

@@ -36,7 +36,7 @@ public:
     // PIDControlDownCalback() {};
     virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0;
     virtual float BoundVelocityDown(float velocity) = 0;
-    //virtual ~PIDControlDownCalback() = 0;
+    // virtual ~PIDControlDownCalback() = 0;
 };
 
 #endif // PIDCONTROLDOWNCALLBACK_H

flight/modules/PathFollower/inc/pathfollowerfsm.h

@@ -43,7 +43,7 @@ typedef enum {
     PFFSM_STATE_ABORT // Abort on error
 } PathFollowerFSMState_T;
 
-class PathFollowerFSM: public PIDControlDownCallback {
+class PathFollowerFSM : public PIDControlDownCallback {
 public:
     // PathFollowerFSM() {};
     virtual void Inactive(void) {}

flight/modules/Stabilization/altitudeloop.cpp

@@ -39,6 +39,7 @@ extern "C" {
 #include <velocitystate.h>
 #include <positionstate.h>
 #include <vtolselftuningstats.h>
+#include <stabilization.h>
 }
 
 #include <pidcontroldown.h>
@@ -56,27 +57,19 @@ extern "C" {
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
 #define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
 
-#define STACK_SIZE_BYTES  512
 // Private types
 
 // Private variables
 static PIDControlDown controlDown;
-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;
-    // this is the max speed in m/s at the extents of thrust
+static float thrustRate;
-    static float thrustRate;
+static uint8_t thrustExp;
-    static uint8_t thrustExp;
 
 
 // Private functions
-static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
-static void VelocityStateUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Setup mode and setpoint
@@ -122,6 +115,37 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
         newaltitude = false;
     }
 
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
+    float velocityStateDown;
+    VelocityStateDownGet(&velocityStateDown);
+    controlDown.UpdateVelocityState(velocityStateDown);
+
+    switch (thrustMode) {
+    case ALTITUDEHOLD:
+    {
+        float positionStateDown;
+        PositionStateDownGet(&positionStateDown);
+        controlDown.UpdatePositionState(positionStateDown);
+        controlDown.ControlPosition();
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        thrustDemand = controlDown.GetDownCommand();
+    }
+    break;
+
+    case ALTITUDEVARIO:
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        thrustDemand = controlDown.GetDownCommand();
+        break;
+
+    case DIRECT:
+        altitudeHoldStatus.VelocityDesired = 0;
+        break;
+    }
+
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
+
     return thrustDemand;
 }
 
@@ -134,99 +158,37 @@ void stabilizationAltitudeloopInit()
     AltitudeHoldStatusInitialize();
     PositionStateInitialize();
     VelocityStateInitialize();
+    VtolSelfTuningStatsInitialize();
 
-    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);
-
-    // Start main task
     SettingsUpdatedCb(NULL);
 }
 
 
-/**
- * Module thread, should not return.
- */
-static void altitudeHoldTask(void)
-{
-    AltitudeHoldStatusData altitudeHoldStatus;
-    AltitudeHoldStatusGet(&altitudeHoldStatus);
-
-    float velocityStateDown;
-    VelocityStateDownGet(&velocityStateDown);
-    controlDown.UpdateVelocityState(velocityStateDown);
-
-    float dT;
-    dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
-
-    switch (thrustMode) {
-    case ALTITUDEHOLD:
-    {
-        float positionStateDown;
-        PositionStateDownGet(&positionStateDown);
-        controlDown.UpdatePositionState(positionStateDown);
-        controlDown.ControlPosition();
-        altitudeHoldStatus.VelocityDesired  = controlDown.GetVelocityDesired();
-    }
-    break;
-    case ALTITUDEVARIO:
-        altitudeHoldStatus.VelocityDesired  = controlDown.GetVelocityDesired();
-        break;
-    default:
-        altitudeHoldStatus.VelocityDesired = 0;
-        break;
-    }
-
-    AltitudeHoldStatusSet(&altitudeHoldStatus);
-
-    switch (thrustMode) {
-    case DIRECT:
-        thrustDemand = thrustSetpoint;
-        break;
-    default:
-    {
-        thrustDemand = controlDown.GetDownCommand();
-    }
-    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);
 
-    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
+    controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
-                                    vtolPathFollowerSettings->LandVerticalVelPID.Ki,
+                                 altitudeHoldSettings.VerticalVelPID.Ki,
-                                    vtolPathFollowerSettings->LandVerticalVelPID.Kd,
+                                 altitudeHoldSettings.VerticalVelPID.Kd,
-                                    vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+                                 altitudeHoldSettings.VerticalVelPID.Beta,
-                                    UPDATE_EXPECTED,
+                                 (float)(OUTERLOOP_SKIPCOUNT * UPDATE_EXPECTED),
-                                    vtolPathFollowerSettings->VerticalVelMax);
+                                 altitudeHoldSettings.VerticalVelMax);
 
-    // The following is not currently used in the landing control.
+    controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
-    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
 
     VtolSelfTuningStatsData vtolSelfTuningStats;
     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
-    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
 
     // initialise limits on thrust but note the FSM can override.
-    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+    controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
 
     AltitudeHoldSettingsThrustExpGet(&thrustExp);
     AltitudeHoldSettingsThrustRateGet(&thrustRate);
 }
 
-static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
-}
-
 
 #endif /* ifdef REVOLUTION */

shared/uavobjectdefinition/altitudeholdsettings.xml

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