Plane: allow for NAV_LOITER_UNLIM and NAV_LOITER_TIME in quadplane

ArduPlane/ArduPlane.cpp

@@ -563,7 +563,7 @@ void Plane::update_flight_mode(void)
         break;
 
     case GUIDED:
-        if (auto_state.vtol_guided && quadplane.available()) {
+        if (auto_state.vtol_loiter && quadplane.available()) {
             quadplane.guided_update();
             break;
         }

ArduPlane/Plane.h

@@ -505,8 +505,8 @@ private:
         // are we in VTOL mode in AUTO?
         bool vtol_mode:1;
 
-        // are we doing GUIDED mode as a VTOL?
-        bool vtol_guided:1;
+        // are we doing loiter mode as a VTOL?
+        bool vtol_loiter:1;
     } auto_state;
 
     struct {

ArduPlane/commands.cpp

@@ -97,7 +97,7 @@ void Plane::set_guided_WP(void)
     loiter.start_time_ms = 0;
 
     // start in non-VTOL mode
-    auto_state.vtol_guided = false;
+    auto_state.vtol_loiter = false;
     
     loiter_angle_reset();
 }

ArduPlane/commands_logic.cpp

@@ -7,7 +7,10 @@
 /********************************************************************************/
 bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 {
-    // log when new commands start
+    // default to non-VTOL loiter
+    auto_state.vtol_loiter = false;
+
+        // log when new commands start
     if (should_log(MASK_LOG_CMD)) {
         DataFlash.Log_Write_Mission_Cmd(mission, cmd);
     }
@@ -636,6 +639,7 @@ bool Plane::verify_loiter_time()
 
     if (result) {
         gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER time complete");
+        auto_state.vtol_loiter = false;
     }
     return result;
 }
@@ -646,6 +650,9 @@ bool Plane::verify_loiter_turns()
     uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
     update_loiter(radius);
 
+    // LOITER_TURNS makes no sense as VTOL
+    auto_state.vtol_loiter = false;
+
     if (condition_value != 0) {
         // primary goal, loiter time
         if (loiter.sum_cd > loiter.total_cd && loiter.sum_cd > 1) {

ArduPlane/navigation.cpp

@@ -175,7 +175,13 @@ void Plane::update_loiter(uint16_t radius)
     if (loiter.start_time_ms != 0 &&
         quadplane.available() &&
         quadplane.guided_mode != 0) {
-        auto_state.vtol_guided = true;
+        if (!auto_state.vtol_loiter) {
+            auto_state.vtol_loiter = true;
+            // reset loiter start time, so we don't consider the point
+            // reached till we get much closer
+            loiter.start_time_ms = 0;
+            quadplane.guided_start();
+        }
     } else if (loiter.start_time_ms == 0 &&
         control_mode == AUTO &&
         !auto_state.no_crosstrack &&

ArduPlane/quadplane.cpp

@@ -1082,6 +1082,9 @@ bool QuadPlane::in_vtol_auto(void)
     case MAV_CMD_NAV_VTOL_LAND:
     case MAV_CMD_NAV_VTOL_TAKEOFF:
         return true;
+    case MAV_CMD_NAV_LOITER_UNLIM:
+    case MAV_CMD_NAV_LOITER_TIME:
+        return plane.auto_state.vtol_loiter;
     default:
         return false;
     }
@@ -1100,7 +1103,7 @@ bool QuadPlane::in_vtol_mode(void)
             plane.control_mode == QLOITER ||
             plane.control_mode == QLAND ||
             plane.control_mode == QRTL ||
-            (plane.control_mode == GUIDED && plane.auto_state.vtol_guided) ||
+            (plane.control_mode == GUIDED && plane.auto_state.vtol_loiter) ||
             in_vtol_auto());
 }
 
@@ -1399,14 +1402,19 @@ void QuadPlane::control_auto(const Location &loc)
 
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
-    if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) {
+    switch (plane.mission.get_current_nav_cmd().id) {
+    case MAV_CMD_NAV_VTOL_TAKEOFF:
         takeoff_controller();
-    } else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND) {
+        break;
+    case MAV_CMD_NAV_VTOL_LAND:
+    case MAV_CMD_NAV_LOITER_UNLIM:
+    case MAV_CMD_NAV_LOITER_TIME:
         vtol_position_controller();
-    } else {
+        break;
+    default:
         waypoint_controller();
+        break;
     }
-    
 }
 
 /*

ArduPlane/system.cpp

@@ -382,7 +382,7 @@ void Plane::set_mode(enum FlightMode mode)
 
     // assume non-VTOL mode
     auto_state.vtol_mode = false;
-    auto_state.vtol_guided = false;
+    auto_state.vtol_loiter = false;
     
     switch(control_mode)
     {