diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c
index 2797b3e14..505643020 100644
--- a/flight/libraries/math/pid.c
+++ b/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_scaled(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;
}
/**
diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h
index 7872ac559..4b4582bd8 100644
--- a/flight/libraries/math/pid.h
+++ b/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_scaled(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);
diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c
index 6cfeed50f..999b26d62 100644
--- a/flight/modules/Stabilization/stabilization.c
+++ b/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
+ if (settings.ScaleToAirspeed < 0.1f) {
+ // feature has been turned off
+ AirspeedStateGet(&airspeedState);
+ 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_scaled(&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_scaled(&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_scaled(&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_scaled(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml
index 9434313e0..b03b1bedf 100644
--- a/shared/uavobjectdefinition/stabilizationsettings.xml
+++ b/shared/uavobjectdefinition/stabilizationsettings.xml
@@ -37,6 +37,9 @@
+
+
+