RTL based on remaining flight time estimation

msg/CMakeLists.txt

@@ -130,7 +130,7 @@ set(msg_files
 	rc_channels.msg
 	rc_parameter_map.msg
 	rpm.msg
-	rtl_flight_time.msg
+	rtl_time_estimate.msg
 	safety.msg
 	satellite_info.msg
 	sensor_accel.msg

msg/rtl_time_estimate.msg

@@ -0,0 +1,5 @@
+uint64 timestamp # time since system start (microseconds)
+
+bool valid			# Flag indicating whether the time estiamtes are valid
+float32 time_estimate		# [s] Estimated time for RTL
+float32 safe_time_estimate	# [s] Same as time_estimate, but with safety factor and safety margin included (factor*t + margin)

src/modules/commander/Commander.cpp

@@ -2134,7 +2134,9 @@ Commander::run()
 
 		_cpuload_sub.update(&_cpuload);
 
-		battery_status_check();
+		if (_battery_status_subs.updated()) {
+			battery_status_check();
+		}
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
@@ -3523,77 +3525,69 @@ void Commander::avoidance_check()
 
 void Commander::battery_status_check()
 {
-	// We need to update the status flag if ANY battery is updated, because the system source might have
-	// changed, or might be nothing (if there is no battery connected)
-	if (!_battery_status_subs.updated()) {
-		// Nothing has changed since the last time this function was called, so nothing needs to be done now.
-		return;
-	}
-
-	battery_status_s batteries[_battery_status_subs.size()];
-	size_t num_connected_batteries = 0;
-
-	for (auto &battery_sub : _battery_status_subs) {
-		if (battery_sub.copy(&batteries[num_connected_batteries])) {
-			if (batteries[num_connected_batteries].connected) {
-				num_connected_batteries++;
-			}
-		}
-	}
-
 	// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
 	// option is to check if ANY of them have a warning, and specifically find which one has the most
 	// urgent warning.
 	uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
 	// To make sure that all connected batteries are being regularly reported, we check which one has the
 	// oldest timestamp.
 	hrt_abstime oldest_update = hrt_absolute_time();
+	size_t num_connected_batteries{0};
+	float worst_battery_time_s{NAN};
 
 	_battery_current = 0.0f;
-	float battery_level = 0.0f;
-
 
-	// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
-	for (size_t i = 0; i < num_connected_batteries; i++) {
-		if (batteries[i].warning > worst_warning) {
-			worst_warning = batteries[i].warning;
-		}
+	for (auto &battery_sub : _battery_status_subs) {
+		battery_status_s battery;
 
-		if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) {
-			oldest_update = batteries[i].timestamp;
+		if (!battery_sub.copy(&battery)) {
+			continue;
 		}
 
-		// Sum up current from all batteries.
-		_battery_current += batteries[i].current_filtered_a;
+		if (battery.connected) {
+			num_connected_batteries++;
 
-		// average levels from all batteries
-		battery_level += batteries[i].remaining;
-	}
+			if (battery.warning > worst_warning) {
+				worst_warning = battery.warning;
+			}
 
-	battery_level /= num_connected_batteries;
+			if (battery.timestamp < oldest_update) {
+				oldest_update = battery.timestamp;
+			}
 
-	_rtl_flight_time_sub.update();
-	float battery_usage_to_home = 0;
+			if (PX4_ISFINITE(battery.time_remaining_s)
+			    && (!PX4_ISFINITE(worst_battery_time_s)
+				|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
+				worst_battery_time_s = battery.time_remaining_s;
+			}
 
-	if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) {
-		battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction;
+			// Sum up current from all batteries.
+			_battery_current += battery.current_filtered_a;
+		}
 	}
 
-	uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE;
-
-	if (PX4_ISFINITE(battery_usage_to_home)) {
-		float battery_at_home = battery_level - battery_usage_to_home;
+	rtl_time_estimate_s rtl_time_estimate{};
 
-		if (battery_at_home < _param_bat_crit_thr.get()) {
-			battery_range_warning =  battery_status_s::BATTERY_WARNING_CRITICAL;
+	// Compare estimate of RTL time to estimate of remaining flight time
+	if (_rtl_time_estimate_sub.copy(&rtl_time_estimate)
+	    && hrt_absolute_time() - rtl_time_estimate.timestamp < 2_s
+	    && rtl_time_estimate.valid
+	    && _armed.armed
+	    && !_rtl_time_actions_done
+	    && PX4_ISFINITE(worst_battery_time_s)
+	    && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s
+	    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
+	    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+		// Try to trigger RTL
+		if (main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
+					  _internal_state) == TRANSITION_CHANGED) {
+			mavlink_log_emergency(&_mavlink_log_pub, "Flight time low, returning to land");
 
-		} else if (battery_at_home < _param_bat_low_thr.get()) {
-			battery_range_warning = battery_status_s::BATTERY_WARNING_LOW;
+		} else {
+			mavlink_log_emergency(&_mavlink_log_pub, "Flight time low, land now!");
 		}
-	}
 
-	if (battery_range_warning > worst_warning) {
-		worst_warning = battery_range_warning;
+		_rtl_time_actions_done = true;
 	}
 
 	bool battery_warning_level_increased_while_armed = false;

src/modules/commander/Commander.hpp

@@ -76,7 +76,7 @@
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/power_button_state.h>
-#include <uORB/topics/rtl_flight_time.h>
+#include <uORB/topics/rtl_time_estimate.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
@@ -319,6 +319,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool		_geofence_warning_action_on{false};
 	bool		_geofence_violated_prev{false};
 
+	bool		_rtl_time_actions_done{false};
+
 	FailureDetector	_failure_detector;
 	bool		_flight_termination_triggered{false};
 	bool		_lockdown_triggered{false};
@@ -404,6 +406,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription					_rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
@@ -427,7 +430,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionData<rtl_flight_time_s>		_rtl_flight_time_sub{ORB_ID(rtl_flight_time)};
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};

src/modules/logger/logged_topics.cpp

@@ -83,7 +83,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
 	add_optional_topic("rpm", 500);
-	add_topic("rtl_flight_time", 1000);
+	add_topic("rtl_time_estimate", 1000);
 	add_topic("safety");
 	add_topic("sensor_combined");
 	add_optional_topic("sensor_correction");

src/modules/navigator/CMakeLists.txt

@@ -56,5 +56,3 @@ px4_add_module(
 		geofence_breach_avoidance
 		motion_planning
 	)
-
-px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman)