Plane: added Q_GUIDED_MODE parameter

this allows you to do hybrid VTOL and fixed wing guided mode

ArduPlane/ArduPlane.cpp

@@ -562,9 +562,15 @@ void Plane::update_flight_mode(void)
         handle_auto_mode();
         break;
 
+    case GUIDED:
+        if (auto_state.vtol_mode && quadplane.available()) {
+            quadplane.guided_update();
+            break;
+        }
+        // fall through
+
     case RTL:
     case LOITER:
-    case GUIDED:
         calc_nav_roll();
         calc_nav_pitch();
         calc_throttle();

ArduPlane/commands.cpp

@@ -93,6 +93,12 @@ void Plane::set_guided_WP(void)
     setup_glide_slope();
     setup_turn_angle();
 
+    // reset loiter start time.
+    loiter.start_time_ms = 0;
+
+    // start in non-VTOL mode
+    auto_state.vtol_mode = false;
+    
     loiter_angle_reset();
 }
 

ArduPlane/navigation.cpp

@@ -172,7 +172,11 @@ void Plane::update_loiter(uint16_t radius)
         }
     }
 
-    if (loiter.start_time_ms == 0 &&
+    if (loiter.start_time_ms != 0 &&
+        quadplane.available() &&
+        quadplane.guided_mode != 0) {
+        auto_state.vtol_mode = true;
+    } else if (loiter.start_time_ms == 0 &&
         control_mode == AUTO &&
         !auto_state.no_crosstrack &&
         get_distance(current_loc, next_WP_loc) > radius*2) {
@@ -192,6 +196,10 @@ void Plane::update_loiter(uint16_t radius)
                 // starting a loiter in GUIDED means we just reached the target point
                 gcs_send_mission_item_reached_message(0);
             }
+            if (quadplane.available() &&
+                quadplane.guided_mode != 0) {
+                quadplane.guided_start();
+            }
         }
     }
 }

ArduPlane/quadplane.cpp

@@ -1060,12 +1060,19 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state)
  */
 bool QuadPlane::in_vtol_auto(void)
 {
-    if (!enable || plane.control_mode != AUTO) {
+    if (!enable) {
+        return false;
+    }
+    if (plane.control_mode != AUTO &&
+        plane.control_mode != GUIDED) {
         return false;
     }
     if (plane.auto_state.vtol_mode) {
         return true;
     }
+    if (plane.control_mode == GUIDED) {
+        return false;
+    }
     switch (plane.mission.get_current_nav_cmd().id) {
     case MAV_CMD_NAV_VTOL_LAND:
     case MAV_CMD_NAV_VTOL_TAKEOFF:
@@ -1690,3 +1697,23 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
     // authority of the weathervane controller
     return weathervane.last_output * (yaw_rate_max/2) * 100;
 }
+
+/*
+  start guided mode control
+ */
+void QuadPlane::guided_start(void)
+{
+    poscontrol.state = QPOS_POSITION1;
+    poscontrol.speed_scale = 0;
+    setup_target_position();
+    poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
+}
+
+/*
+  update guided mode control
+ */
+void QuadPlane::guided_update(void)
+{
+    // run VTOL position controller
+    vtol_position_controller();
+}

ArduPlane/quadplane.h

@@ -182,6 +182,9 @@ private:
     
     // setup correct aux channels for frame class
     void setup_default_channels(uint8_t num_motors);
+
+    void guided_start(void);
+    void guided_update(void);
     
     AP_Int16 transition_time_ms;