Plane: implement Q_OPTIONS

this allows for NAV_TAKEOFF to be treated as NAV_VTOL_TAKEOFF in
quadplanes, and for level flight to be maintained during quadplane
transitions

ArduPlane/Attitude.cpp

@@ -663,6 +663,15 @@ void Plane::update_load_factor(void)
     }
     aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));
 
+    if (quadplane.in_transition() &&
+        (quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION)) {
+        /*
+          the user has asked for transitions to be kept level to
+          within LEVEL_ROLL_LIMIT
+         */
+        roll_limit_cd = MIN(roll_limit_cd, g.level_roll_limit*100);
+    }
+    
     if (!aparm.stall_prevention) {
         // stall prevention is disabled
         return;

ArduPlane/altitude.cpp

@@ -603,7 +603,7 @@ void Plane::rangefinder_height_update(void)
                 (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
                  control_mode == QLAND ||
                  control_mode == QRTL ||
-                 (control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) &&
+                 (control_mode == AUTO && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) &&
                 g.rangefinder_landing) {
                 rangefinder_state.in_use = true;
                 gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);

ArduPlane/commands_logic.cpp

@@ -32,7 +32,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 
         AP_Mission::Mission_Command next_nav_cmd;
         const uint16_t next_index = mission.get_current_nav_index() + 1;
-        auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
+        auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND) &&
+            !quadplane.is_vtol_land(next_nav_cmd.id);
 
         gcs().send_text(MAV_SEVERITY_INFO, "Executing nav command ID #%i",cmd.id);
     } else {
@@ -43,6 +44,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 
     case MAV_CMD_NAV_TAKEOFF:
         crash_state.is_crashed = false;
+        if (quadplane.is_vtol_takeoff(cmd.id)) {
+            return quadplane.do_vtol_takeoff(cmd);
+        }
         do_takeoff(cmd);
         break;
 
@@ -51,6 +55,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_LAND:              // LAND to Waypoint
+        if (quadplane.is_vtol_land(cmd.id)) {
+            crash_state.is_crashed = false;
+            return quadplane.do_vtol_land(cmd);            
+        }
         do_land(cmd);
         break;
 
@@ -238,12 +246,18 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     switch(cmd.id) {
 
     case MAV_CMD_NAV_TAKEOFF:
+        if (quadplane.is_vtol_takeoff(cmd.id)) {
+            return quadplane.verify_vtol_takeoff(cmd);
+        }
         return verify_takeoff();
 
     case MAV_CMD_NAV_WAYPOINT:
         return verify_nav_wp(cmd);
 
     case MAV_CMD_NAV_LAND:
+        if (quadplane.is_vtol_land(cmd.id)) {
+            return quadplane.verify_vtol_land();            
+        }
         if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) {
             return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);
 
@@ -349,6 +363,9 @@ void Plane::do_RTL(int32_t rtl_altitude)
     DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
 }
 
+/*
+  start a NAV_TAKEOFF command
+ */
 void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
 {
     prev_WP_loc = current_loc;

ArduPlane/is_flying.cpp

@@ -303,7 +303,8 @@ bool Plane::in_preLaunch_flight_stage(void) {
     return (control_mode == AUTO &&
             throttle_suppressed &&
             flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL &&
-            mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF);
+            mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF &&
+            !quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id));
 }
 
 

ArduPlane/quadplane.cpp

@@ -399,6 +399,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Description: This controls the mavlink type given in HEARTBEAT messages. For some GCS types a particular setting will be needed for correct operation.
     // @Values: 0:AUTO,1:FIXED_WING,2:QUADROTOR,3:COAXIAL,4:HELICOPTER,7:AIRSHIP,8:FREE_BALLOON,9:ROCKET,10:GROUND_ROVER,11:SURFACE_BOAT,12:SUBMARINE,16:FLAPPING_WING,17:KITE,19:VTOL_DUOROTOR,20:VTOL_QUADROTOR,21:VTOL_TILTROTOR
     AP_GROUPINFO("MAV_TYPE", 57, QuadPlane, mav_type, 0),
+
+    // @Param: OPTIONS
+    // @DisplayName: quadplane options
+    // @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight. If AllowFWTakeoff is disabled then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand is disabled then fixed wing land on quadplanes will instead perform a VTOL land.
+    // @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand
+    AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
     
     AP_GROUPEND
 };
@@ -1463,7 +1469,7 @@ void QuadPlane::check_throttle_suppression(void)
     }
 
     // allow for takeoff
-    if (plane.control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) {
+    if (plane.control_mode == AUTO && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
         return;
     }
     
@@ -1631,7 +1637,8 @@ bool QuadPlane::in_vtol_auto(void)
     if (plane.auto_state.vtol_mode) {
         return true;
     }
-    switch (plane.mission.get_current_nav_cmd().id) {
+    uint16_t id = plane.mission.get_current_nav_cmd().id;
+    switch (id) {
     case MAV_CMD_NAV_VTOL_LAND:
     case MAV_CMD_NAV_VTOL_TAKEOFF:
         return true;
@@ -1640,6 +1647,10 @@ bool QuadPlane::in_vtol_auto(void)
     case MAV_CMD_NAV_LOITER_TURNS:
     case MAV_CMD_NAV_LOITER_TO_ALT:
         return plane.auto_state.vtol_loiter;
+    case MAV_CMD_NAV_TAKEOFF:
+        return is_vtol_takeoff(id);
+    case MAV_CMD_NAV_LAND:
+        return is_vtol_land(id);
     default:
         return false;
     }
@@ -1970,9 +1981,18 @@ void QuadPlane::control_auto(const Location &loc)
 
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
-    switch (plane.mission.get_current_nav_cmd().id) {
+    uint16_t id = plane.mission.get_current_nav_cmd().id;
+    switch (id) {
     case MAV_CMD_NAV_VTOL_TAKEOFF:
-        takeoff_controller();
+    case MAV_CMD_NAV_TAKEOFF:
+        if (is_vtol_takeoff(id)) {
+            takeoff_controller();
+        }
+        break;
+    case MAV_CMD_NAV_LAND:
+        if (is_vtol_land(id)) {
+            vtol_position_controller();
+        }
         break;
     case MAV_CMD_NAV_VTOL_LAND:
     case MAV_CMD_NAV_LOITER_UNLIM:
@@ -2440,3 +2460,43 @@ uint8_t QuadPlane::get_mav_type(void) const
     }
     return uint8_t(mav_type.get());
 }
+
+/*
+  return true if current mission item is a vtol takeoff
+*/
+bool QuadPlane::is_vtol_takeoff(uint16_t id) const
+{
+    if (id == MAV_CMD_NAV_VTOL_TAKEOFF) {
+        return true;
+    }
+    if (id == MAV_CMD_NAV_TAKEOFF && available() && (options & OPTION_ALLOW_FW_TAKEOFF) == 0) {
+        // treat fixed wing takeoff as VTOL takeoff
+        return true;
+    }
+    return false;
+}
+
+/*
+  return true if current mission item is a vtol land
+*/
+bool QuadPlane::is_vtol_land(uint16_t id) const
+{
+    if (id == MAV_CMD_NAV_VTOL_LAND) {
+        return true;
+    }
+    if (id == MAV_CMD_NAV_LAND && available() && (options & OPTION_ALLOW_FW_LAND) == 0) {
+        // treat fixed wing land as VTOL land
+        return true;
+    }
+    return false;
+}
+
+/*
+  return true if we are in a transition to fwd flight from hover
+ */
+bool QuadPlane::in_transition(void) const
+{
+    return available() && assisted_flight &&
+        (transition_state == TRANSITION_AIRSPEED_WAIT ||
+         transition_state == TRANSITION_TIMER);
+}

ArduPlane/quadplane.h

@@ -49,6 +49,11 @@ public:
     bool in_assisted_flight(void) const {
         return available() && assisted_flight;
     }
+
+    /*
+      return true if we are in a transition to fwd flight from hover
+    */
+    bool in_transition(void) const;
     
     bool handle_do_vtol_transition(enum MAV_VTOL_STATE state);
 
@@ -304,7 +309,7 @@ private:
 
     // are we in a guided takeoff?
     bool guided_takeoff:1;
-
+    
     struct {
         // time when motors reached lower limit
         uint32_t lower_limit_start_ms;
@@ -418,6 +423,24 @@ private:
     
     // adjust altitude target smoothly
     void adjust_alt_target(float target_cm);
+
+    // additional options
+    AP_Int32 options;
+    enum {
+        OPTION_LEVEL_TRANSITION=(1<<0),
+        OPTION_ALLOW_FW_TAKEOFF=(1<<1),
+        OPTION_ALLOW_FW_LAND=(1<<2),
+    };
+
+    /*
+      return true if current mission item is a vtol takeoff
+     */
+    bool is_vtol_takeoff(uint16_t id) const;
+
+    /*
+      return true if current mission item is a vtol landing
+     */
+    bool is_vtol_land(uint16_t id) const;
     
 public:
     void motor_test_output();

ArduPlane/system.cpp

@@ -463,7 +463,8 @@ void Plane::exit_mode(enum FlightMode mode)
         if (mission.state() == AP_Mission::MISSION_RUNNING) {
             mission.stop();
 
-            if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND)
+            if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
+                !quadplane.is_vtol_land(mission.get_current_nav_cmd().id))
             {
                 landing.restart_landing_sequence();
             }