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

Author: AIRCAP

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>
@@ -78,6 +79,7 @@ void stabilizationOuterloopInit()
     RateDesiredInitialize();
     StabilizationDesiredInitialize();
     AttitudeStateInitialize();
+    GyroStateInitialize();
     StabilizationStatusInitialize();
     FlightStatusInitialize();
     ManualControlCommandInitialize();
@@ -97,11 +99,13 @@ void stabilizationOuterloopInit()
 static void stabilizationOuterloopTask()
 {
     AttitudeStateData attitudeState;
+    GyroStateData gyroState;
     RateDesiredData rateDesired;
     StabilizationDesiredData stabilizationDesired;
     StabilizationStatusOuterLoopData enabled;
 
     AttitudeStateGet(&attitudeState);
+    GyroStateGet(&gyroState);
     StabilizationDesiredGet(&stabilizationDesired);
     RateDesiredGet(&rateDesired);
     StabilizationStatusOuterLoopGet(&enabled);
@@ -190,6 +194,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] - (gyroState.x * stabSettings.stabBank.AttitudeFeedForward.Roll);
+    local_error[1] = local_error[1] - (gyroState.y * stabSettings.stabBank.AttitudeFeedForward.Pitch);
+    local_error[2] = local_error[2] - (gyroState.z * stabSettings.stabBank.AttitudeFeedForward.Yaw);
 
     for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
         reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);

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; ; "/>