Add Range-based RTL

msg/CMakeLists.txt

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

msg/rtl_flight_time.msg

@@ -0,0 +1,5 @@
+uint64 timestamp				# time since system start (microseconds)
+
+uint8 rtl_vehicle_type				# from the vehicle_status message
+float32 rtl_time_s				# how long in seconds will the RTL take
+float32 rtl_limit_fraction			# what fraction of the allowable RTL time would be taken

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)

src/modules/navigator/rtl.cpp

@@ -43,6 +43,8 @@
 #include "navigator.h"
 #include <dataman/dataman.h>
 
+#include <climits>
+
 
 static constexpr float DELAY_SIGMA = 0.01f;
 
@@ -212,6 +214,23 @@ void RTL::find_RTL_destination()
 		}
 	}
 
+	// figure out how long the RTL will take
+	const vehicle_local_position_s &local_pos_s = *_navigator->get_local_position();
+
+	matrix::Vector3f local_pos(local_pos_s.x, local_pos_s.y, local_pos_s.z);
+	matrix::Vector3f local_destination; // TODO
+
+	uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type;
+	float xy_speed, z_speed;
+	get_rtl_xy_z_speed(vehicle_type, xy_speed, z_speed);
+	float time_to_home_s = time_to_home(local_pos, local_destination, get_wind(), xy_speed, z_speed);
+
+	float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get());
+	rtl_flight_time_s rtl_flight_time;
+	rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
+	rtl_flight_time.rtl_time_s = time_to_home_s;
+	rtl_flight_time.rtl_vehicle_type = vehicle_type;
+	_rtl_flight_time_pub.publish(rtl_flight_time);
 }
 
 void RTL::on_activation()
@@ -611,3 +630,67 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
 
 	return max(return_altitude_amsl, gpos.alt);
 }
+
+void RTL::get_rtl_xy_z_speed(uint8_t vehicle_type, float &xy, float &z)
+{
+	// Caution: here be dragons!
+	// Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover
+
+	if (vehicle_type != _rtl_vehicle_type) {
+		_rtl_vehicle_type = vehicle_type;
+
+		switch (vehicle_type) {
+		case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
+			_rtl_xy_speed = param_find("MPC_XY_CRUISE");
+			_rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN");
+			break;
+
+		case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
+			_rtl_xy_speed = param_find("FW_AIRSPD_TRIM");
+			_rtl_descent_speed = param_find("FW_T_SINK_MAX");
+			break;
+
+		case vehicle_status_s::VEHICLE_TYPE_ROVER:
+			_rtl_xy_speed = param_find("GND_SPEED_THR_SC");
+			_rtl_descent_speed = 65535;
+			break;
+		}
+	}
+
+	if (param_get(_rtl_xy_speed, &xy) != 0) {
+		xy = 1e6f;
+	}
+
+	if (param_get(_rtl_descent_speed, &z) != 0) {
+		z = 1e6f;
+	}
+
+}
+
+matrix::Vector2f RTL::get_wind()
+{
+	_wind_estimate_sub.update();
+	matrix::Vector2f wind(_wind_estimate_sub.get().windspeed_north, _wind_estimate_sub.get().windspeed_east);
+	return wind;
+}
+
+float time_to_home(const matrix::Vector3f &vehicle_local_pos,
+		   const matrix::Vector3f &rtl_point_local_pos,
+		   const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s)
+{
+	const matrix::Vector2f to_home = (rtl_point_local_pos - vehicle_local_pos).xy();
+	const float alt_change = rtl_point_local_pos(2) - vehicle_local_pos(2);
+	const matrix::Vector2f to_home_dir = to_home.unit_or_zero();
+	const float dist_to_home = to_home.norm();
+
+	const float wind_towards_home = wind_velocity.dot(to_home_dir);
+	const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * fabsf(wind_towards_home)).norm();
+
+	// Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient
+	const float cruise_speed = sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + fminf(0,
+				   wind_towards_home);
+
+	// assume horizontal and vertical motions happen serially, so their time adds
+	const float time_to_home = dist_to_home / cruise_speed + fabsf(alt_change) / vehicle_descent_speed_m_s;
+	return time_to_home;
+}

src/modules/navigator/rtl.h

@@ -46,7 +46,11 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/rtl_flight_time.h>
+#include <uORB/topics/wind_estimate.h>
+#include <matrix/math.hpp>
 
 class Navigator;
 
@@ -100,6 +104,8 @@ private:
 	 */
 	void advance_rtl();
 
+	void get_rtl_xy_z_speed(uint8_t vehicle_type, float &xy, float &z);
+	matrix::Vector2f get_wind();
 
 	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
 
@@ -150,6 +156,21 @@ private:
 		(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
 		(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type,
 		(ParamInt<px4::params::RTL_CONE_ANG>) _param_rtl_cone_half_angle_deg,
+		(ParamFloat<px4::params::RTL_FLT_TIME>) _param_rtl_flt_time,
 		(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
 	)
+
+	// These need to point at different parameters depending on vehicle type.
+	// Can't hard-code them because we have non-MC/FW/Rover builds
+	uint8_t _rtl_vehicle_type{255};
+	param_t _rtl_xy_speed;
+	param_t _rtl_descent_speed;
+
+	uORB::SubscriptionData<wind_estimate_s>		_wind_estimate_sub{ORB_ID(wind_estimate)};
+	uORB::PublicationData<rtl_flight_time_s>	_rtl_flight_time_pub{ORB_ID(rtl_flight_time)};
 };
+
+float time_to_home(const matrix::Vector3f &vehicle_local_pos,
+		   const matrix::Vector3f &rtl_point_local_pos,
+		   const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s,
+		   float vehicle_descent_speed_m_s);

src/modules/navigator/rtl_params.c

@@ -137,6 +137,16 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0);
  */
 PARAM_DEFINE_INT32(RTL_CONE_ANG, 45);
 
+/**
+ * Maximum allowed RTL flight in minutes
+ *
+ * This is used to determine when the vehicle should be switched to RTL due to low battery.
+ * Note, particularly for multirotors this should reflect flight time at cruise speed, not while stationary
+ *
+ * @group Commander
+ */
+PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15);
+
 /**
  * RTL precision land mode
  *
@@ -147,4 +157,4 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 45);
  * @value 2 Required precision landing
  * @group Return To Land
  */
-PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
+PARAM_DEFINE_INT32(RTL_PLD_MD, 0);