Commander: make low remaining flight time configurable and fix clearing condition (#22863)

* Commander: make low remaining flight time configurable and do not clear

- add _ACT param to disable/warning/RTL this feature
- publish rtl flight time estimate also in RTL, and thus fix re-validation
- make failure message clearer, distinguish from battery low

* battery check: add hysteresis for declaring battery_low_remaining_time false again


---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: KonradRudin <98741601+KonradRudin@users.noreply.github.com>

src/lib/events/enums.json

@@ -381,6 +381,10 @@
                         "5": {
                             "name": "emergency_battery_level",
                             "description": "emergency battery level"
+                        },
+                        "6": {
+                            "name": "low_remaining_flight_time",
+                            "description": "low remaining flight time"
                         }
                     }
                 },

src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp

@@ -285,12 +285,16 @@ void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, f
 	rtl_time_estimate_s rtl_time_estimate;
 
 	// Compare estimate of RTL time to estimate of remaining flight time
+	// add hysterisis: if already in the condition, only get out of it if the remaining flight time is significantly higher again
+	const float hysteresis_factor = reporter.failsafeFlags().battery_low_remaining_time ? 1.1f : 1.0f;
+
 	reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate)
-			&& (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s
+			&& (hrt_absolute_time() - rtl_time_estimate.timestamp) < 3_s
 			&& rtl_time_estimate.valid
 			&& context.isArmed()
 			&& PX4_ISFINITE(worst_battery_time_s)
-			&& rtl_time_estimate.safe_time_estimate >= worst_battery_time_s;
+			&& rtl_time_estimate.safe_time_estimate * hysteresis_factor >= worst_battery_time_s;
+
 
 	if (reporter.failsafeFlags().battery_low_remaining_time) {
 		/* EVENT

src/modules/commander/commander_params.c

@@ -1030,3 +1030,17 @@ PARAM_DEFINE_INT32(COM_THROW_EN, 0);
  * @unit m/s
  */
 PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
+
+/**
+ * Remaining flight time low failsafe
+ *
+ * Action the system takes when the remaining flight time is below
+ * the estimated time it takes to reach the RTL destination.
+ *
+ * @group Commander
+ * @value 0 None
+ * @value 1 Warning
+ * @value 3 Return
+ * @increment 1
+ */
+PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);

src/modules/commander/failsafe/failsafe.cpp

@@ -365,6 +365,36 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
 	return options;
 }
 
+FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
+{
+	ActionOptions options{};
+
+	options.allow_user_takeover = UserTakeoverAllowed::Always;
+	options.cause = Cause::RemainingFlightTimeLow;
+
+	switch (command_after_remaining_flight_time_low(param_value)) {
+	case command_after_remaining_flight_time_low::None:
+		options.action = Action::None;
+		break;
+
+	case command_after_remaining_flight_time_low::Warning:
+		options.action = Action::Warn;
+		break;
+
+	case command_after_remaining_flight_time_low::Return_mode:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	default:
+		options.action = Action::None;
+		break;
+
+	}
+
+	return options;
+}
+
 void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 				 const failsafe_flags_s &status_flags)
 {
@@ -444,9 +474,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
 	CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
 
-	// Battery
+	// Battery flight time remaining failsafe
+
 	CHECK_FAILSAFE(status_flags, battery_low_remaining_time,
-		       ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm));
+		       ActionOptions(fromRemainingFlightTimeLowActParam(_param_com_fltt_low_act.get())));
 
 	if ((_armed_time != 0)
 	    && (time_us < _armed_time + static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s))
@@ -457,6 +488,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
 	}
 
+	// Battery low failsafe
 	switch (status_flags.battery_warning) {
 	case battery_status_s::BATTERY_WARNING_LOW:
 		_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,

src/modules/commander/failsafe/failsafe.h

@@ -141,6 +141,12 @@ private:
 		Land_mode = 5
 	};
 
+	enum class command_after_remaining_flight_time_low : int32_t {
+		None = 0,
+		Warning = 1,
+		Return_mode = 3
+	};
+
 	static ActionOptions fromNavDllOrRclActParam(int param_value);
 
 	static ActionOptions fromGfActParam(int param_value);
@@ -150,6 +156,7 @@ private:
 	static ActionOptions fromQuadchuteActParam(int param_value);
 	static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
 	static ActionOptions fromHighWindLimitActParam(int param_value);
+	static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
 
 	const int _caller_id_mode_fallback{genCallerId()};
 	bool _last_state_mode_fallback{false};
@@ -182,7 +189,8 @@ private:
 					(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
 					(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
 					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
-					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act
+					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
+					(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act
 				       );
 
 };

src/modules/commander/failsafe/framework.cpp

@@ -250,6 +250,11 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
 					events::send(events::ID("commander_failsafe_enter_crit_low_bat_warn"), {events::Log::Emergency, events::LogInternal::Info},
 						     "Emergency battery level, land immediately");
 
+				} else if (cause == Cause::RemainingFlightTimeLow) {
+					events::send(events::ID("commander_failsafe_enter_low_flight_time_warn"),
+					{events::Log::Warning, events::LogInternal::Info},
+					"Low remaining flight time, return advised");
+
 				} else {
 					/* EVENT
 					* @description No action is triggered.

src/modules/commander/failsafe/framework.h

@@ -83,6 +83,7 @@ public:
 		BatteryLow,
 		BatteryCritical,
 		BatteryEmergency,
+		RemainingFlightTimeLow,
 
 		Count
 	};

src/modules/navigator/rtl.cpp

@@ -208,32 +208,37 @@ void RTL::on_inactive()
 	if ((now - _destination_check_time) > 2_s) {
 		_destination_check_time = now;
 		setRtlTypeAndDestination();
-
-		const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0
-				&& hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s;
-
-		rtl_time_estimate_s estimated_time{};
-		estimated_time.valid = false;
-
-		if (_navigator->home_global_position_valid() && global_position_recently_updated) {
-			switch (_rtl_type) {
-			case RtlType::RTL_DIRECT:
-				estimated_time = _rtl_direct.calc_rtl_time_estimate();
-				break;
-
-			case RtlType::RTL_DIRECT_MISSION_LAND:
-			case RtlType::RTL_MISSION_FAST:
-			case RtlType::RTL_MISSION_FAST_REVERSE:
-				estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate();
-				break;
-
-			default:
-				break;
-			}
-		}
-
-		_rtl_time_estimate_pub.publish(estimated_time);
+		publishRemainingTimeEstimate();
 	}
+
+}
+
+void RTL::publishRemainingTimeEstimate()
+{
+	const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0
+			&& hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s;
+
+	rtl_time_estimate_s estimated_time{};
+	estimated_time.valid = false;
+
+	if (_navigator->home_global_position_valid() && global_position_recently_updated) {
+		switch (_rtl_type) {
+		case RtlType::RTL_DIRECT:
+			estimated_time = _rtl_direct.calc_rtl_time_estimate();
+			break;
+
+		case RtlType::RTL_DIRECT_MISSION_LAND:
+		case RtlType::RTL_MISSION_FAST:
+		case RtlType::RTL_MISSION_FAST_REVERSE:
+			estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate();
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	_rtl_time_estimate_pub.publish(estimated_time);
 }
 
 void RTL::on_activation()
@@ -287,6 +292,14 @@ void RTL::on_active()
 	default:
 		break;
 	}
+
+	// Keep publishing remaining time estimates every 2 seconds
+	hrt_abstime now{hrt_absolute_time()};
+
+	if ((now - _destination_check_time) > 2_s) {
+		_destination_check_time = now;
+		publishRemainingTimeEstimate();
+	}
 }
 
 void RTL::setRtlTypeAndDestination()

src/modules/navigator/rtl.h

@@ -106,6 +106,12 @@ private:
 
 	void setRtlTypeAndDestination();
 
+	/**
+	 * @brief Publish the remaining time estimate to go to the RTL landing point.
+	 *
+	 */
+	void publishRemainingTimeEstimate();
+
 	/**
 	 * @brief Find RTL destination.
 	 *