TECS: remove airspeed adaption in case of underspeed

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

src/lib/tecs/TECS.cpp

@@ -627,26 +627,6 @@ void TECSControl::resetIntegrals()
 	_throttle_integ_state = 0.0f;
 }
 
-float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas)
-{
-	float new_setpoint{tas_setpoint};
-	const float percent_undersped = _control.getRatioUndersped();
-
-	// Set the TAS demand to the minimum value if an underspeed condition exists to maximise climb rate
-	if (percent_undersped > FLT_EPSILON) {
-		// TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still
-		// between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint
-		// from this line back into this method each time).
-		// TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to
-		// avoid the fear of side effects in simple operations like this.
-		new_setpoint = tas_min * percent_undersped + (1.0f - percent_undersped) * tas_setpoint;
-	}
-
-	new_setpoint = constrain(new_setpoint, tas_min, tas_max);
-
-	return new_setpoint;
-}
-
 void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
 		      const float eas_to_tas)
 {
@@ -729,11 +709,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-
+		control_setpoint.tas_setpoint = eas_to_tas * EAS_setpoint;
-		// Calculate the demanded true airspeed
-		// TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved.
-		control_setpoint.tas_setpoint = _update_speed_setpoint(eas_to_tas * _equivalent_airspeed_min,
-						eas_to_tas * _equivalent_airspeed_max, EAS_setpoint * eas_to_tas, eas_to_tas * eas.speed);
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,

src/lib/tecs/TECS.hpp

@@ -614,7 +614,6 @@ public:
 	void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
 	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };
 
-	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
 	void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }
 
@@ -665,7 +664,6 @@ private:
 	hrt_abstime _update_timestamp{0};				///< last timestamp of the update function call.
 
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
-	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
 
 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
@@ -723,10 +721,5 @@ private:
 		.airspeed_enabled = false,
 		.detect_underspeed_enabled = false,
 	};
-
-	/**
-	 * Update the desired airspeed
-	 */
-	float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas);
 };
 

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -132,7 +132,6 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
 	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
-	_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
@@ -1210,7 +1209,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get(),
-				   tecs_status_s::TECS_MODE_NORMAL,
+				   false,
 				   pos_sp_curr.vz);
 }