From 247738518e0847ade20e13a69c3a4b00d2ddc872 Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Thu, 18 Oct 2018 11:42:57 +0100
Subject: [PATCH] AP_TECS: Add a feed-forward term from adjusted demanded
 airspeed to nav pitch.

---
 libraries/AP_TECS/AP_TECS.cpp | 18 ++++++++++++++++++
 libraries/AP_TECS/AP_TECS.h   |  3 +++
 2 files changed, 21 insertions(+)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index c60303d04a..f7cdc7c3d5 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -249,6 +249,20 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0),
 
+    // @Param: PTCH_FF_V0
+    // @DisplayName: Baseline airspeed for pitch feed-forward.
+    // @Description: This parameter sets the airspeed at which no feed-forward is applied between demanded airspeed and pitch. It should correspond to the airspeed at which the plane glides at zero pitch.
+    // @Range 5.0 50.0
+    // @User: Advanced
+    AP_GROUPINFO("PTCH_FF_V0", 29, AP_TECS, _pitch_ff_v0, 12.0),
+
+    // @Param: PTCH_FF_K
+    // @DisplayName: Gain for pitch feed-forward.
+    // @Description: This parameter sets the gain between demanded airspeed and pitch. It should generally be negative.
+    // @Range -5.0 0.0
+    // @User: Advanced
+    AP_GROUPINFO("PTCH_FF_K", 30, AP_TECS, _pitch_ff_k, 0.0),
+    
     AP_GROUPEND
 };
 
@@ -886,6 +900,10 @@ void AP_TECS::_update_pitch(void)
     // Calculate pitch demand from specific energy balance signals
     _pitch_dem_unc = (temp + _integSEB_state) / gainInv;
 
+
+    // Add a feedforward term from demanded airspeed to pitch.
+    _pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
+
     // Constrain pitch demand
     _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
 
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 8b21e123e4..9f0890cd64 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -177,6 +177,9 @@ private:
         OPTION_GLIDER_ONLY=(1<<0),
     };
 
+    AP_Float _pitch_ff_v0;
+    AP_Float _pitch_ff_k;
+
     // temporary _pitch_max_limit. Cleared on each loop. Clear when >= 90
     int8_t _pitch_max_limit = 90;