Merge pull request PX4#12 from aviant-tech/rtl_mr_descend_1.13

Faster MR descend for RTL - ported to 1.13

src/modules/navigator/rtl.cpp

@@ -49,6 +49,7 @@
 
 
 static constexpr float DELAY_SIGMA = 0.01f;
+static constexpr float ALT_SIGMA = 0.01f;
 
 using namespace time_literals;
 using namespace math;
@@ -334,6 +335,8 @@ void RTL::set_rtl_item()
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
 	const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
 	const float loiter_altitude = min(descend_altitude_target, _rtl_alt);
+	const float mc_descend_altitude_target = min(_destination.alt + _param_rtl_descend_mc.get(), gpos.alt);
+	const float mc_descend_altitude = min(mc_descend_altitude_target, _rtl_alt);
 
 	const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
 
@@ -505,6 +508,9 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _navigator->get_local_position()->heading;
 			}
 
+			_mission_item.vtol_back_transition = true;
+			// acceptance_radius will be overwritten since vtol_back_transition is set,
+			// set as a default value only
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
@@ -542,6 +548,28 @@ void RTL::set_rtl_item()
 			break;
 		}
 
+	case RTL_STATE_MC_DESCEND: {
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.lat = _destination.lat;
+			_mission_item.lon = _destination.lon;
+
+			if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
+				_mission_item.yaw = _navigator->get_local_position()->heading;
+
+			} else {
+				_mission_item.yaw = _destination.yaw;
+			}
+
+			_mission_item.altitude = mc_descend_altitude;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: MC descend to %d m (%d m above destination)",
+					 (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
+			break;
+		}
+
 	case RTL_STATE_LAND: {
 			// Land at destination.
 			_mission_item.nav_cmd = NAV_CMD_LAND;
@@ -604,73 +632,87 @@ void RTL::set_rtl_item()
 
 void RTL::advance_rtl()
 {
-	// determines if the vehicle should loiter above land
-	const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA;
+	// vehicle is a VTOL, either in FW or MC mode
+	const bool is_vtol = _navigator->get_vstatus()->is_vtol;
+
+	// vehicle is FW, which could be a VTOL in FW mode
+	const bool is_fw = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 
-	// vehicle is a vtol and currently in fixed wing mode
-	const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol
-				     && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+	// determines if the vehicle should loiter above land, only relevant in FW
+	const bool do_fw_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA;
+
+	// determines if the vehicle should descent in MC before land
+	const bool do_mc_descend = _param_rtl_descend_mc.get() > ALT_SIGMA;
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB:
 		_rtl_state = RTL_STATE_RETURN;
 		break;
 
 	case RTL_STATE_RETURN:
-		setClimbAndReturnDone(true);
-
-		if (vtol_in_fw_mode || descend_and_loiter) {
+		if (is_fw) {
 			_rtl_state = RTL_STATE_DESCEND;
 
-		} else {
+		} else if (do_mc_descend) {
+			_rtl_state = RTL_STATE_MC_DESCEND;
+
+		} else {  // MC with RTL_DESCEND_MC <= 0
 			_rtl_state = RTL_STATE_LAND;
 		}
 
 		break;
 
 	case RTL_STATE_DESCEND:
 
-		if (descend_and_loiter) {
+		// We should normally always be in FW in this state,
+		// but we handle MC as well
+		if (is_fw && do_fw_loiter) {
 			_rtl_state = RTL_STATE_LOITER;
 
-		} else if (vtol_in_fw_mode) {
+		} else if (is_fw && is_vtol) {
 			_rtl_state = RTL_STATE_HEAD_TO_CENTER;
 
-		} else {
+		} else if (!is_fw && do_mc_descend) {
+			_rtl_state = RTL_STATE_MC_DESCEND;
+
+		} else {  // FW and not VTOL, or MC
 			_rtl_state = RTL_STATE_LAND;
 		}
 
 		break;
 
 	case RTL_STATE_LOITER:
-		if (vtol_in_fw_mode) {
-			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
+		if (is_vtol) {
+			_rtl_state = RTL_STATE_HEAD_TO_CENTER;
 
-		} else {
+		} else {  // FW and not VTOL
 			_rtl_state = RTL_STATE_LAND;
 		}
 
-		_rtl_state = RTL_STATE_LAND;
 		break;
 
 	case RTL_STATE_HEAD_TO_CENTER:
-
 		_rtl_state = RTL_STATE_TRANSITION_TO_MC;
-
 		break;
 
 	case RTL_STATE_TRANSITION_TO_MC:
-
 		_rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL;
-
 		break;
 
 	case RTL_MOVE_TO_LAND_HOVER_VTOL:
+		if (do_mc_descend) {
+			_rtl_state = RTL_STATE_MC_DESCEND;
 
-		_rtl_state = RTL_STATE_LAND;
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
 
 		break;
 
+	case RTL_STATE_MC_DESCEND:
+		_rtl_state = RTL_STATE_LAND;
+		break;
+
 	case RTL_STATE_LAND:
 		_rtl_state = RTL_STATE_LANDED;
 		break;
@@ -782,6 +824,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
 
 		// FALLTHROUGH
 		case RTL_MOVE_TO_LAND_HOVER_VTOL:
+		case RTL_STATE_MC_DESCEND:
 		case RTL_STATE_LAND: {
 				float initial_altitude;
 

src/modules/navigator/rtl.h

@@ -132,6 +132,7 @@ class RTL : public MissionBlock, public ModuleParams
 		RTL_STATE_LAND,
 		RTL_STATE_LANDED,
 		RTL_STATE_HEAD_TO_CENTER,
+		RTL_STATE_MC_DESCEND,
 	} _rtl_state{RTL_STATE_NONE};
 
 	struct RTLPosition {
@@ -167,6 +168,7 @@ class RTL : public MissionBlock, public ModuleParams
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_RETURN_ALT>)  _param_rtl_return_alt,
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
+		(ParamFloat<px4::params::RTL_DESCEND_MC>)  _param_rtl_descend_mc,
 		(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,

src/modules/navigator/rtl_params.c

@@ -64,6 +64,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
  *
  * Descend to this altitude (above destination position) after return, and wait for time defined in RTL_LAND_DELAY.
  * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ * Only used for FW vehicles (including VTOL in FW mode).
  *
  * @unit m
  * @min 2
@@ -79,6 +80,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
  *
  * Delay before landing (after initial descent) in Return mode.
  * If set to -1 the system will not land but loiter at RTL_DESCEND_ALT.
+ * Only used for FW vehicles (including VTOL in FW mode).
  *
  * @unit s
  * @min -1
@@ -89,6 +91,26 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, 0.0f);
 
+/**
+ * Return mode MC descend altitude
+ *
+ * Descend to this altitude (above destination position) before starting the actual landing.
+ * Only for MC vehicles (including VTOL in MC mode).
+ * For VTOLs that start RTL in FW, the vehicle will descend to this altitide
+ * after loitering at RTL_RETURN_ALT in FW and transition to MC.
+ * This allows for a faster MC descent than the landing speed to conserve battery,
+ * while still setting RTL_DESCEND_ALT at an altitude high enough for safe FW loitering.
+ * Set to -1 to disable. Should be less than RTL_DESCEND_ALT if vehicle is VTOL.
+ *
+ * @unit m
+ * @min -1
+ * @max 100
+ * @decimal 1
+ * @increment 0.5
+ * @group Return Mode
+ */
+PARAM_DEFINE_FLOAT(RTL_DESCEND_MC, -1.0f);
+
 /**
  * Horizontal radius from return point within which special rules for return mode apply.
  *