TECS: remove TECS_MODE enum and instead add descriptive boolean flag to tecs_status

New flag: underspeed_mode_enabled.

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

msg/TecsStatus.msg

@@ -26,7 +26,4 @@ float32 throttle_sp			# Current throttle setpoint [-]
 float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
 
-# TECS mode
+bool underspeed_mode_enabled		# System has detected a low airspeed and takes measures to avoid stalling the plane
-uint8 mode
-uint8 TECS_MODE_NORMAL = 0
-uint8 TECS_MODE_UNDERSPEED = 1

src/lib/tecs/TECS.cpp

@@ -649,7 +649,6 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 	_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
 
-	_debug_status.tecs_mode = _tecs_mode;
 	_debug_status.control = _control.getDebugOutput();
 	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
 	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
@@ -721,17 +720,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		// Update time stamps
 		_update_timestamp = now;
 
-
-		// Set TECS mode for next frame
-		if (_control.getRatioUndersped() > FLT_EPSILON) {
-			_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
-
-		} else {
-			// This is the default operation mode
-			_tecs_mode = ECL_TECS_MODE_NORMAL;
-		}
-
-		_debug_status.tecs_mode = _tecs_mode;
 		_debug_status.control = _control.getDebugOutput();
 		_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
 		_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;

src/lib/tecs/TECS.hpp

@@ -539,11 +539,6 @@ private:
 class TECS
 {
 public:
-	enum ECL_TECS_MODE {
-		ECL_TECS_MODE_NORMAL = 0,
-		ECL_TECS_MODE_UNDERSPEED
-	};
-
 	struct DebugOutput {
 		TECSControl::DebugOutput control;
 		float true_airspeed_filtered;
@@ -551,7 +546,6 @@ public:
 		float altitude_reference;
 		float height_rate_reference;
 		float height_rate_direct;
-		enum ECL_TECS_MODE tecs_mode;
 	};
 public:
 	TECS() = default;
@@ -652,15 +646,13 @@ public:
 	float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
 
 	uint64_t timestamp() { return _update_timestamp; }
-	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
+	bool underspeed_detected() { return _control.getRatioUndersped() > 0.f; }
 
 private:
 	TECSControl 			_control;			///< Control submodule.
 	TECSAirspeedFilter 		_airspeed_filter;		///< Airspeed filter submodule.
 	TECSAltitudeReferenceModel 	_altitude_reference_model;	///< Setpoint reference model submodule.
 
-	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};		///< Current activated mode.
-
 	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)

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -485,16 +485,6 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 
 	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
 
-	switch (_tecs.tecs_mode()) {
-	case TECS::ECL_TECS_MODE_NORMAL:
-		tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
-		break;
-
-	case TECS::ECL_TECS_MODE_UNDERSPEED:
-		tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
-		break;
-	}
-
 	tecs_status.altitude_sp = alt_sp;
 	tecs_status.altitude_reference = debug_output.altitude_reference;
 	tecs_status.height_rate_reference = debug_output.height_rate_reference;
@@ -516,6 +506,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 	tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
 	tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
 	tecs_status.throttle_trim = throttle_trim;
+	tecs_status.underspeed_mode_enabled = _tecs.underspeed_detected();
 
 	tecs_status.timestamp = hrt_absolute_time();