Plane: added support for VTOL_TAKEOFF and VTOL_LAND commands

ArduPlane/Plane.h

@@ -813,6 +813,8 @@ private:
     bool verify_change_alt();
     bool verify_within_distance();
     bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);
+    bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
+    bool verify_vtol_land(void);
     void do_loiter_at_location();
     void do_take_picture();
     void log_picture();
@@ -987,6 +989,8 @@ private:
     void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
     void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
+    void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
+    void do_vtol_land(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
     void do_change_alt(const AP_Mission::Mission_Command& cmd);

ArduPlane/commands_logic.cpp

@@ -82,6 +82,14 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         do_altitude_wait(cmd);
         break;
 
+    case MAV_CMD_NAV_VTOL_TAKEOFF:
+        crash_state.is_crashed = false;
+        return quadplane.do_vtol_takeoff(cmd);
+
+    case MAV_CMD_NAV_VTOL_LAND:
+        crash_state.is_crashed = false;
+        return quadplane.do_vtol_land(cmd);
+        
     // Conditional commands
 
     case MAV_CMD_CONDITION_DELAY:
@@ -272,6 +280,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
         break;
 #endif
 
+    case MAV_CMD_NAV_VTOL_TAKEOFF:
+        return quadplane.verify_vtol_takeoff(cmd);
+
+    case MAV_CMD_NAV_VTOL_LAND:
+        return quadplane.verify_vtol_land();
+        
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:

ArduPlane/quadplane.cpp

@@ -630,6 +630,7 @@ void QuadPlane::update_transition(void)
             transition_state = TRANSITION_TIMER;
             GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", aspeed);
         }
+        assisted_flight = true;
         hold_hover(assist_climb_rate_cms());
         attitude_control->rate_controller_run();
         motors->output();
@@ -825,5 +826,90 @@ void QuadPlane::control_auto(const Location &loc)
     plane.nav_roll_cd = wp_nav->get_roll();
     plane.nav_pitch_cd = wp_nav->get_pitch();
 
+    switch (plane.mission.get_current_nav_cmd().id) {
+    case MAV_CMD_NAV_VTOL_LAND:
+        pos_control->set_alt_target_from_climb_rate_ff(-50, plane.G_Dt, true);
+        break;
+    case MAV_CMD_NAV_VTOL_TAKEOFF:
+        pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t());
+        break;
+    default:
+        pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false);
+        break;
+    }
+    
     pos_control->update_z_controller();
 }
+
+/*
+  start a VTOL takeoff
+ */
+bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
+{
+    if (!available()) {
+        return false;
+    }
+    plane.prev_WP_loc = plane.current_loc;
+    plane.next_WP_loc = cmd.content.location;
+    plane.next_WP_loc.alt = plane.current_loc.alt + cmd.content.location.alt;
+    plane.auto_state.vtol_mode = true;
+    throttle_wait = false;
+
+    // also update nav_controller for status output
+    plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
+    return true;
+}
+
+
+/*
+  start a VTOL landing
+ */
+bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
+{
+    if (!available()) {
+        return false;
+    }
+    plane.prev_WP_loc = plane.current_loc;
+    plane.next_WP_loc = cmd.content.location;
+    plane.auto_state.vtol_mode = true;
+    throttle_wait = false;    
+    land_complete = false;
+
+    // also update nav_controller for status output
+    plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
+    return true;
+}
+
+/*
+  check if a VTOL takeoff has completed
+ */
+bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
+{
+    if (plane.auto_state.wp_distance > 2) {
+        return false;
+    }
+    const int32_t threshold = 30;
+    if (plane.current_loc.alt+threshold < plane.next_WP_loc.alt) {
+        return false;
+    }
+    plane.auto_state.vtol_mode = false;
+    transition_state = TRANSITION_AIRSPEED_WAIT;
+    return true;
+}
+
+/*
+  check if a VTOL landing has completed
+ */
+bool QuadPlane::verify_vtol_land(void)
+{
+    if (!should_relax()) {
+        return false;
+    }
+    wp_nav->loiter_soften_for_landing();
+    if (millis() - motors_lower_limit_start_ms > 5000) {
+        plane.disarm_motors();
+        land_complete = true;
+        plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
+    }
+    return false;
+}

ArduPlane/quadplane.h

@@ -42,6 +42,11 @@ public:
     
     bool handle_do_vtol_transition(const mavlink_command_long_t &packet);
 
+    bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
+    bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
+    bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
+    bool verify_vtol_land(void);
+    
 private:
     AP_AHRS_NavEKF &ahrs;
     AP_Vehicle::MultiCopter aparm;
@@ -148,4 +153,6 @@ private:
 
     // time we last set the loiter target
     uint32_t last_loiter_ms;
+
+    bool land_complete:1;
 };