[RTL] Add VTOL land approach for home with designated loiter points i…

…n the RTL mode.

src/modules/navigator/CMakeLists.txt

@@ -50,7 +50,8 @@ set(NAVIGATOR_SOURCES
 	land.cpp
 	precland.cpp
 	mission_feasibility_checker.cpp
-	geofence.cpp)
+	geofence.cpp
+	vtol_land.cpp)
 
 if(CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF)
 	set(NAVIGATOR_SOURCES

src/modules/navigator/navigation.h

@@ -228,6 +228,18 @@ struct mission_fence_point_s {
 	uint8_t _padding0[5];				/**< padding struct size to alignment boundary  */
 };
 
+/**
+ * @brief Return to launch position.
+ * Defines the position and landing yaw for the return to launch destination.
+ *
+ */
+struct RtlPosition {
+	double lat;	/**< latitude in WGS84 [rad].*/
+	double lon;	/**< longitude in WGS84 [rad].*/
+	float alt;	/**< altitude in MSL [m].*/
+	float yaw;	/**< final yaw when landed [rad].*/
+};
+
 #if (__GNUC__ >= 5) || __clang__
 #pragma GCC diagnostic pop
 #endif // GCC >= 5 || Clang

src/modules/navigator/navigator_main.cpp

@@ -714,7 +714,19 @@ void Navigator::run()
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_rtl;
+
+#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
+
+			// If we are in VTOL takeoff, do not switch until it is finished.
+			if (_navigation_mode == &_vtol_takeoff && !get_mission_result()->finished) {
+				navigation_mode_new = &_vtol_takeoff;
+
+			} else
+#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
+			{
+				navigation_mode_new = &_rtl;
+			}
+
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: