From 22d0e507c68ce5006b6369242e2576b0ef0b5b16 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 26 Feb 2017 00:09:01 -0500
Subject: [PATCH] TECS fix height demand change

---
 src/lib/external_lgpl/tecs/tecs.cpp                 | 13 ++-----------
 .../fw_pos_control_l1/fw_pos_control_l1_params.c    | 10 +++++-----
 2 files changed, 7 insertions(+), 16 deletions(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index d2cf30f556..75c9b04099 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -252,18 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
 	}
 
 	_hgt_dem_prev = _hgt_dem;
-//
-//	// Apply first order lag to height demand
-//	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
-//	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
-//	_hgt_dem_adj_last = _hgt_dem_adj;
-//
-//	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
-//	// 	_hgt_rate_dem);
 
+	// Apply first order lag to height demand
 	_hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last;
-	_hgt_dem_adj_last = _hgt_dem_adj;
 	_hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT;
+	_hgt_dem_adj_last = _hgt_dem_adj;
 
 	// Limit height rate of change
 	if (_hgt_rate_dem > _maxClimbRate) {
@@ -272,8 +265,6 @@ void TECS::_update_height_demand(float demand, float state)
 	} else if (_hgt_rate_dem < -_maxSinkRate) {
 		_hgt_rate_dem = -_maxSinkRate;
 	}
-
-	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed()
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 561d4e4349..8663d97d5b 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -615,10 +615,10 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
 /**
- * Height rate P factor
+ * Height rate proportional factor
  *
  * @min 0.0
- * @max 2.0
+ * @max 1.0
  * @decimal 2
  * @increment 0.05
  * @group FW TECS
@@ -626,15 +626,15 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 
 /**
- * Height rate FF factor
+ * Height rate feed forward
  *
  * @min 0.0
- * @max 2.0
+ * @max 1.0
  * @decimal 2
  * @increment 0.05
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
+PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.1f);
 
 /**
  * Speed rate P factor