Merged in corvusvcorax/librepilot/LP-604_Stabilization_Feedforward (pull request #521)

LP-604 Add a feed forward term to Stabilization outer loop

Approved-by: Eric Price <corvuscorax@cybertrench.com>
Approved-by: Alessio Morale <alessiomorale@gmail.com>
Approved-by: Lalanne Laurent <f5soh@free.fr>

Author: CorvusCorax

flight/modules/Stabilization/inc/stabilization.h

@@ -63,6 +63,7 @@ typedef struct {
     struct pid innerPids[3], outerPids[3];
     // TPS [Roll,Pitch,Yaw][P,I,D]
     bool  thrust_pid_scaling_enabled[3][3];
+    float feedForward_alpha[3];
 } StabilizationData;
 
 

flight/modules/Stabilization/outerloop.c

@@ -38,6 +38,7 @@
 #include <ratedesired.h>
 #include <stabilizationdesired.h>
 #include <attitudestate.h>
+#include <gyrostate.h>
 #include <stabilizationstatus.h>
 #include <flightstatus.h>
 #include <manualcontrolcommand.h>
@@ -63,6 +64,7 @@ static DelayedCallbackInfo *callbackHandle;
 static AttitudeStateData attitude;
 
 static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
+static float gyro_filtered[3] = { 0, 0, 0 };
 static PiOSDeltatimeConfig timeval;
 static bool pitchMin = false;
 static bool pitchMax = false;
@@ -71,20 +73,23 @@ static bool rollMax  = false;
 
 // Private functions
 static void stabilizationOuterloopTask();
+static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
 static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
 
 void stabilizationOuterloopInit()
 {
     RateDesiredInitialize();
     StabilizationDesiredInitialize();
     AttitudeStateInitialize();
+    GyroStateInitialize();
     StabilizationStatusInitialize();
     FlightStatusInitialize();
     ManualControlCommandInitialize();
 
     PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 
     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
+    GyroStateConnectCallback(GyroStateUpdatedCb);
     AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
 }
 
@@ -190,6 +195,10 @@ static void stabilizationOuterloopTask()
 #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
     }
 
+    // Feed forward: Assume things always get worse before they get better
+    local_error[0] = local_error[0] - (gyro_filtered[0] * stabSettings.stabBank.AttitudeFeedForward.Roll);
+    local_error[1] = local_error[1] - (gyro_filtered[1] * stabSettings.stabBank.AttitudeFeedForward.Pitch);
+    local_error[2] = local_error[2] - (gyro_filtered[2] * stabSettings.stabBank.AttitudeFeedForward.Yaw);
 
     for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
         reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
@@ -380,6 +389,18 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 #endif
 }
 
+static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    GyroStateData gyroState;
+
+    GyroStateGet(&gyroState);
+
+    gyro_filtered[0] = gyro_filtered[0] * stabSettings.feedForward_alpha[0] + gyroState.x * (1 - stabSettings.feedForward_alpha[0]);
+    gyro_filtered[1] = gyro_filtered[1] * stabSettings.feedForward_alpha[1] + gyroState.y * (1 - stabSettings.feedForward_alpha[1]);
+    gyro_filtered[2] = gyro_filtered[2] * stabSettings.feedForward_alpha[2] + gyroState.z * (1 - stabSettings.feedForward_alpha[2]);
+}
+
+
 /**
  * @}
  * @}

flight/modules/Stabilization/stabilization.c

@@ -364,6 +364,21 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f;
     stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f;
     stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f;
+
+    // The dT has some jitter iteration to iteration that we don't want to
+    // make thie result unpredictable.  Still, it's nicer to specify the constant
+    // based on a time (in ms) rather than a fixed multiplier.  The error between
+    // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
+    // calculation
+    const float fakeDt = 0.0025f;
+    for (int t = 0; t < STABILIZATIONBANK_ATTITUDEFEEDFORWARD_NUMELEM; t++) {
+        float tau = StabilizationBankAttitudeFeedForwardToArray(stabSettings.stabBank.AttitudeFeedForward)[t] * 0.1f;
+        if (tau < 0.0001f) {
+            stabSettings.feedForward_alpha[t] = 0.0f; // not trusting this to resolve to 0
+        } else {
+            stabSettings.feedForward_alpha[t] = expf(-fakeDt / tau);
+        }
+    }
 }
 
 

shared/uavobjectdefinition/stabilizationbank.xml

@@ -8,6 +8,7 @@
 	<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
+	<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>

shared/uavobjectdefinition/stabilizationsettingsbank1.xml

@@ -8,6 +8,7 @@
 	<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
+	<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>

shared/uavobjectdefinition/stabilizationsettingsbank2.xml

@@ -8,6 +8,7 @@
 	<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
+	<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>

shared/uavobjectdefinition/stabilizationsettingsbank3.xml

@@ -8,6 +8,7 @@
 	<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
+	<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>