Add Range-based RTL

src/modules/commander/Commander.cpp

@@ -3858,6 +3858,8 @@ void Commander::battery_status_check()
 	hrt_abstime oldest_update = hrt_absolute_time();
 
 	_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++) {
@@ -3871,6 +3873,36 @@ void Commander::battery_status_check()
 
 		// Sum up current from all batteries.
 		_battery_current += batteries[i].current_filtered_a;
+
+		// average levels from all batteries
+		battery_level += batteries[i].remaining;
+	}
+
+	battery_level /= num_connected_batteries;
+
+	_rtl_flight_time_sub.update();
+	float battery_usage_to_home = _rtl_flight_time_sub.valid() ?
+				      _rtl_flight_time_sub.get().rtl_limit_fraction : 0;
+
+
+	auto warning_level = [this](float battery_level_fraction, float battery_to_home) {
+		float battery_at_home = battery_level_fraction - battery_to_home;
+
+		if (battery_at_home < _param_bat_crit_thr.get()) {
+			return battery_status_s::BATTERY_WARNING_CRITICAL;
+		}
+
+		if (battery_at_home < _param_bat_low_thr.get()) {
+			return battery_status_s::BATTERY_WARNING_LOW;
+		}
+
+		return battery_status_s::BATTERY_WARNING_NONE;
+	};
+
+	uint8_t battery_range_warning = warning_level(battery_level, battery_usage_to_home);
+
+	if (battery_range_warning > worst_warning) {
+		worst_warning = battery_range_warning;
 	}
 
 	bool battery_warning_level_increased_while_armed = false;
@@ -3892,6 +3924,7 @@ void Commander::battery_status_check()
 		_battery_warning = worst_warning;
 	}
 
+
 	_status_flags.condition_battery_healthy =
 		// All connected batteries are regularly being published
 		(hrt_elapsed_time(&oldest_update) < 5_s)

src/modules/commander/Commander.hpp

@@ -33,6 +33,8 @@
 
 #pragma once
 
+
+/*   Helper classes  */
 #include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
 #include "state_machine_helper.h"
@@ -75,6 +77,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/safety.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
@@ -269,7 +272,10 @@ private:
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
 		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
 
-		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist
+		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist,
+
+		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
+		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr
 	)
 
 	enum class PrearmedMode {
@@ -426,6 +432,7 @@ private:
 	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/commander/state_machine_helper.cpp

@@ -985,9 +985,6 @@ void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsaf
 	}
 }
 
-
-
-
 void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
 		      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
 		      const low_battery_action_t low_battery_action)