Merge branch 'corvuscorax/OP-1036_fixed-wing-improvements' into next

flight/libraries/math/pid.c

@@ -69,6 +69,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
 /**
  * Update the PID computation with setpoint weighting on the derivative
  * @param[in] pid The PID struture which stores temporary information
+ * @param[in] factor A dynamic factor to scale pid's by, to compensate nonlinearities
  * @param[in] setpoint The setpoint to use
  * @param[in] measured The measured value of output
  * @param[in] dT  The time step
@@ -77,12 +78,12 @@ float pid_apply(struct pid *pid, const float err, float dT)
  * This version of apply uses setpoint weighting for the derivative component so the gain
  * on the gyro derivative can be different than the gain on the setpoint derivative
  */
-float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT)
+float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT)
 {
     float err = setpoint - measured;
 
     // Scale up accumulator by 1000 while computing to avoid losing precision
-    pid->iAccumulator += err * (pid->i * dT * 1000.0f);
+    pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
     pid->iAccumulator  = bound(pid->iAccumulator, pid->iLim * 1000.0f);
 
     // Calculate DT1 term,
@@ -90,11 +91,11 @@ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float meas
     float diff  = ((deriv_gamma * setpoint - measured) - pid->lastErr);
     pid->lastErr = (deriv_gamma * setpoint - measured);
     if (pid->d > 0.0f && dT > 0.0f) {
-        dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
+        dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer);
         pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
     } // 7.9577e-3  means 20 Hz f_cutoff
 
-    return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm;
+    return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm;
 }
 
 /**

flight/libraries/math/pid.h

@@ -44,7 +44,7 @@ struct pid {
 
 // ! Methods to use the pid structures
 float pid_apply(struct pid *pid, const float err, float dT);
-float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT);
+float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
 void pid_zero(struct pid *pid);
 void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
 void pid_configure_derivative(float cutoff, float gamma);

flight/modules/Stabilization/stabilization.c

@@ -41,6 +41,7 @@
 #include "relaytuningsettings.h"
 #include "stabilizationdesired.h"
 #include "attitudestate.h"
+#include "airspeedstate.h"
 #include "gyrostate.h"
 #include "flightstatus.h"
 #include "manualcontrol.h" // Just to get a macro
@@ -129,6 +130,9 @@ int32_t StabilizationInitialize()
     ActuatorDesiredInitialize();
 #ifdef DIAG_RATEDESIRED
     RateDesiredInitialize();
+#endif
+#ifdef REVOLUTION
+    AirspeedStateInitialize();
 #endif
     // Code required for relay tuning
     sin_lookup_initalize();
@@ -156,6 +160,10 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
     GyroStateData gyroStateData;
     FlightStatusData flightStatus;
 
+#ifdef REVOLUTION
+    AirspeedStateData airspeedState;
+#endif
+
     SettingsUpdatedCb((UAVObjEvent *)NULL);
 
     // Main task loop
@@ -183,6 +191,26 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 #ifdef DIAG_RATEDESIRED
         RateDesiredGet(&rateDesired);
 #endif
+#ifdef REVOLUTION
+        float speedScaleFactor;
+        // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
+        AirspeedStateGet(&airspeedState);
+        if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
+            // feature has been turned off
+            speedScaleFactor = 1.0f;
+        } else {
+            // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
+            speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
+            if (speedScaleFactor < settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]) {
+                speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN];
+            }
+            if (speedScaleFactor > settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]) {
+                speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX];
+            }
+        }
+#else
+        const float speedScaleFactor = 1.0f;
+#endif
 
 #if defined(PIOS_QUATERNION_STABILIZATION)
         // Quaternion calculation of error in each axis.  Uses more memory.
@@ -262,7 +290,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 rateDesiredAxis[i]     = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
 
                 // Compute the inner loop
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
+                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
                 actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
 
                 break;
@@ -278,7 +306,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
                 rateDesiredAxis[i]     = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
 
                 // Compute the inner loop
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
+                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
                 actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
 
                 break;
@@ -304,7 +332,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 
                 // Compute desired rate as input biased towards leveling
                 rateDesiredAxis[i]     = attitudeDesiredAxis[i] + weak_leveling;
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
+                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
                 actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
 
                 break;
@@ -328,7 +356,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 
                 rateDesiredAxis[i]     = bound(rateDesiredAxis[i], settings.ManualRate[i]);
 
-                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
+                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
                 actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
 
                 break;

shared/uavobjectdefinition/stabilizationsettings.xml

@@ -37,6 +37,9 @@
 
 	<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
 
+	<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
+	<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
+
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>
 	<telemetryflight acked="true" updatemode="onchange" period="0"/>