From 68dd61c7c769460f717b7ca9f49e75007d149051 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 27 Aug 2014 14:28:39 +1000
Subject: [PATCH] AP_TECS: added TECS_LAND_TCONST

this allows control of the time constant for landing in TECS. A lower
time constant gives tighter altitude control on landing approach
---
 libraries/AP_TECS/AP_TECS.cpp | 26 +++++++++++++++++++++++---
 libraries/AP_TECS/AP_TECS.h   |  4 ++++
 2 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index f66ea37b31..863bd247f0 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -157,6 +157,14 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = {
 	// @User: Advanced
     AP_GROUPINFO("LAND_SINK", 17, AP_TECS, _land_sink, 0.25f),
 
+    // @Param: TECS_LAND_TCONST
+    // @DisplayName: Land controller time constant (sec)
+    // @Description: This is the time constant of the TECS control algorithm when in final landing stage of flight. It should be smaller than TECS_TIME_CONST to allow for faster flare
+	// @Range: 1.0-5.0
+	// @Increment: 0.2
+	// @User: Advanced
+    AP_GROUPINFO("LAND_TCONST", 18, AP_TECS, _landTimeConst, 2.0f),
+
     AP_GROUPEND
 };
 
@@ -404,6 +412,18 @@ void AP_TECS::_update_energies(void)
 
 }
 
+/*
+  current time constant. It is lower in landing to try to give a precise approach
+ */
+float AP_TECS::timeConstant(void)
+{
+    if (_flight_stage==FLIGHT_LAND_FINAL ||
+        _flight_stage==FLIGHT_LAND_APPROACH) {
+        return _landTimeConst;
+    }
+    return _timeConst;
+}
+
 void AP_TECS::_update_throttle(void)
 {
     // Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
@@ -429,7 +449,7 @@ void AP_TECS::_update_throttle(void)
     else
     {
 		// Calculate gain scaler from specific energy error to throttle
-		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+		float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min));
 
         // Calculate feed-forward throttle
         float ff_throttle = 0;
@@ -595,8 +615,8 @@ 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.
-	float gainInv = (_integ5_state * _timeConst * GRAVITY_MSS);
-    float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	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;
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index e4680fdce8..d73789141d 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -114,6 +114,7 @@ private:
     AP_Float _minSinkRate;
     AP_Float _maxSinkRate;
     AP_Float _timeConst;
+    AP_Float _landTimeConst;
     AP_Float _ptchDamp;
     AP_Float _thrDamp;
     AP_Float _integGain;
@@ -274,6 +275,9 @@ private:
 
     // declares a 5point average filter using floats
 	AverageFilterFloat_Size5 _vdot_filter;
+
+	// current time constant
+	float timeConstant(void);
 };
 
 #define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning),	\