Compute RTL time and react if lower than flight time

- Compute RTL time also during RTL
- Calculate correct altitude when finding destination

msg/CMakeLists.txt

@@ -131,7 +131,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_flight_time.msg

@@ -1,4 +0,0 @@
-uint64 timestamp				# time since system start (microseconds)
-
-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

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

@@ -3606,17 +3606,6 @@ void Commander::avoidance_check()
 
 void Commander::battery_status_check()
 {
-	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.
@@ -3624,52 +3613,62 @@ void Commander::battery_status_check()
 	// 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;
-
-	_rtl_flight_time_sub.update();
-	float battery_usage_to_home = 0;
+			if (battery.timestamp < oldest_update) {
+				oldest_update = battery.timestamp;
+			}
 
-	if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) {
-		battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction;
+			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;
 			}
 
-	uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE;
+			// Sum up current from all batteries.
+			_battery_current += battery.current_filtered_a;
+		}
+	}
 
-	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

@@ -77,7 +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/rtl_time_estimate.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
@@ -321,6 +321,8 @@ private:
 	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};
@@ -408,6 +410,7 @@ private:
 	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)};
@@ -431,7 +434,6 @@ 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/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/rtl.cpp

@@ -57,6 +57,14 @@ RTL::RTL(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
+	_param_mpc_z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP");
+	_param_mpc_z_vel_max_down = param_find("MPC_Z_VEL_MAX_DN");
+	_param_mpc_land_speed = param_find("MPC_LAND_SPEED");
+	_param_fw_climb_rate = param_find("FW_T_CLMB_R_SP");
+	_param_fw_sink_rate = param_find("FW_T_SINK_R_SP");
+	_param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM");
+	_param_mpc_xy_cruise = param_find("MPC_XY_CRUISE");
+	_param_rover_cruise_speed = param_find("GND_SPEED_THR_SC");
 }
 
 void RTL::on_inactivation()
@@ -71,22 +79,20 @@ void RTL::on_inactive()
 	// Reset RTL state.
 	_rtl_state = RTL_STATE_NONE;
 
-	find_RTL_destination();
-}
+	// Limit inactive calculation to 1Hz
+	if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
+		_destination_check_time = hrt_absolute_time();
 
-void RTL::find_RTL_destination()
-{
-	// don't update RTL destination faster than 1 Hz
-	if (hrt_elapsed_time(&_destination_check_time) < 1_s) {
-		return;
+		if (_navigator->home_position_valid()) {
+			find_RTL_destination();
 		}
 
-	if (!_navigator->home_position_valid()) {
-		return;
+		calc_and_pub_rtl_time_estimate();
 	}
+}
 
-	_destination_check_time = hrt_absolute_time();
-
+void RTL::find_RTL_destination()
+{
 	// get home position:
 	home_position_s &home_landing_position = *_navigator->get_home_position();
 
@@ -225,23 +231,13 @@ void RTL::find_RTL_destination()
 		}
 	}
 
-	// figure out how long the RTL will take
-	float rtl_xy_speed, rtl_z_speed;
-	get_rtl_xy_z_speed(rtl_xy_speed, rtl_z_speed);
-
-	matrix::Vector3f to_destination_vec;
-	get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
-				    &to_destination_vec(0), &to_destination_vec(1));
-	to_destination_vec(2) = _destination.alt - global_position.alt;
-
-	float time_to_home_s = time_to_home(to_destination_vec, get_wind(), rtl_xy_speed, rtl_z_speed);
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
 
-	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.timestamp = hrt_absolute_time();
-	rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
-	rtl_flight_time.rtl_time_s = time_to_home_s;
-	_rtl_flight_time_pub.publish(rtl_flight_time);
+	} else {
+		_rtl_alt = max(global_position.alt, max(_destination.alt,
+							_navigator->get_home_position()->alt + _param_rtl_return_alt.get()));
+	}
 }
 
 void RTL::on_activation()
@@ -268,14 +264,6 @@ void RTL::on_activation()
 
 	_rtl_loiter_rad = _param_rtl_loiter_rad.get();
 
-	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
-
-	} else {
-		_rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get());
-	}
-
-
 	if (_navigator->get_land_detected()->landed) {
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
@@ -316,6 +304,12 @@ void RTL::on_active()
 	} else if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
 	}
+
+	// Limit rtl time calculation to 1Hz
+	if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
+		_destination_check_time = hrt_absolute_time();
+		calc_and_pub_rtl_time_estimate();
+	}
 }
 
 void RTL::set_rtl_item()
@@ -730,41 +724,148 @@ 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(float &xy, float &z)
+void RTL::calc_and_pub_rtl_time_estimate()
 {
-	uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type;
-	// Caution: here be dragons!
-	// Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover
+	rtl_time_estimate_s rtl_time_estimate{};
+
+	// Calculate RTL time estimate only when there is a valid home position
+	// TODO: Also check if vehicle position is valid
+	if (!_navigator->home_position_valid()) {
+		rtl_time_estimate.valid = false;
 
-	if (vehicle_type != _rtl_vehicle_type) {
-		_rtl_vehicle_type = vehicle_type;
+	} else {
+		rtl_time_estimate.valid = true;
 
-		switch (vehicle_type) {
-		case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
-			_param_rtl_xy_speed = param_find("MPC_XY_CRUISE");
-			_param_rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN");
-			break;
+		const vehicle_global_position_s &gpos = *_navigator->get_global_position();
+
+		// Sum up time estimate for various segments of the landing procedure
+		switch (_rtl_state) {
+		case RTL_STATE_NONE:
+		case RTL_STATE_CLIMB: {
+				// Climb segment is only relevant if the drone is below return altitude
+				const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0;
+
+				if (climb_dist > 0) {
+					rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
+				}
+			}
+
+		// FALLTHROUGH
+		case RTL_STATE_RETURN:
+
+			// Add cruise segment to home
+			rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
+					_destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed();
+
+		// FALLTHROUGH
+		case RTL_STATE_HEAD_TO_CENTER:
+		case RTL_STATE_TRANSITION_TO_MC:
+		case RTL_STATE_DESCEND: {
+				// when descending, the target altitude is stored in the current mission item
+				float initial_altitude = 0;
+				float loiter_altitude = 0;
+
+				if (_rtl_state == RTL_STATE_DESCEND) {
+					// Take current vehicle altitude as the starting point for calculation
+					initial_altitude = gpos.alt;  // TODO: Check if this is in the right frame
+					loiter_altitude = _mission_item.altitude;  // Next waypoint = loiter
+
+
+				} else {
+					// Take the return altitude as the starting point for the calculation
+					initial_altitude = _rtl_alt; // CLIMB and RETURN
+					loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
+				}
+
+				// Add descend segment (first landing phase: return alt to loiter alt)
+				rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
+			}
+
+		// FALLTHROUGH
+		case RTL_STATE_LOITER:
+			// Add land delay (the short pause for deploying landing gear)
+			// TODO: Check if landing gear is deployed or not
+			rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
+
+		// FALLTHROUGH
+		case RTL_MOVE_TO_LAND_HOVER_VTOL:
+		case RTL_STATE_LAND: {
+				float initial_altitude;
+
+				// Add land segment (second landing phase) which comes after LOITER
+				if (_rtl_state == RTL_STATE_LAND) {
+					// If we are in this phase, use the current vehicle altitude  instead
+					// of the altitude paramteter to get a continous time estimate
+					initial_altitude = gpos.alt;
+
+
+				} else {
+					// If this phase is not active yet, simply use the loiter altitude,
+					// which is where the LAND phase will start
+					const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
+					initial_altitude = loiter_altitude;
+				}
+
+				// Prevent negative times when close to the ground
+				if (initial_altitude > _destination.alt) {
+					rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed();
+				}
+
+			}
 
-		case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
-			_param_rtl_xy_speed = param_find("FW_AIRSPD_TRIM");
-			_param_rtl_descent_speed = param_find("FW_T_SINK_MIN");
 			break;
 
-		case vehicle_status_s::VEHICLE_TYPE_ROVER:
-			_param_rtl_xy_speed = param_find("GND_SPEED_THR_SC");
-			_param_rtl_descent_speed = PARAM_INVALID;
+		case RTL_STATE_LANDED:
+			// Remaining time is 0
 			break;
 		}
+
+		// Prevent negative durations as phyiscally they make no sense. These can
+		// occur during the last phase of landing when close to the ground.
+		rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate);
+
+		// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
+		rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate
+						       + _param_rtl_time_margin.get();
 	}
 
-	if ((_param_rtl_xy_speed == PARAM_INVALID) || param_get(_param_rtl_xy_speed, &xy) != PX4_OK) {
-		xy = 1e6f;
+	// Publish message
+	rtl_time_estimate.timestamp = hrt_absolute_time();
+	_rtl_time_estimate_pub.publish(rtl_time_estimate);
+}
+
+float RTL::getCruiseSpeed()
+{
+	float ret = 1e6f;
+
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) {
+			ret = 1e6f;
 		}
 
-	if ((_param_rtl_descent_speed == PARAM_INVALID) || param_get(_param_rtl_descent_speed, &z) != PX4_OK) {
-		z = 1e6f;
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
+		if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+	}
+
+	return ret;
+}
+
+float RTL::getHoverLandSpeed()
+{
+	float ret = 1e6f;
+
+	if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) {
+		ret = 1e6f;
 	}
 
+	return ret;
 }
 
 matrix::Vector2f RTL::get_wind()
@@ -780,27 +881,67 @@ matrix::Vector2f RTL::get_wind()
 	return wind;
 }
 
-float time_to_home(const matrix::Vector3f &to_home_vec,
-		   const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s)
+float RTL::getClimbRate()
 {
-	const matrix::Vector2f to_home = to_home_vec.xy();
-	const float alt_change = to_home_vec(2);
-	const matrix::Vector2f to_home_dir = to_home.unit_or_zero();
-	const float dist_to_home = to_home.norm();
+	float ret = 1e6f;
+
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (_param_mpc_z_vel_max_up == PARAM_INVALID || param_get(_param_mpc_z_vel_max_up, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+
+		if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+	}
+
+	return ret;
+}
+
+float RTL::getDescendRate()
+{
+	float ret = 1e6f;
+
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (_param_mpc_z_vel_max_down == PARAM_INVALID || param_get(_param_mpc_z_vel_max_down, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+	}
+
+	return ret;
+}
+
+float RTL::getCruiseGroundSpeed()
+{
+	float cruise_speed = getCruiseSpeed();
+
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		const vehicle_global_position_s &global_position = *_navigator->get_global_position();
+		matrix::Vector2f wind = get_wind();
+
+		matrix::Vector2f to_destination_vec;
+		get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
+					    &to_destination_vec(0), &to_destination_vec(1));
+
+		const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero();
+
+		const float wind_towards_home = wind.dot(to_home_dir);
+		const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_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 * 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(
+		const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf(
 						   0.f, wind_towards_home);
 
-	if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) {
-		return INFINITY; // we never reach home if the wind is stronger than vehicle speed
+		cruise_speed = ground_speed;
 	}
 
-	// assume horizontal and vertical motions happen serially, so their time adds
-	float horiz = dist_to_home / cruise_speed;
-	float descent = fabsf(alt_change) / vehicle_descent_speed_m_s;
-	return horiz + descent;
+	return cruise_speed;
 }

src/modules/navigator/rtl.h

@@ -48,7 +48,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/rtl_flight_time.h>
+#include <uORB/topics/rtl_time_estimate.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <matrix/math.hpp>
@@ -109,6 +109,17 @@ private:
 	void advance_rtl();
 
 	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
+	void calc_and_pub_rtl_time_estimate();
+
+	float getCruiseGroundSpeed();
+
+	float getClimbRate();
+
+	float getDescendRate();
+
+	float getCruiseSpeed();
+
+	float getHoverLandSpeed();
 
 	enum RTLState {
 		RTL_STATE_NONE = 0,
@@ -159,21 +170,25 @@ 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,
 		(ParamFloat<px4::params::RTL_LOITER_RAD>)  _param_rtl_loiter_rad,
-		(ParamInt<px4::params::RTL_HDG_MD>)        _param_rtl_hdg_md
+		(ParamInt<px4::params::RTL_HDG_MD>)        _param_rtl_hdg_md,
+		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
+		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin
 	)
 
-	// 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{vehicle_status_s::VEHICLE_TYPE_UNKNOWN};
+	param_t		_param_mpc_z_vel_max_up{PARAM_INVALID};
+	param_t		_param_mpc_z_vel_max_down{PARAM_INVALID};
+	param_t		_param_mpc_land_speed{PARAM_INVALID};
+	param_t		_param_fw_climb_rate{PARAM_INVALID};
+	param_t		_param_fw_sink_rate{PARAM_INVALID};
 
-	param_t _param_rtl_xy_speed{PARAM_INVALID};
-	param_t _param_rtl_descent_speed{PARAM_INVALID};
+	param_t 	_param_fw_airspeed_trim{PARAM_INVALID};
+	param_t 	_param_mpc_xy_cruise{PARAM_INVALID};
+	param_t 	_param_rover_cruise_speed{PARAM_INVALID};
 
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
-	uORB::Publication<rtl_flight_time_s>	_rtl_flight_time_pub{ORB_ID(rtl_flight_time)};
+	uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
 };
 
 float time_to_home(const matrix::Vector3f &to_home_vec,

src/modules/navigator/rtl_params.c

@@ -137,17 +137,6 @@ 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
- *
- * @unit min
- * @group Commander
- */
-PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15);
-
 /**
  * RTL precision land mode
  *
@@ -185,3 +174,32 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_HDG_MD, 0);
+
+/**
+ * RTL time estimate safety margin factor
+ *
+ * Safety factor that is used to scale the actual RTL time estiamte.
+ * Time with margin = RTL_TIME_FACTOR * time + RTL_TIME_MARGIN
+ *
+ * @min 1.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 0.1
+ * @group Return To Land
+ */
+PARAM_DEFINE_FLOAT(RTL_TIME_FACTOR, 1.1f);
+
+/**
+ * RTL time estimate safety margin offset
+ *
+ * Margin that is added to the time estimate, after it has already been scaled
+ * Time with margin = RTL_TIME_FACTOR * time + RTL_TIME_MARGIN
+ *
+ * @unit s
+ * @min 0
+ * @max 300
+ * @decimal 1
+ * @increment 1
+ * @group Return To Land
+ */
+PARAM_DEFINE_INT32(RTL_TIME_MARGIN, 110);