From 0912f05e7cffbb2047b9162fd5d35859fec016ab Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Fri, 28 Nov 2014 19:32:15 +1100
Subject: [PATCH] AP_TECS: Add parameter to adjust height rate gain during
 flare

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

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index 6740dd19f2..bd94fc9da2 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -168,6 +168,14 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = {
 	// @User: Advanced
     AP_GROUPINFO("LAND_TCONST", 18, AP_TECS, _landTimeConst, 2.0f),
 
+    // @Param: LAND_DAMP
+    // @DisplayName: Controller sink rate to pitch gain during flare
+    // @Description: This is the sink rate gain for the pitch demand loop when in final landing stage of flight. It should be larger than TECS_PTCH_DAMP to allow for better sink rate control during flare.
+    // @Range: 0.1 1.0
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("LAND_DAMP", 19, AP_TECS, _landDamp, 0.0f),
+
     AP_GROUPEND
 };
 
@@ -647,11 +655,16 @@ void AP_TECS::_update_pitch(void)
     // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle 
     // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
     // integrator has to catch up before the nose can be raised to reduce speed during climbout.
+    // During flare a different damping gain is used
 	float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS);
-    float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * timeConstant();
-    if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF)
-    {
-    	temp += _PITCHminf * gainInv;
+    float temp = SEB_error + SEBdot_dem * timeConstant();
+    if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
+        temp += SEBdot_error * _landDamp;
+    } else {
+        temp += SEBdot_error * _ptchDamp;
+    }
+    if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) {
+        temp += _PITCHminf * gainInv;
     }
     _integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
 
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 547c9e4938..aef30f6288 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -117,6 +117,7 @@ private:
     AP_Float _timeConst;
     AP_Float _landTimeConst;
     AP_Float _ptchDamp;
+    AP_Float _landDamp;
     AP_Float _thrDamp;
     AP_Float _integGain;
     AP_Float _vertAccLim;
@@ -186,7 +187,7 @@ private:
     float _hgt_dem_adj_last;
     float _hgt_rate_dem;
 	float _hgt_dem_prev;
-    float _initial_land_hgt;
+    float _land_hgt_dem;
 
     // Speed demand after application of rate limiting
     // This is the demand tracked by the TECS control loops