dr rtl flight task: implemented flight task completion based on estimated flown distance

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp

@@ -51,11 +51,13 @@ bool FlightTaskReturnDeadReckoning::activate(const trajectory_setpoint_s &last_s
 	_updateSubscriptions();
 	_state = State::INIT;
 
-	if (!(_updateBearingToHome() && _computeReturnParameters() && _initializeSmoothers())) {
+	if (!(_updateBearingToHome() && _initializeSmoothers())) {
 		PX4_ERR("Failed to initialize task");
 		return false;
 	}
 
+	_computeReturnParameters();
+
 	return true;
 }
 
@@ -77,6 +79,7 @@ bool FlightTaskReturnDeadReckoning::update()
 
 	_updateState();
 	_updateSetpoints();
+	_updateDistanceFlownEstimate();
 
 	return true;
 }
@@ -110,7 +113,7 @@ void FlightTaskReturnDeadReckoning::_updateState()
 		break;
 
 	case State::RETURN:
-		if (_isWithinHomePositionRadius()) {
+		if (_isReturnComplete()) {
 			_state = State::HOLD;
 			events::send<float>(events::ID("dead_reckon_rtl_hold"), events::Log::Info,
 					    "Holding altitude at {1:.2m_v} over home position", _rtl_alt);
@@ -186,11 +189,23 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
 	return;
 }
 
+void FlightTaskReturnDeadReckoning::_updateDistanceFlownEstimate()
+{
+	if (_state == State::RETURN) {
+		_distance_flown_estimate += _param_mpc_xy_vel_max.get() * _deltatime;
+	}
+}
+
 bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
 {
 	if (_readHomePosition(_home_position)) {
 		_readGlobalPosition(_start_vehicle_global_position);
 		_bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position);
+
+		_distance_flown_estimate = .0f;
+		_initial_distance_to_home = get_distance_to_next_waypoint(
+			_start_vehicle_global_position(0), _start_vehicle_global_position(1),
+			_home_position(0), _home_position(1));
 	}
 
 	return !isnanf(_bearing_to_home);
@@ -234,14 +249,11 @@ float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_gl
 	return bearing;
 }
 
-bool FlightTaskReturnDeadReckoning::_computeReturnParameters()
+void FlightTaskReturnDeadReckoning::_computeReturnParameters()
 {
-	_rtl_alt = _param_rtl_return_alt.get();
+	_rtl_alt = math::max((float) _start_vehicle_global_position(2), (float) _home_position(2) + _param_rtl_return_alt.get());
-	_rtl_alt = math::max((float) _start_vehicle_global_position(2), (float) _home_position(2) + _rtl_alt);
 
 	_rtl_acc = _param_mpc_acc_hor_max.get();
-
-	return true;
 }
 
 void FlightTaskReturnDeadReckoning::_updateSubscriptions()
@@ -281,14 +293,20 @@ bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const
 	return current_alt > target_alt;
 }
 
-bool FlightTaskReturnDeadReckoning::_isWithinHomePositionRadius()
+bool FlightTaskReturnDeadReckoning::_isReturnComplete()
 {
+	bool ret = false;
 	if (_isGlobalPositionValid()) {
+		// Close enough to home posititon
 		_readGlobalPosition(_start_vehicle_global_position);
-		return get_distance_to_next_waypoint(
+		ret = get_distance_to_next_waypoint(
 			       _start_vehicle_global_position(0), _start_vehicle_global_position(1),
 			       _home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
 	}
 
-	return false;
+	if (!ret) {
+		ret = _distance_flown_estimate > 2.5f * _initial_distance_to_home; // 2.5x the initial distance to home
+	}
+
+	return ret;
 }

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp

@@ -66,6 +66,11 @@ private:
 	 */
 	void _updateSetpoints();
 
+	/**
+	 * Update estimated flown distance since the last time home bearing was updated
+	 */
+	void _updateDistanceFlownEstimate();
+
 	/**
 	 * Update bearing to home position with the last available GNSS position
 	 * @return true on success, false on error
@@ -91,10 +96,9 @@ private:
 			      const matrix::Vector3d &_global_position_end);
 
 	/**
-	 * Computes return altitude velocity
+	 * Computes return altitude and acceleration
-	 * @return true on success, false on error
 	 */
-	bool _computeReturnParameters();
+	void _computeReturnParameters();
 
 	/**
 	 * Update uORB subscriptions
@@ -120,15 +124,17 @@ private:
 	bool _isAboveReturnAltitude() const;
 
 	/**
-	 * Check if the vehicle is within the home position waypoint acceptance radius
+	 * Check if the vehicle is within the home position waypoint acceptance radius, or if the distance traveled is greater than the initial distance to home (times a factor)
-	 * @return true if within, false otherwise
+	 * @return true if completed, false otherwise
 	 */
-	bool _isWithinHomePositionRadius();
+	bool _isReturnComplete();
 
 
 	matrix::Vector3d _home_position;			/**< Stores home position */
 	matrix::Vector3d _start_vehicle_global_position;	/**< Stores vehicle last known GNSS position */
 	float _bearing_to_home{0.0f};				/**< Stores bearing between home and last GNSS position */
+	float _initial_distance_to_home{0.0f};			/**< Initial distance to home position */
+	float _distance_flown_estimate{0.0f};
 
 	HeadingSmoothing _heading_smoothing;			/**< Smoother for heading */
 	SlewRate<float> _slew_rate_acceleration_x{0.0f};	/**< Slew rate for x-acceleration setpoint */
@@ -157,6 +163,7 @@ private:
 					(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,		//< maximum jerk in auto modes
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,	//< max vertical velocity up
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,	//< max vertical acceleration up
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,	//< max horizontal velocity
 					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad,	//< max vertical error
 					(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad		//< max horizontal error
 				       );