TECS: move to new control loop architecture

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/tecs/TECS.hpp

@@ -86,7 +86,7 @@ public:
 				   float throttle_min, float throttle_setpoint_max, float throttle_cruise,
 				   float pitch_limit_min, float pitch_limit_max);
 
-	float get_throttle_setpoint() { return _throttle_setpoint; }
+	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _pitch_setpoint; }
 	float get_speed_weight() { return _pitch_speed_weight; }
 
@@ -102,7 +102,6 @@ public:
 	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
 
 	// setters for controller parameters
-	void set_time_const(float time_const) { _pitch_time_constant = time_const; }
 	void set_integrator_gain(float gain) { _integrator_gain = gain; }
 
 	void set_min_sink_rate(float rate) { _min_sink_rate = rate; }
@@ -110,7 +109,7 @@ public:
 	void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; }
 
 	void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
-	void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; }
+	void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); }
 
 	void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; }
 	void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; }
@@ -120,9 +119,8 @@ public:
 
 	void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; }
 	void set_speed_weight(float weight) { _pitch_speed_weight = weight; }
-	void set_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; }
+	void set_airspeed_error_time_constant(float time_const) { _airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); }
 
-	void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; }
 	void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; }
 	void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; }
 
@@ -206,17 +204,15 @@ private:
 	float _max_climb_rate{2.0f};					///< climb rate produced by max allowed throttle (m/sec)
 	float _min_sink_rate{1.0f};					///< sink rate produced by min allowed throttle (m/sec)
 	float _max_sink_rate{2.0f};					///< maximum safe sink rate (m/sec)
-	float _pitch_time_constant{5.0f};				///< control time constant used by the pitch demand calculation (sec)
-	float _throttle_time_constant{8.0f};				///< control time constant used by the throttle demand calculation (sec)
 	float _pitch_damping_gain{0.0f};				///< damping gain of the pitch demand calculation (sec)
 	float _throttle_damping_gain{0.0f};				///< damping gain of the throttle demand calculation (sec)
 	float _integrator_gain{0.0f};					///< integrator gain used by the throttle and pitch demand calculation
 	float _vert_accel_limit{0.0f};					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
 	float _load_factor_correction{0.0f};				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
 	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation
-	float _height_error_gain{0.0f};					///< gain from height error to demanded climb rate (1/sec)
+	float _height_error_gain{0.2f};					///< height error inverse time constant [1/s]
 	float _height_setpoint_gain_ff{0.0f};				///< gain from height demand derivative to demanded climb rate
-	float _speed_error_gain{0.0f};					///< gain from speed error to demanded speed rate (1/sec)
+	float _airspeed_error_gain{0.1f};							///< airspeed error inverse time constant [1/s]
 	float _indicated_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _indicated_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
 	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
@@ -224,7 +220,6 @@ private:
 	float _speed_derivative_time_const{0.01f};		///< speed derivative filter time constant (s)
 
 	// controller outputs
-	float _throttle_setpoint{0.0f};					///< normalized throttle demand (0..1)
 	float _pitch_setpoint{0.0f};					///< pitch angle demand (radians)
 
 	// complimentary filter states