rtl: implemented RTL based on cone shape

- user defines a cone half angle which defines the RTL altitude behavior
- if vehicle is outside the cone it will climb until it intersects the cone
and then return at this altitude.
- if vehicle is inside the cone or above the predefined return altitude (RTL_RETURN_ALT)
then it will return at the current altitude

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/rtl.cpp

@@ -67,6 +67,9 @@ RTL::rtl_type() const
 void
 RTL::on_activation()
 {
+
+	_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
+
 	if (_navigator->get_land_detected()->landed) {
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
@@ -133,14 +136,6 @@ RTL::set_rtl_item()
 	// Check if we are pretty close to home already.
 	const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
 
-	// Compute the return altitude.
-	float return_alt = math::max(home.alt + _param_rtl_return_alt.get(), gpos.alt);
-
-	// We are close to home, limit climb to min.
-	if (home_dist < _param_rtl_min_dist.get()) {
-		return_alt = home.alt + _param_rtl_descend_alt.get();
-	}
-
 	// Compute the loiter altitude.
 	const float loiter_altitude = math::min(home.alt + _param_rtl_descend_alt.get(), gpos.alt);
 
@@ -150,7 +145,7 @@ RTL::set_rtl_item()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.lat = gpos.lat;
 			_mission_item.lon = gpos.lon;
-			_mission_item.altitude = return_alt;
+			_mission_item.altitude = _rtl_alt;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.yaw = _navigator->get_local_position()->yaw;
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@@ -159,7 +154,7 @@ RTL::set_rtl_item()
 			_mission_item.origin = ORIGIN_ONBOARD;
 
 			mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
-						     (int)ceilf(return_alt), (int)ceilf(return_alt - _navigator->get_home_position()->alt));
+						     (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _navigator->get_home_position()->alt));
 			break;
 		}
 
@@ -169,7 +164,7 @@ RTL::set_rtl_item()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
-			_mission_item.altitude = return_alt;
+			_mission_item.altitude = _rtl_alt;
 			_mission_item.altitude_is_relative = false;
 
 			// Use home yaw if close to home.
@@ -353,3 +348,45 @@ RTL::advance_rtl()
 		break;
 	}
 }
+
+
+float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
+{
+	const home_position_s &home = *_navigator->get_home_position();
+	const vehicle_global_position_s &gpos = *_navigator->get_global_position();
+
+	float rtl_altitude;
+
+	if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
+		rtl_altitude = gpos.alt;
+
+	} else if (cone_half_angle_deg <= 0) {
+		rtl_altitude = home.alt + _param_rtl_return_alt.get();
+
+	} else {
+		// horizontal distance to home position
+		const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
+
+		// constrain cone half angle to meaningful values. All other cases are already handled above.
+		const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));
+
+		// minimum height above home position required
+		float height_above_home_min = home_dist / tanf(cone_half_angle_rad);
+
+		// minimum altitude we need in order to be within the user defined cone
+		const float altitude_min = math::constrain(height_above_home_min + home.alt, home.alt,
+					   home.alt + _param_rtl_return_alt.get());
+
+		if (gpos.alt < altitude_min) {
+			rtl_altitude = altitude_min;
+
+		} else {
+			rtl_altitude = gpos.alt;
+		}
+	}
+
+	// always demand altitude which is higher or equal the RTL descend altitude
+	rtl_altitude = math::max(rtl_altitude, home.alt + _param_rtl_descend_alt.get());
+
+	return rtl_altitude;
+}

src/modules/navigator/rtl.h

@@ -80,6 +80,9 @@ private:
 	 */
 	void		advance_rtl();
 
+
+	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
+
 	enum RTLState {
 		RTL_STATE_NONE = 0,
 		RTL_STATE_CLIMB,
@@ -91,6 +94,7 @@ private:
 		RTL_STATE_LANDED,
 	} _rtl_state{RTL_STATE_NONE};
 
+	float _rtl_alt;	// AMSL altitude at which the vehicle should return to the home position
 	bool _rtl_alt_min{false};
 
 	DEFINE_PARAMETERS(
@@ -98,6 +102,7 @@ private:
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
 		(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay,
 		(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
-		(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type
+		(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type,
+		(ParamInt<px4::params::RTL_CONE_ANG>) _param_rtl_cone_half_angle_deg
 	)
 };

src/modules/navigator/rtl_params.c

@@ -116,3 +116,16 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f);
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_TYPE, 0);
+
+/**
+ * Half-angle of the RTL cone.
+ *
+ * Defines the half-angle of the cone which defines the vehicle RTL behavior.
+ *
+ * @unit degrees
+ * @min 0
+ * @max 90
+ * @increment 1
+ * @group Return Mode
+ */
+PARAM_DEFINE_INT32(RTL_CONE_ANG, 30);