TECS: rename airspeed/throttle "cruise" to "trim"

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

src/lib/tecs/TECS.cpp

@@ -116,9 +116,9 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 	_TAS_max   = _equivalent_airspeed_max * EAS2TAS;
 	_TAS_min   = _equivalent_airspeed_min * EAS2TAS;
 
-	// If airspeed measurements are not being used, fix the airspeed estimate to the nominal cruise airspeed
+	// If airspeed measurements are not being used, fix the airspeed estimate to the nominal trim airspeed
 	if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
-		_EAS = _equivalent_airspeed_cruise;
+		_EAS = _equivalent_airspeed_trim;
 
 	} else {
 		_EAS = equivalent_airspeed;
@@ -248,11 +248,11 @@ void TECS::_update_energy_estimates()
 	_SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change
 }
 
-void TECS::_update_throttle_setpoint(const float throttle_cruise)
+void TECS::_update_throttle_setpoint()
 {
 	// Calculate demanded rate of change of total energy, respecting vehicle limits.
 	// We will constrain the value below.
-	float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
+	_STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
 
 	// Calculate the total energy rate error, applying a first order IIR filter
 	// to reduce the effect of accelerometer noise
@@ -270,24 +270,26 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise)
 		// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
 		// Assume induced drag scales linearly with normal load factor.
 		// The additional normal load factor is given by (1/cos(bank angle) - 1)
-		STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (_load_factor - 1.f);
+		_STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f);
 
-		STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
+		_STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
 
-		// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
+		// Calculate a predicted throttle from the demanded rate of change of energy, using the trim throttle
 		// as the starting point. Assume:
 		// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
-		// Specific total energy rate = 0 at cruise throttle
+		// Specific total energy rate = 0 at trim throttle
 		// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
 		float throttle_predicted = 0.0f;
 
-		if (STE_rate_setpoint >= 0) {
-			// throttle is between cruise and maximum
-			throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise);
+		if (_STE_rate_setpoint >= 0) {
+			// throttle is between trim and maximum
+			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max -
+					     _throttle_trim);
 
 		} else {
-			// throttle is between cruise and minimum
-			throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise);
+			// throttle is between trim and minimum
+			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min -
+					     _throttle_trim);
 
 		}
 
@@ -488,7 +490,7 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 	}
 }
 
-void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
+void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout,
 			      float EAS2TAS)
 {
 	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) {
@@ -500,7 +502,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		_tas_state = _EAS * EAS2TAS;
 		_throttle_integ_state =  0.0f;
 		_pitch_integ_state = 0.0f;
-		_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
+		_last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);;
 		_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
 		_pitch_setpoint_unc = _last_pitch_setpoint;
 		_TAS_setpoint_last = _EAS * EAS2TAS;
@@ -559,7 +561,8 @@ void TECS::_update_STE_rate_lim()
 
 void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				 float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
-				 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
+				 float throttle_min, float throttle_max, float throttle_trim,
+				 float pitch_limit_min, float pitch_limit_max,
 				 float target_climbrate, float target_sinkrate, float hgt_rate_sp)
 {
 	// Calculate the time since last update (seconds)
@@ -572,9 +575,10 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_pitch_setpoint_max = pitch_limit_max;
 	_pitch_setpoint_min = pitch_limit_min;
 	_climbout_mode_active = climb_out_setpoint;
+	_throttle_trim = throttle_trim;
 
 	// Initialize selected states and variables as required
-	_initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas);
+	_initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas);
 
 	// Don't run TECS control algorithms when not in flight
 	if (!_in_air) {
@@ -606,7 +610,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_update_energy_estimates();
 
 	// Calculate the throttle demand
-	_update_throttle_setpoint(throttle_cruise);
+	_update_throttle_setpoint();
 
 	// Calculate the pitch demand
 	_update_pitch_setpoint();

src/lib/tecs/TECS.hpp

@@ -95,6 +95,7 @@ public:
 	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _last_pitch_setpoint; }
 	float get_speed_weight() { return _pitch_speed_weight; }
+	float get_throttle_trim() { return _throttle_trim; }
 
 	void reset_state() { _states_initialized = false; }
 
@@ -140,7 +141,6 @@ public:
 
 	void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
 
-
 	// TECS status
 	uint64_t timestamp() { return _pitch_update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }