new design of altitude hold - warning not tested yet!

flight/modules/AltitudeHold/altitudehold.c

@@ -46,39 +46,37 @@
 #include <openpilot.h>
 
 #include <math.h>
+#include <pid.h>
 #include <CoordinateConversions.h>
-#include <altholdsmoothed.h>
 #include <attitudestate.h>
 #include <altitudeholdsettings.h>
 #include <altitudeholddesired.h> // object that will be updated by the module
-#include <barosensor.h>
-#include <positionstate.h>
 #include <flightstatus.h>
 #include <stabilizationdesired.h>
 #include <accelstate.h>
-#include <taskinfo.h>
 #include <pios_constants.h>
 #include <velocitystate.h>
 #include <positionstate.h>
 // Private constants
-#define MAX_QUEUE_SIZE         2
+
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
+#define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
+
 #define STACK_SIZE_BYTES       1024
-#define TASK_PRIORITY          (tskIDLE_PRIORITY + 1)
-#define ACCEL_DOWNSAMPLE       4
-#define TIMEOUT_TRESHOLD       200000
 #define DESIRED_UPDATE_RATE_MS 100 // milliseconds
 // Private types
 
 // Private variables
-static xTaskHandle altitudeHoldTaskHandle;
+static DelayedCallbackInfo *altitudeHoldCBInfo;
-static xQueueHandle queue;
 static AltitudeHoldSettingsData altitudeHoldSettings;
-static float throttleAlpha = 1.0f;
+static struct pid accelpid;
-static float throttle_old  = 0.0f;
+static float accelStateDown;
+
 
 // Private functions
-static void altitudeHoldTask(void *parameters);
+static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
+static void AccelStateUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Initialise the module, called on startup
@@ -87,8 +85,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
 int32_t AltitudeHoldStart()
 {
     // Start main task
-    xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
+    SettingsUpdatedCb(NULL);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
+    DelayedCallbackDispatch(altitudeHoldCBInfo);
 
     return 0;
 }
@@ -101,216 +99,111 @@ int32_t AltitudeHoldInitialize()
 {
     AltitudeHoldSettingsInitialize();
     AltitudeHoldDesiredInitialize();
-    AltHoldSmoothedInitialize();
 
     // Create object queue
-    queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 
+    altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
     AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
+    AccelStateConnectCallback(&AccelStateUpdatedCb);
 
     return 0;
 }
 MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
 
-float tau;
-float positionAlpha;
-float velAlpha;
-bool running = false;
-
-float velocity;
-float velocityIntegral;
-float altitudeIntegral;
-float error;
-float velError;
-float derivative;
-uint32_t timeval;
-bool posUpdated;
 
 /**
  * Module thread, should not return.
  */
-static void altitudeHoldTask(__attribute__((unused)) void *parameters)
+static void altitudeHoldTask(void)
 {
-    AltitudeHoldDesiredData altitudeHoldDesired;
-    StabilizationDesiredData stabilizationDesired;
-    AltHoldSmoothedData altHold;
-    VelocityStateData velocityData;
-    float dT;
-    float fblimit = 0;
 
-    portTickType thisTime, lastUpdateTime;
+	static float startThrottle =0.5f;
-    UAVObjEvent ev;
 
-    dT = 0;
+	// make sure we run only when we are supposed to run
-    timeval = 0;
+	FlightStatusData flightStatus;
-    lastUpdateTime = 0;
+	FlightStatusGet(&flightStatus);
-    // Force update of the settings
+	switch (flightStatus.FlightMode) {
-    SettingsUpdatedCb(&ev);
+		case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-    // Failsafe handling
+		case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
-    uint32_t lastAltitudeHoldDesiredUpdate = 0;
+			break;
-    bool enterFailSafe = false;
+		default:
-    // Listen for updates.
+			pid_zero(&accelpid);
-    AltitudeHoldDesiredConnectQueue(queue);
+			StabilizationDesiredThrottleGet(&startThrottle);
-    // PositionStateConnectQueue(queue);
+			DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-    FlightStatusConnectQueue(queue);
+			return;
-    VelocityStateConnectQueue(queue);
+			break;
-    bool altitudeHoldFlightMode = false;
+	}
-    running = false;
-    enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
 
-    uint8_t flightMode;
+	// do the actual control loop(s)
-    FlightStatusFlightModeGet(&flightMode);
+	AltitudeHoldDesiredData altitudeHoldDesired;
-    // initialize enable flag
+	AltitudeHoldDesiredGet(&altitudeHoldDesired);
-    altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
+	float positionStateDown;
-    // Main task loop
+	PositionStateDownGet(&positionStateDown);
-    while (1) {
+	float velocityStateDown;
-        enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
+	VelocityStateDownGet(&velocityStateDown);
-        // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
-        if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
-            if (!running) {
-                altitudeIntegral = 0;
-            }
 
-            // Todo: Add alarm if it should be running
+	// altitude control loop
-            continue;
+	float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity;
-        } else if (ev.obj == FlightStatusHandle()) {
-            FlightStatusFlightModeGet(&flightMode);
-            altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
-            if (altitudeHoldFlightMode && !running) {
-                AttitudeStateData attitudeState;
-                float q[4], Rbe[3][3];
-                AttitudeStateGet(&attitudeState);
-                q[0] = attitudeState.q1;
-                q[1] = attitudeState.q2;
-                q[2] = attitudeState.q3;
-                q[3] = attitudeState.q4;
-                Quaternion2R(q, Rbe);
-                // Copy the current throttle as a starting point for integral
-                float initThrottle;
-                StabilizationDesiredThrottleGet(&initThrottle);
-                initThrottle *= Rbe[2][2]; // rotate into earth frame
-                if (initThrottle > 1) {
-                    initThrottle = 1;
-                } else if (initThrottle < 0) {
-                    initThrottle = 0;
-                }
-                error = 0;
-                altitudeHoldDesired.Velocity = 0;
-                altitudeHoldDesired.Altitude = altHold.Altitude;
-                altitudeIntegral = initThrottle;
-                velocityIntegral = 0;
-                running = true;
-            } else if (!altitudeHoldFlightMode) {
-                running = false;
-                lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-            }
-        } else if (ev.obj == VelocityStateHandle()) {
-            init    = (init == WAITING_BARO) ? WAITIING_INIT : init;
-            dT      = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT;
-            timeval = PIOS_DELAY_GetRaw();
 
-            AltHoldSmoothedGet(&altHold);
+	// velocity control loop
-
+	float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f;
-            VelocityStateGet(&velocityData);
-
-            altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down);
-
-            float position;
-            PositionStateDownGet(&position);
-            altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude;
-
-            AltHoldSmoothedSet(&altHold);
-
-            // Verify that we are in altitude hold mode
-            uint8_t armed;
-            FlightStatusArmedGet(&armed);
-            if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
-                running = false;
-            }
-
-            if (!running) {
-                lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-                continue;
-            }
-
-            float lastError = error;
-            error      = altitudeHoldDesired.Altitude - altHold.Altitude;
-            derivative = (error - lastError) / dT;
-
-            velError   = altitudeHoldDesired.Velocity - altHold.Velocity;
-
-            // Compute altitude and velocity integral
-            altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT;
-            velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT;
 
 
-            thisTime = xTaskGetTickCount();
+	// compensate acceleration by rotation
-            // Only update stabilizationDesired less frequently
+	// explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q
-            if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) {
+	// It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation
-                continue;
+	// multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical
-            }
+	// acceleration at the current tilt angle.
-            lastUpdateTime = thisTime;
+	// around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep
+	// integrals from winding in any direction
  
-            // Instead of explicit limit on integral you output limit feedback
+	AttitudeStateData attitudeState;
-            StabilizationDesiredGet(&stabilizationDesired);
+	AttitudeStateGet(&attitudeState);
-            if (!enterFailSafe) {
+	float Rbe[3][3];
-                stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral
+	Quaternion2R(&attitudeState.q1,Rbe);
-                                                + error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP]
-                                                + velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP]
-                                                + derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD];
 	
-                // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling
+	float rotatedAccelDesired = realAccelDesired;
-                AttitudeStateData attitudeState;
+	if (fabsf(Rbe[2][2])>1e-3f) {
-                float q[4], Rbe[3][3];
+		rotatedAccelDesired /= Rbe[2][2];
-                AttitudeStateGet(&attitudeState);
+	} else {
-                q[0] = attitudeState.q1;
+		rotatedAccelDesired = accelStateDown;
-                q[1] = attitudeState.q2;
+	}
-                q[2] = attitudeState.q3;
-                q[3] = attitudeState.q4;
-                Quaternion2R(q, Rbe);
-                float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
-                stabilizationDesired.Throttle /= throttlescale;
-                stabilizationDesired.Throttle  = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha);
-                throttle_old = stabilizationDesired.Throttle;
-                fblimit = 0;
 	
-                if (stabilizationDesired.Throttle > 1) {
+	// acceleration control loop
-                    fblimit = stabilizationDesired.Throttle - 1;
+	float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS);
-                    stabilizationDesired.Throttle = 1;
-                } else if (stabilizationDesired.Throttle < 0) {
-                    fblimit = stabilizationDesired.Throttle;
-                    stabilizationDesired.Throttle = 0;
-                }
-            } else {
-                // shutdown motors
-                stabilizationDesired.Throttle = -1;
-            }
-            stabilizationDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-            stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-            stabilizationDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-            stabilizationDesired.Roll  = altitudeHoldDesired.Roll;
-            stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
-            stabilizationDesired.Yaw   = altitudeHoldDesired.Yaw;
 
-            StabilizationDesiredSet(&stabilizationDesired);
+	if (throttle>=1.0f) {
-        } else if (ev.obj == AltitudeHoldDesiredHandle()) {
+		throttle=1.0f;
-            // reset the failsafe timer
+	}
-            lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
+	if (throttle<=0.0f) {
-            AltitudeHoldDesiredGet(&altitudeHoldDesired);
+		throttle=0.0f;
-        }
+	}
-    }
+	StabilizationDesiredData stab;
+	StabilizationDesiredGet(&stab);
+	stab.Roll = altitudeHoldDesired.Roll;
+	stab.Pitch = altitudeHoldDesired.Pitch;
+	stab.Yaw = altitudeHoldDesired.Yaw;
+	stab.Throttle = throttle;
+        stab.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stab.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+	
+	StabilizationDesiredSet(&stab);
+
+	DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
+}
+
+static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+	float down;
+	AccelStatezGet(&down);
+	accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha);
 }
 
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     AltitudeHoldSettingsGet(&altitudeHoldSettings);
-    positionAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.PositionTau);
+    pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit);
-    velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau);
+    pid_zero(&accelpid);
-
+    accelStateDown=0.0f;
-    // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency)
-    if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) {
-        throttleAlpha = (float)DESIRED_UPDATE_RATE_MS / ((float)DESIRED_UPDATE_RATE_MS + 1000.0f / (2.0f * M_PI_F * altitudeHoldSettings.ThrottleFilterCutoff));
-    } else {
-        throttleAlpha = 1.0f;
-    }
 }

flight/modules/ManualControl/manualcontrol.c

@@ -887,10 +887,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
     } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
         // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
         // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
-        altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate;
+        altitudeHoldDesiredData.Velocity = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
         altitudeHoldDesiredData.Altitude = posState.Down;
     } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
-        altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate;
+        altitudeHoldDesiredData.Velocity = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
         altitudeHoldDesiredData.Altitude = posState.Down;
     } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
         // Require the stick to enter the dead band before they can move height

shared/uavobjectdefinition/altitudeholdsettings.xml

@@ -1,13 +1,12 @@
 <xml>
     <object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
         <description>Settings for the @ref AltitudeHold module</description>
-        <field name="AltitudePID" units="throttle/m" type="float" elementnames="Kp,Ki,Kd" defaultvalue="0.18,0.06,0.01"/>
+	<field name="AltitudeP" units="(m/s)/m" type="float" elements="1" defaultvalue="2" />
-        <field name="VelocityPI" units="throttle/m" type="float" elementnames="Kp,Ki" defaultvalue="0.1,0.1"/>
+	<field name="VelocityP" units="(m/s^2)/(m/s)" type="float" elements="1" defaultvalue="0.5" />
-        <field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="AccelPI" units="throttle/(m/s^2)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.1,0.01,1.0" />
-        <field name="PositionTau" units="" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="AccelAlpha" units="" type="float" elements="1" defaultvalue="0.1" />
-        <field name="ThrottleFilterCutoff" units="Hz" type="float" elements="1" defaultvalue="2"/>
+        <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" />
-        <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
+        <field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5" />
-        <field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>