[TECS]: WIP Add Stall prevention function into TECS for testing.

TODO remove testing code from airspeed selector to forse airspeedless in FW mode.
TODO test stall prevention better

msg/TecsStatus.msg

@@ -33,3 +33,4 @@ uint8 TECS_MODE_LAND = 3
 uint8 TECS_MODE_LAND_THROTTLELIM = 4
 uint8 TECS_MODE_BAD_DESCENT = 5
 uint8 TECS_MODE_CLIMBOUT = 6
+uint8 TECS_MODE_STALL_PREVENTION = 7

src/lib/tecs/TECS.cpp

@@ -344,7 +344,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt
 void TECSControl::_detectUnderspeed(const Input &input, const Param &param, const Flag &flag)
 {
 	if (!flag.detect_underspeed_enabled) {
-		_ratio_undersped = 0.0f;
+		_ratio_underspeed = 0.0f;
 		return;
 	}
 
@@ -360,8 +360,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param &param, cons
 	const float tas_fully_undersped = math::max(param.tas_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
 	const float tas_starting_to_underspeed = math::max(param.tas_min - tas_error_bound, tas_fully_undersped);
 
-	_ratio_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
-			   math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
+	_ratio_underspeed = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
+			    math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
 }
 
 TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param &param, const Flag &flag)
@@ -371,8 +371,8 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	// Calculate the weight applied to control of specific kinetic energy error
 	float pitch_speed_weight = constrain(param.pitch_speed_weight, 0.0f, 2.0f);
 
-	if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
-		pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight;
+	if (_ratio_underspeed > FLT_EPSILON && flag.airspeed_enabled) {
+		pitch_speed_weight = 2.0f * _ratio_underspeed + (1.0f - _ratio_underspeed) * pitch_speed_weight;
 
 	} else if (!flag.airspeed_enabled) {
 		pitch_speed_weight = 0.0f;
@@ -540,7 +540,7 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit
 		if (param.integrator_gain_throttle > FLT_EPSILON) {
 			// underspeed conditions zero out integration
 			float throttle_integ_input = (_getControlError(ste_rate) * param.integrator_gain_throttle) * dt *
-						     STE_rate_to_throttle * (1.0f - _ratio_undersped);
+						     STE_rate_to_throttle * (1.0f - _ratio_underspeed);
 
 			// only allow integrator propagation into direction which unsaturates throttle
 			if (_throttle_setpoint >= param.throttle_max) {
@@ -603,7 +603,7 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C
 
 
 	// ramp in max throttle setting with underspeediness value
-	throttle_setpoint = _ratio_undersped * param.throttle_max + (1.0f - _ratio_undersped) * throttle_setpoint;
+	throttle_setpoint = _ratio_underspeed * param.throttle_max + (1.0f - _ratio_underspeed) * throttle_setpoint;
 
 	return constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
 }
@@ -614,23 +614,32 @@ void TECSControl::resetIntegrals()
 	_throttle_integ_state = 0.0f;
 }
 
+TECS::TECS() :
+	ModuleParams(nullptr)
+{
+
+}
+
 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();
+	const float percent_underspeed = _control.getRatioUnderspeed();
 
 	// 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) {
+	if (_stall_detection_percentage > FLT_EPSILON) {
+		new_setpoint = (1 - _stall_detection_percentage) * tas_setpoint + _stall_detection_percentage * tas_max;
+
+	} else if (_uncommanded_descent_recovery) {
 		new_setpoint = tas_min;
 
-	} else if (percent_undersped > FLT_EPSILON) {
+	} else if (percent_underspeed > 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 = tas_min * percent_underspeed + (1.0f - percent_underspeed) * tas_setpoint;
 	}
 
 	new_setpoint = constrain(new_setpoint, tas_min, tas_max);
@@ -660,7 +669,7 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu
 	const float SKE_error = SKE_setpoint - SKE_estimate;
 	const float STE_error = SPE_error + SKE_error;
 
-	const bool underspeed_detected = _control.getRatioUndersped() > FLT_EPSILON;
+	const bool underspeed_detected = _control.getRatioUnderspeed() > FLT_EPSILON;
 
 	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
 	const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f)
@@ -731,10 +740,16 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
-	_control_param.throttle_trim = throttle_trim;
 	_control_param.throttle_max = throttle_setpoint_max;
 	_control_param.throttle_min = throttle_min;
 
+	if (throttle_trim > _control_param.throttle_trim / _stall_thr_trim_scale_to_prevent_stall) {
+		_stall_thr_trim_scale_to_prevent_stall = 1.0f;
+
+	}
+
+	_control_param.throttle_trim = throttle_trim * _stall_thr_trim_scale_to_prevent_stall;
+
 	if (dt < DT_MIN) {
 		// Update intervall too small, do not update. Assume constant states/output in this case.
 		return;
@@ -766,6 +781,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		// 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);
+		_update_altitude_setpoint(control_setpoint, altitude);
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -778,17 +794,23 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed * eas_to_tas,
 					    control_setpoint.tas_setpoint);
 
+		// Detect stall
+		_detect_airspeedless_stall(dt, hgt_setpoint, altitude, pitch, hgt_rate);
+
 		// Update time stamps
 		_update_timestamp = now;
 
 
 		// Set TECS mode for next frame
-		if (_control.getRatioUndersped() > FLT_EPSILON) {
+		if (_control.getRatioUnderspeed() > FLT_EPSILON) {
 			_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
 
 		} else if (_uncommanded_descent_recovery) {
 			_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
 
+		} else if (_stall_detection_percentage > FLT_EPSILON) {
+			_tecs_mode = ECL_TECS_MODE_STALL_PREVENTION;
+
 		} else {
 			// This is the default operation mode
 			_tecs_mode = ECL_TECS_MODE_NORMAL;
@@ -804,3 +826,78 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	}
 }
 
+void TECS::_update_altitude_setpoint(TECSControl::Setpoint &control_setpoint, const float altitude) const
+{
+	if (_stall_detection_percentage > FLT_EPSILON) {
+		control_setpoint.altitude_reference.alt = (1.0f - _stall_detection_percentage) * control_setpoint.altitude_reference.alt
+				+ _stall_detection_percentage * altitude;
+		control_setpoint.altitude_reference.alt_rate = (1.0f - _stall_detection_percentage) *
+				control_setpoint.altitude_reference.alt_rate;
+		control_setpoint.altitude_rate_setpoint = (1.0f - _stall_detection_percentage) * control_setpoint.altitude_rate_setpoint
+				- _stall_detection_percentage * _control_param.max_sink_rate;
+	}
+}
+
+void TECS::_detect_airspeedless_stall(const float dt, const float altitude_sp, const float altitude, const float pitch,
+				      const float altitude_rate)
+{
+	if (_param_fw_stall_prv.get()) {
+		// Checks to detect an aircraft stalling: a positive altitude change is given,
+		// while the pitch command is maxed and the AV is sinking.
+		// TODO could also add airspeed rate negative, would need to filter the speed_deriv_forward.
+		if (((altitude_sp - altitude) > 0.0f) &&
+		    (pitch > 0.0f) &&
+		    (altitude_rate < 0.0f) &&
+		    (fabsf(_control_param.pitch_max - _control.getPitchSetpoint()) < FLT_EPSILON)) {
+			if (_stall_state == STALL_MODE::NO_STALL) {
+				PX4_WARN("Entered stall condition");
+				_stall_start_timestamp = hrt_absolute_time();
+				_stall_state = STALL_MODE::MAYBE_STALL;
+			}
+
+		} else if (_stall_state == STALL_MODE::MAYBE_STALL) {
+			PX4_WARN("Aborting stall");
+			_stall_state = STALL_MODE::NO_STALL;
+			_stall_start_timestamp = 0ULL;
+		}
+
+		// Only be sure to enter stall prevention if the stall state persists for a certain amount of time.
+		if (_stall_state == STALL_MODE::MAYBE_STALL
+		    && hrt_elapsed_time(&_stall_start_timestamp) > math::max(_param_fw_stall_delay_ms.get() * 1000ULL, 0ULL)) {
+			PX4_WARN("Start stall reaction.");
+			_stall_state = STALL_MODE::STALL;
+		}
+
+		// End stall if we pitched the nose downwards and flying nearly at max sinkrate.
+		if ((_stall_state == STALL_MODE::STALL) && (pitch < 0.0f) && (altitude_rate < (0.5f - _control_param.max_sink_rate))) {
+			PX4_WARN("Stall ending, speed increased.");
+			_stall_state = STALL_MODE::STALL_ENDING;
+		}
+
+		if ((_stall_state == STALL_MODE::STALL_ENDING) && (_stall_detection_percentage < FLT_EPSILON)) {
+			PX4_WARN("Stall ended.");
+			_stall_state = STALL_MODE::NO_STALL;
+			_stall_start_timestamp = 0ULL;
+			_stall_thr_trim_scale_to_prevent_stall += math::max(_param_fw_stall_throttle_scale.get(),
+					0.f); // Increase throttle trim to prevent future stall.
+		}
+
+		switch (_stall_state) {
+		case STALL_MODE::STALL: {
+				_stall_detection_percentage = math::min(_stall_detection_percentage + dt / math::max(FLT_EPSILON,
+									_param_fw_stall_ramp_up.get()), 1.0f);
+				break;
+			}
+
+		case STALL_MODE::STALL_ENDING: {
+				_stall_detection_percentage = math::max(_stall_detection_percentage - dt / math::max(FLT_EPSILON,
+									_param_fw_stall_ramp_up.get()), 0.0f);
+				break;
+			}
+
+		default: {
+				_stall_detection_percentage = 0.0f;
+			}
+		}
+	}
+}

src/lib/tecs/TECS.hpp

@@ -43,6 +43,7 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <px4_platform_common/module_params.h>
 
 #include <uORB/Publication.hpp>
 #include <uORB/topics/tecs_status.h>
@@ -299,7 +300,7 @@ public:
 	 *
 	 * @return Ratio of detected undersped [0,1].
 	 */
-	float getRatioUndersped() const {return _ratio_undersped;};
+	float getRatioUnderspeed() const {return _ratio_underspeed;};
 	/**
 	 * @brief Get the throttle setpoint.
 	 *
@@ -533,18 +534,19 @@ private:
 	DebugOutput _debug_output;				///< Debug output.
 	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
 	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
-	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
+	float _ratio_underspeed{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
 	float _ste_rate{0.0f};					///< Specific total energy rate [m²/s³].
 };
 
-class TECS
+class TECS : public ModuleParams
 {
 public:
 	enum ECL_TECS_MODE {
 		ECL_TECS_MODE_NORMAL = 0,
 		ECL_TECS_MODE_UNDERSPEED,
 		ECL_TECS_MODE_BAD_DESCENT,
-		ECL_TECS_MODE_CLIMBOUT
+		ECL_TECS_MODE_CLIMBOUT,
+		ECL_TECS_MODE_STALL_PREVENTION
 	};
 
 	struct DebugOutput {
@@ -557,7 +559,7 @@ public:
 		enum ECL_TECS_MODE tecs_mode;
 	};
 public:
-	TECS() = default;
+	TECS();
 	~TECS() = default;
 
 	// no copy, assignment, move, move assignment
@@ -664,13 +666,21 @@ private:
 
 	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.
-
+	hrt_abstime _update_timestamp{0ULL};				///< last timestamp of the update function call.
+	hrt_abstime _stall_start_timestamp{0ULL};			///< timestamp when the stall condition was first achieved.
+	enum class STALL_MODE {
+		NO_STALL,
+		MAYBE_STALL,
+		STALL,
+		STALL_ENDING,
+	} _stall_state{STALL_MODE::NO_STALL};				///< State machine of the stall detection algorithm
+	float _stall_thr_trim_scale_to_prevent_stall{1.0f};		///< Scale applied to throttle trim to avoid future stall conditions.
 	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)
 
 	// controller mode logic
 	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
+	float _stall_detection_percentage{0.f};			///< Continuous representation of how certain we are about a potential stall situation between [0 1]
 
 	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)
@@ -737,5 +747,16 @@ private:
 	 */
 	void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
 					 float tas_setpoint);
+
+	void _detect_airspeedless_stall(float dt, float altitude_sp, float altitude, float pitch, float altitude_rate);
+
+	void _update_altitude_setpoint(TECSControl::Setpoint &control_setpoint, float altitude) const;
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::FW_T_STALL_PRV>) _param_fw_stall_prv,
+		(ParamInt<px4::params::FW_T_STALL_DELAY>) _param_fw_stall_delay_ms,
+		(ParamFloat<px4::params::FW_T_STALL_TH_SC>) _param_fw_stall_throttle_scale,
+		(ParamFloat<px4::params::FW_T_STALL_RAMP>) _param_fw_stall_ramp_up
+	)
 };
 

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -567,6 +567,11 @@ void AirspeedModule::select_airspeed_and_publish()
 	// we need to re-evaluate the sensors if we're currently not on a phyisical sensor or the current sensor got invalid
 	bool airspeed_sensor_switching_necessary = false;
 
+	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		_number_of_airspeed_sensors = 0;
+		_valid_airspeed_index = -1;
+	}
+
 	if (_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX) {
 		airspeed_sensor_switching_necessary = true;
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -481,6 +481,10 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 	case TECS::ECL_TECS_MODE_CLIMBOUT:
 		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
 		break;
+
+	case TECS::ECL_TECS_MODE_STALL_PREVENTION:
+		t.mode = tecs_status_s::TECS_MODE_STALL_PREVENTION;
+		break;
 	}
 
 	t.altitude_sp = alt_sp;

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -860,6 +860,65 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f);
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f);
 
+/**
+ * Stall detection and recovery
+ *
+ * Define a crude stall detection based on positive altitude change setpoint, while sinking
+ * with a positive pitch as well as a saturated pitch command.
+ * If stall is detected, ramp up a command to sink with maximum sink rate as well as maximum speed.
+ *
+ * @boolean
+ * @group FW TECS
+ */
+PARAM_DEFINE_INT32(FW_T_STALL_PRV, 0);
+
+/**
+ * Stall prevention throttle trim scale increase.
+ *
+ *
+ * If a stall condition is detected, the throttle trim is increased to prevent future stall condition.
+ * The throttle is scaled up by the amount defined in this parameter each time a stall has been detected.
+ *
+ * @unit ms
+ * @min 0
+ * @max 5000
+ * @group FW TECS
+ */
+PARAM_DEFINE_INT32(FW_T_STALL_DELAY, 100);
+
+/**
+ * Stall prevention throttle trim scale increase.
+ *
+ *
+ * If a stall condition is detected, the throttle trim is increased to prevent future stall condition.
+ * The throttle is scaled up by the amount defined in this parameter each time a stall has been detected.
+ *
+ * @unit %
+ * @min 0.01
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_STALL_TH_SC, 0.05f);
+
+/**
+ * Stall action ramp up
+ *
+ *
+ * If a stall condition is detected, a sink rate and an airspeed increase is commanded. to avoid sudden steps
+ * those commands are ramped up. This parameters defines the amount of time spend in stall, before the commands
+ * are completely ramped up. When stall is prevented. the commands are ramped down within the same time.
+ *
+ * @unit s
+ * @min 0.01
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_STALL_RAMP, 0.2f);
+
 /**
  * GPS failure loiter time
  *