TECS: rename variables and methods to make clear which are EAS and which TAS

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/tecs/TECS.cpp

@@ -55,7 +55,8 @@ static constexpr float DT_MAX = 1.0f;	///< max value of _dt allowed before a fil
  * inertial nav data is not available. It also calculates a true airspeed derivative
  * which is used by the airspeed complimentary filter.
  */
-void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock,
+void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward,
+		bool altitude_lock,
 		bool in_air,
 		float altitude, float vz)
 {
@@ -79,7 +80,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
 	}
 
 	_state_update_timestamp = now;
-	_EAS = airspeed;
+	_EAS = equivalent_airspeed;
 
 	_in_air = in_air;
 
@@ -88,7 +89,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
 	_vert_pos_state = altitude;
 
 	// Update and average speed rate of change if airspeed is being measured
-	if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
+	if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) {
 		// Apply some noise filtering
 		_TAS_rate_filter.update(speed_deriv_forward);
 		_speed_derivative = _TAS_rate_filter.getState();
@@ -103,25 +104,24 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
 
 }
 
-void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS)
+void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS)
 {
 	// Calculate the time in seconds since the last update and use the default time step value if out of bounds
 	uint64_t now = hrt_absolute_time();
 	const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
 
 	// Convert equivalent airspeed quantities to true airspeed
-	_EAS_setpoint = airspeed_setpoint;
+	_EAS_setpoint = equivalent_airspeed_setpoint;
 	_TAS_setpoint  = _EAS_setpoint * EAS2TAS;
-	_TAS_max   = _indicated_airspeed_max * EAS2TAS;
+	_TAS_max   = _equivalent_airspeed_max * EAS2TAS;
-	_TAS_min   = _indicated_airspeed_min * EAS2TAS;
+	_TAS_min   = _equivalent_airspeed_min * EAS2TAS;
 
-	// If airspeed measurements are not being used, fix the airspeed estimate to halfway between
+	// If airspeed measurements are not being used, fix the EAS estimate to halfway between min and max limits
-	// min and max limits
+	if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
-	if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
+		_EAS = 0.5f * (_equivalent_airspeed_min + _equivalent_airspeed_max);
-		_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
 
 	} else {
-		_EAS = indicated_airspeed;
+		_EAS = equivalent_airspeed;
 	}
 
 	// If first time through or not flying, reset airspeed states
@@ -130,7 +130,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
 		_tas_state = (_EAS * EAS2TAS);
 	}
 
-	// Obtain a smoothed airspeed estimate using a second order complementary filter
+	// Obtain a smoothed TAS estimate using a second order complementary filter
 
 	// Update TAS rate state
 	float tas_error = (_EAS * EAS2TAS) - _tas_state;
@@ -139,7 +139,6 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
 	// limit integrator input to prevent windup
 	if (_tas_state < 3.1f) {
 		tas_rate_state_input = max(tas_rate_state_input, 0.0f);
-
 	}
 
 	// Update TAS state
@@ -147,7 +146,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
 	float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
 	_tas_state = _tas_state + tas_state_input * dt;
 
-	// Limit the airspeed state to a minimum of 3 m/s
+	// Limit the TAS state to a minimum of 3 m/s
 	_tas_state = max(_tas_state, 3.0f);
 	_speed_update_timestamp = now;
 
@@ -155,7 +154,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
 
 void TECS::_update_speed_setpoint()
 {
-	// Set the airspeed demand to the minimum value if an underspeed or
+	// Set the TAS demand to the minimum value if an underspeed or
 	// or a uncontrolled descent condition exists to maximise climb rate
 	if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
 		_TAS_setpoint = _TAS_min;
@@ -535,7 +534,7 @@ void TECS::_update_STE_rate_lim()
 }
 
 void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
-				 float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
+				 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)
 {
 	// Calculate the time since last update (seconds)
@@ -558,7 +557,7 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float
 	}
 
 	// Update the true airspeed state estimate
-	_update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas);
+	_update_speed_states(EAS_setpoint, equivalent_airspeed, eas_to_tas);
 
 	// Calculate rate limits for specific total energy
 	_update_STE_rate_lim();

src/lib/tecs/TECS.hpp

@@ -74,14 +74,15 @@ public:
 	 * Must be called prior to udating tecs control loops
 	 * Must be called at 50Hz or greater
 	 */
-	void update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock, bool in_air,
+	void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock,
+					    bool in_air,
 					    float altitude, float vz);
 
 	/**
 	 * Update the control loop calculations
 	 */
 	void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
-				   float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
+				   float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
 				   float throttle_min, float throttle_setpoint_max, float throttle_cruise,
 				   float pitch_limit_min, float pitch_limit_max);
 
@@ -111,8 +112,8 @@ public:
 	void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
 	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_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
-	void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; }
+	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
 
 	void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
 	void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
@@ -214,12 +215,12 @@ private:
 	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation
 	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 _airspeed_error_gain{0.1f};							///< airspeed error inverse time constant [1/s]
+	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 _equivalent_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 _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
 	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
 	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
-	float _speed_derivative_time_const{0.01f};		///< speed derivative filter time constant (s)
+	float _speed_derivative_time_const{0.01f};			///< speed derivative filter time constant (s)
 
 	// complimentary filter states
 	float _vert_vel_state{0.0f};					///< complimentary filter state - height rate (m/sec)
@@ -297,7 +298,7 @@ private:
 	/**
 	 * Update the airspeed internal state using a second order complementary filter
 	 */
-	void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas);
+	void _update_speed_states(float airspeed_setpoint, float equivalent_airspeed, float cas_to_tas);
 
 	/**
 	 * Update the desired airspeed

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -96,8 +96,8 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get());
+	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
-	_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get());
+	_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());