RTL: during climb, set NAV_CMD_WAYPOINT for RW modes, NAV_CMD_LOITER_…

…TO_ALT for FW Do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, even if current clib altitude is below (e.g. RTL immediately after take off) Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · Dec 29, 2020 · 0bc54f2 · 0bc54f2
1 parent 9eaef07
commit 0bc54f2
Showing 1 changed file with 9 additions and 1 deletion.
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
@@ -319,7 +319,15 @@ void RTL::set_rtl_item()
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
 
-			_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+			// do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT,
+			// even if current climb altitude is below (e.g. RTL immediately after take off)
+			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+			} else {
+				_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+			}
+
 			_mission_item.lat = gpos.lat;
 			_mission_item.lon = gpos.lon;
 			_mission_item.altitude = _rtl_alt;