From b218909c7b5057f463d9d0f61184a7666cc13bc9 Mon Sep 17 00:00:00 2001
From: Eric Price <eric.price@tuebingen.mpg.de>
Date: Fri, 19 Oct 2018 08:29:25 +0200
Subject: [PATCH] LP-604 Add suitable lowpass filter to feed forward term

---
 flight/modules/Stabilization/inc/stabilization.h |  1 +
 flight/modules/Stabilization/outerloop.c         | 23 ++++++++++++++++++-----
 flight/modules/Stabilization/stabilization.c     | 15 +++++++++++++++
 3 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h
index fbf8dbf9b..6884bfe6f 100644
--- a/flight/modules/Stabilization/inc/stabilization.h
+++ b/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;
 
 
diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c
index 23db9067b..f57483aa2 100644
--- a/flight/modules/Stabilization/outerloop.c
+++ b/flight/modules/Stabilization/outerloop.c
@@ -64,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;
@@ -72,6 +73,7 @@ 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()
@@ -87,6 +89,7 @@ void stabilizationOuterloopInit()
     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);
 }
 
@@ -99,13 +102,11 @@ 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);
@@ -195,9 +196,9 @@ static void stabilizationOuterloopTask()
     }
 
     // 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);
+    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]);
@@ -388,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]);
+}
+
+
 /**
  * @}
  * @}
diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c
index ac667f599..c5fa49472 100644
--- a/flight/modules/Stabilization/stabilization.c
+++ b/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];
+        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);
+        }
+    }
 }
 
 
-- 
2.11.4.GIT