From 73ba9ff5895f653be69317758f52b27087dac1a0 Mon Sep 17 00:00:00 2001
From: Eric Price <eric.price@tuebingen.mpg.de>
Date: Thu, 18 Oct 2018 19:15:28 +0200
Subject: [PATCH] LP-604 Add a feed forward term to Stabilization outer loop

---
 flight/modules/Stabilization/outerloop.c                  | 8 ++++++++
 shared/uavobjectdefinition/stabilizationbank.xml          | 1 +
 shared/uavobjectdefinition/stabilizationsettingsbank1.xml | 1 +
 shared/uavobjectdefinition/stabilizationsettingsbank2.xml | 1 +
 shared/uavobjectdefinition/stabilizationsettingsbank3.xml | 1 +
 5 files changed, 12 insertions(+)

diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c
index 9fc3c6e4a..23db9067b 100644
--- a/flight/modules/Stabilization/outerloop.c
+++ b/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]);
diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml
index 168cc2644..760a46a0e 100644
--- a/shared/uavobjectdefinition/stabilizationbank.xml
+++ b/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; ; "/>
diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml
index a7bd0c105..c9cdf1d58 100644
--- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml
+++ b/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; ; "/>
diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml
index fdf0ad5c3..14433c5e6 100644
--- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml
+++ b/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; ; "/>
diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml
index 056b9ef30..015ffbe2d 100644
--- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml
+++ b/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; ; "/>
-- 
2.11.4.GIT