RTL: fix RTL time estimation (#23807)

* RTL Direct: fix rlt time prediction in final lanidng phase It was previously checking if RTL was already running through active(), which though actually is coming from the mission mode that rtl_direct inherits from. Signed-off-by: Silvan Fuhrer <[email protected]> * RTL Direct: remove unnecessary active() check As the method setRtlPosition() is anyway not called when RTL is active, plus it checks the wrong thing, as it is the active() method from the Mission mode. Signed-off-by: Silvan Fuhrer <[email protected]> * RTL Direct: remove unnecessary land_detector_sub.update() It is already getting updated just before the .get() Signed-off-by: Silvan Fuhrer <[email protected]> * RTL time estiate: do not distinguish land from sink for MC To avoid rtl time prediction jump when entering LAND phase due to no correct handling of loiter altitude (LAND phase doesn't have to start only when lower then RTL_DESCND_ALT). Signed-off-by: Silvan Fuhrer <[email protected]> * Revert "RTL Direct: remove unnecessary active() check" This reverts commit d5165ba. * Revert "RTL Direct: fix rlt time prediction in final lanidng phase" This reverts commit 5af7c92. * RTL: Make sure to call the initialilze function of the Navigator RTL modes * RTL: use the navigator_mode run function instead of the on_xxx function directly * RTL: Make sure that for vtol the right vehicle type is used for each RTL state * RTL: move to loiter distance estimate should substract the loiter radius for fixed wing * RTL: time prediction: do not assume VTOL is in FW at start of RTL Signed-off-by: Silvan Fuhrer <[email protected]> * RTL: time estimation: fix is_in_climbing_submode Signed-off-by: Silvan Fuhrer <[email protected]> * RTL: time estimation: subtract loiter radius from distance in rtl_direct_mission_land Signed-off-by: Silvan Fuhrer <[email protected]> * RTL direct: poll important topics also on_inactive such that time estimate is correct Signed-off-by: Silvan Fuhrer <[email protected]> * navigator rtl: fix setter spacing * navigator rtl: check pointer before dereferencing * RTL: only subract loiter radius when in FW Signed-off-by: Silvan Fuhrer <[email protected]> --------- Signed-off-by: Silvan Fuhrer <[email protected]> Co-authored-by: Konrad <[email protected]> Co-authored-by: Matthias Grob <[email protected]>
PX4 · Oct 24, 2024 · 3b0dac6 · 3b0dac6
1 parent 84bb6d1
commit 3b0dac6
Show file tree

Hide file tree

Showing 7 changed files with 93 additions and 102 deletions.
diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp
@@ -51,7 +51,6 @@ RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr)
 {
 	_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
 	_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_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");
@@ -79,7 +78,6 @@ rtl_time_estimate_s RtlTimeEstimator::getEstimate() const
 
 void RtlTimeEstimator::update()
 {
-	_vehicle_status_sub.update();
 	_wind_sub.update();
 
 	if (_parameter_update_sub.updated()) {
@@ -130,22 +128,11 @@ void RtlTimeEstimator::addWait(float time_s)
 	}
 }
 
-void RtlTimeEstimator::addDescendMCLand(float alt)
-{
-	if (PX4_ISFINITE(alt)) {
-		_is_valid = true;
-
-		if (alt < -FLT_EPSILON && PX4_ISFINITE(alt)) {
-			_time_estimate += -alt / getHoverLandSpeed();
-		}
-	}
-}
-
 float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_norm)
 {
 	float cruise_speed = getCruiseSpeed();
 
-	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+	if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		matrix::Vector2f wind = get_wind();
 
 		const float wind_along_dir = wind.dot(direction_norm);
@@ -186,17 +173,17 @@ float RtlTimeEstimator::getCruiseSpeed()
 {
 	float ret = 1e6f;
 
-	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	if (_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 (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+	} else if (_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;
 		}
 
-	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
+	} else if (_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;
 		}
@@ -205,17 +192,6 @@ float RtlTimeEstimator::getCruiseSpeed()
 	return ret;
 }
 
-float RtlTimeEstimator::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 RtlTimeEstimator::get_wind()
 {
 	_wind_sub.update();
@@ -233,12 +209,12 @@ float RtlTimeEstimator::getClimbRate()
 {
 	float ret = 1e6f;
 
-	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) {
 			ret = 1e6f;
 		}
 
-	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+	} else if (_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;
@@ -252,12 +228,12 @@ float RtlTimeEstimator::getDescendRate()
 {
 	float ret = 1e6f;
 
-	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) {
 			ret = 1e6f;
 		}
 
-	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+	} else if (_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;
 		}

diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h
@@ -63,12 +63,12 @@ class RtlTimeEstimator : public ModuleParams
 	~RtlTimeEstimator() = default;
 
 	void update();
-	void reset() { _time_estimate = 0.f; _is_valid = false;};
+	void reset() { _time_estimate = 0.f; _is_valid = false; };
 	rtl_time_estimate_s getEstimate() const;
 	void addDistance(float hor_dist, const matrix::Vector2f &hor_direction, float vert_dist);
 	void addVertDistance(float alt);
 	void addWait(float time_s);
-	void addDescendMCLand(float alt);
+	void setVehicleType(uint8_t vehicle_type) { _vehicle_type = vehicle_type; };
 
 private:
 	/**
@@ -106,31 +106,25 @@ class RtlTimeEstimator : public ModuleParams
 	 */
 	float getCruiseSpeed();
 
-	/**
-	 * @brief Get the Hover Land Speed
-	 *
-	 * @return Hover land speed [m/s]
-	 */
-	float getHoverLandSpeed();
-
 	/**
 	 * @brief Get the horizontal wind velocity
 	 *
 	 * @return horizontal wind velocity.
 	 */
 	matrix::Vector2f get_wind();
 
-	float _time_estimate; 		/**< Accumulated time estimate [s] */
+	float _time_estimate{0.f}; 		/**< Accumulated time estimate [s] */
 	bool _is_valid{false};		/**< Checks if time estimate is valid */
 
+	uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; /**< the defined vehicle type to use for the calculation*/
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,  /**< Safety factory for safe time estimate */
 		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin   /**< Safety margin for safe time estimate */
 	)
 
 	param_t		_param_mpc_z_v_auto_up{PARAM_INVALID}; 	/**< MC climb velocity parameter */
 	param_t		_param_mpc_z_v_auto_dn{PARAM_INVALID};  /**< MC descend velocity parameter */
-	param_t		_param_mpc_land_speed{PARAM_INVALID};   /**< MC land descend speed parameter */
 	param_t		_param_fw_climb_rate{PARAM_INVALID};    /**< FW climb speed parameter */
 	param_t		_param_fw_sink_rate{PARAM_INVALID};     /**< FW descend speed parameter */
 
@@ -139,7 +133,6 @@ class RtlTimeEstimator : public ModuleParams
 	param_t 	_param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< Parameter update topic */
-	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};	/**< vehicle status subscription */
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};		/**< wind topic */
 };
 

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
@@ -59,6 +59,7 @@ RTL::RTL(Navigator *navigator) :
 	ModuleParams(navigator),
 	_rtl_direct(navigator)
 {
+	_rtl_direct.initialize();
 }
 
 void RTL::updateDatamanCache()
@@ -157,24 +158,6 @@ void RTL::updateDatamanCache()
 	_dataman_cache_landItem.update();
 }
 
-void RTL::on_inactivation()
-{
-	switch (_rtl_type) {
-	case RtlType::RTL_MISSION_FAST: // Fall through
-	case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through
-	case RtlType::RTL_DIRECT_MISSION_LAND:
-		_rtl_mission_type_handle->on_inactivation();
-		break;
-
-	case RtlType::RTL_DIRECT:
-		_rtl_direct.on_inactivation();
-		break;
-
-	default:
-		break;
-	}
-}
-
 void RTL::on_inactive()
 {
 	_global_pos_sub.update();
@@ -187,21 +170,12 @@ void RTL::on_inactive()
 
 	parameters_update();
 
-	switch (_rtl_type) {
-	case RtlType::RTL_MISSION_FAST:
-	case RtlType::RTL_MISSION_FAST_REVERSE:
-	case RtlType::RTL_DIRECT_MISSION_LAND:
-		_rtl_mission_type_handle->on_inactive();
-		break;
-
-	case RtlType::RTL_DIRECT:
-		_rtl_direct.on_inactive();
-		break;
-
-	default:
-		break;
+	if (_rtl_mission_type_handle) {
+		_rtl_mission_type_handle->run(false);
 	}
 
+	_rtl_direct.run(false);
+
 	// Limit inactive calculation to 0.5Hz
 	hrt_abstime now{hrt_absolute_time()};
 
@@ -230,7 +204,10 @@ void RTL::publishRemainingTimeEstimate()
 		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();
+			if (_rtl_mission_type_handle) {
+				estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate();
+			}
+
 			break;
 
 		default:
@@ -250,12 +227,10 @@ void RTL::on_activation()
 	case RtlType::RTL_MISSION_FAST: // Fall through
 	case RtlType::RTL_MISSION_FAST_REVERSE:
 		_rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt);
-		_rtl_mission_type_handle->on_activation();
 		break;
 
 	case RtlType::RTL_DIRECT:
 		_rtl_direct.setReturnAltMin(_enforce_rtl_alt);
-		_rtl_direct.on_activation();
 		break;
 
 	default:
@@ -279,16 +254,23 @@ void RTL::on_active()
 	updateDatamanCache();
 
 	switch (_rtl_type) {
-	case RtlType::RTL_MISSION_FAST:
-	case RtlType::RTL_MISSION_FAST_REVERSE:
+	case RtlType::RTL_MISSION_FAST: // Fall through
+	case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through
 	case RtlType::RTL_DIRECT_MISSION_LAND:
-		_rtl_mission_type_handle->on_active();
-		_rtl_mission_type_handle->updateFailsafeChecks();
+		if (_rtl_mission_type_handle) {
+			_rtl_mission_type_handle->run(true);
+		}
+
+		_rtl_direct.run(false);
 		break;
 
 	case RtlType::RTL_DIRECT:
-		_rtl_direct.on_active();
-		_rtl_direct.updateFailsafeChecks();
+		_rtl_direct.run(true);
+
+		if (_rtl_mission_type_handle) {
+			_rtl_mission_type_handle->run(false);
+		}
+
 		break;
 
 	default:
@@ -312,7 +294,10 @@ bool RTL::isLanding()
 	case RtlType::RTL_MISSION_FAST:
 	case RtlType::RTL_MISSION_FAST_REVERSE:
 	case RtlType::RTL_DIRECT_MISSION_LAND:
-		is_landing = _rtl_mission_type_handle->isLanding();
+		if (_rtl_mission_type_handle) {
+			is_landing = _rtl_mission_type_handle->isLanding();
+		}
+
 		break;
 
 	case RtlType::RTL_DIRECT:
@@ -622,6 +607,10 @@ void RTL::init_rtl_mission_type()
 	default:
 		break;
 	}
+
+	if (_rtl_mission_type_handle) {
+		_rtl_mission_type_handle->initialize();
+	}
 }
 
 void RTL::parameters_update()

diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
@@ -78,7 +78,6 @@ class RTL : public NavigatorMode, public ModuleParams
 		RTL_MISSION_FAST_REVERSE,
 	};
 
-	void on_inactivation() override;
 	void on_inactive() override;
 	void on_activation() override;
 	void on_active() override;

diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp
@@ -73,7 +73,6 @@ void RtlDirect::on_inactivation()
 void RtlDirect::on_activation()
 {
 	_global_pos_sub.update();
-	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 
 	parameters_update();
@@ -120,6 +119,12 @@ void RtlDirect::on_active()
 	}
 }
 
+void RtlDirect::on_inactive()
+{
+	_global_pos_sub.update();
+	_vehicle_status_sub.update();
+}
+
 void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s loiter_pos)
 {
 	_home_pos_sub.update();
@@ -421,7 +426,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 {
 	_global_pos_sub.update();
 	_rtl_time_estimator.update();
-
+	_rtl_time_estimator.setVehicleType(_vehicle_status_sub.get().vehicle_type);
 	_rtl_time_estimator.reset();
 
 	RTLState start_state_for_estimate;
@@ -455,8 +460,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 				matrix::Vector2f direction{};
 				get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
 							    land_approach.lon, &direction(0), &direction(1));
-				_rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
-								land_approach.lat, land_approach.lon), direction, 0.f);
+				float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)};
+
+				if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+					move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m);
+				}
+
+				_rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f);
 			}
 
 		// FALLTHROUGH
@@ -527,7 +537,11 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 					initial_altitude = loiter_altitude;
 				}
 
-				_rtl_time_estimator.addDescendMCLand(_destination.alt - initial_altitude);
+				if (_vehicle_status_sub.get().is_vtol) {
+					_rtl_time_estimator.setVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+				}
+
+				_rtl_time_estimator.addVertDistance(_destination.alt - initial_altitude);
 			}
 
 			break;
@@ -560,7 +574,6 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con
 	if (!PX4_ISFINITE(land_approach.lat) || !PX4_ISFINITE(land_approach.lon)) {
 		sanitized_land_approach.lat = _destination.lat;
 		sanitized_land_approach.lon = _destination.lon;
-
 	}
 
 	if (!PX4_ISFINITE(land_approach.height_m)) {

diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h
@@ -91,6 +91,13 @@ class RtlDirect : public MissionBlock, public ModuleParams
 	 */
 	void on_active() override;
 
+	/**
+	 * @brief on inactive
+	 * Poll required topics also when incative for rtl time estimate.
+	 *
+	 */
+	void on_inactive() override;
+
 	/**
 	 * @brief Calculate the estimated time needed to return to launch.
 	 *