Plane: added support for NAV_PACKAGE_PLACE mission item

this works in conjunction with a lua script to allow for a wide
variety of package place options

ArduPlane/AP_Arming.cpp

@@ -413,7 +413,7 @@ bool AP_Arming_Plane::mission_checks(bool report)
             if (!plane.mission.read_cmd_from_storage(i, cmd)) {
                 break;
             }
-            if ((cmd.id == MAV_CMD_NAV_VTOL_LAND || cmd.id == MAV_CMD_NAV_LAND) &&
+            if (plane.is_land_command(cmd.id) &&
                 prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
                 const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
                 const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();

ArduPlane/ArduPlane.cpp

@@ -801,6 +801,19 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
     return false;
 }
 
+// allow for override of land descent rate
+bool Plane::set_land_descent_rate(float descent_rate)
+{
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.in_vtol_land_descent()) {
+        quadplane.poscontrol.override_descent_rate = descent_rate;
+        quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
+        return true;
+    }
+#endif
+    return false;
+}
+
 #endif // AP_SCRIPTING_ENABLED
 
 // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL

ArduPlane/GCS_Mavlink.cpp

@@ -1039,8 +1039,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
         {
             uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
             bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) ||
-                                 (mission_id == MAV_CMD_NAV_LAND) ||
+                plane.is_land_command(mission_id);
-                                 (mission_id == MAV_CMD_NAV_VTOL_LAND);
             if (is_in_landing) {
                 // fly a user planned abort pattern if available
                 if (plane.mission.jump_to_abort_landing_sequence()) {

ArduPlane/Plane.h

@@ -938,6 +938,13 @@ private:
     void do_nav_delay(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
 
+    bool is_land_command(uint16_t cmd) const;
+
+    /*
+      return true if in a specific AUTO mission command
+    */
+    bool in_auto_mission_id(uint16_t command) const;
+    
     // Delay the next navigation command
     struct {
         uint32_t time_max_ms;
@@ -956,12 +963,6 @@ private:
     // set home location and store it persistently:
     bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
 
-    // quadplane.cpp
-#if HAL_QUADPLANE_ENABLED
-    bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
-    bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
-#endif
-
     // control_modes.cpp
     void read_control_switch();
     uint8_t readSwitch(void) const;
@@ -1229,6 +1230,9 @@ public:
     bool get_target_location(Location& target_loc) override;
     bool update_target_location(const Location &old_loc, const Location &new_loc) override;
     bool set_velocity_match(const Vector2f &velocity) override;
+
+    // allow for landing descent rate to be overridden by a script, may be -ve to climb
+    bool set_land_descent_rate(float descent_rate) override;
 #endif // AP_SCRIPTING_ENABLED
 
 };

ArduPlane/commands_logic.cpp

@@ -98,6 +98,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         return quadplane.do_vtol_takeoff(cmd);
 
     case MAV_CMD_NAV_VTOL_LAND:
+    case MAV_CMD_NAV_PAYLOAD_PLACE:
         if (quadplane.landing_with_fixed_wing_spiral_approach()) {
             // the user wants to approach the landing in a fixed wing flight mode
             // the waypoint will be used as a loiter_to_alt
@@ -243,7 +244,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_NAV_LAND:
 #if HAL_QUADPLANE_ENABLED
         if (quadplane.is_vtol_land(cmd.id)) {
-            return quadplane.verify_vtol_land();            
+            return quadplane.verify_vtol_land();
         }
 #endif
         if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
@@ -286,6 +287,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_NAV_VTOL_TAKEOFF:
         return quadplane.verify_vtol_takeoff(cmd);
     case MAV_CMD_NAV_VTOL_LAND:
+    case MAV_CMD_NAV_PAYLOAD_PLACE:
         if (quadplane.landing_with_fixed_wing_spiral_approach() && !verify_landing_vtol_approach(cmd)) {
             // verify_landing_vtol_approach will return true once we have completed the approach,
             // in which case we fall over to normal vtol landing code
@@ -1276,3 +1278,24 @@ bool Plane::nav_scripting_enable(uint8_t mode)
    return nav_scripting.enabled;
 }
 #endif // AP_SCRIPTING_ENABLED
+
+/*
+  return true if this is a LAND command
+  note that we consider a PAYLOAD_PLACE to be a land command as it
+  follows the landing logic for quadplanes
+ */
+bool Plane::is_land_command(uint16_t command) const
+{
+    return
+        command == MAV_CMD_NAV_VTOL_LAND ||
+        command == MAV_CMD_NAV_LAND ||
+        command == MAV_CMD_NAV_PAYLOAD_PLACE;
+}
+
+/*
+  return true if in a specific AUTO mission command
+ */
+bool Plane::in_auto_mission_id(uint16_t command) const
+{
+    return control_mode == &mode_auto && mission.get_current_nav_id() == command;
+}

ArduPlane/quadplane.cpp

@@ -1209,6 +1209,13 @@ bool QuadPlane::is_flying_vtol(void) const
  */
 float QuadPlane::landing_descent_rate_cms(float height_above_ground)
 {
+    if (poscontrol.last_override_descent_ms != 0) {
+        const uint32_t now = AP_HAL::millis();
+        if (now - poscontrol.last_override_descent_ms < 1000) {
+            return poscontrol.override_descent_rate*100;
+        }
+    }
+
     if (poscontrol.get_state() == QPOS_LAND_FINAL) {
         // when in final use descent rate for final even if alt has climbed again
         height_above_ground = MIN(height_above_ground, land_final_alt);
@@ -2099,6 +2106,7 @@ bool QuadPlane::in_vtol_auto(void) const
         return is_vtol_takeoff(id);
     case MAV_CMD_NAV_VTOL_LAND:
     case MAV_CMD_NAV_LAND:
+    case MAV_CMD_NAV_PAYLOAD_PLACE:
         return is_vtol_land(id);
     default:
         return false;
@@ -2285,6 +2293,11 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
         } else if (s == QPOS_LAND_DESCEND) {
             // reset throttle descent control
             qp.thr_ctrl_land = false;
+            qp.land_descend_start_alt = plane.current_loc.alt*0.01;
+            last_override_descent_ms = 0;
+        } else if (s == QPOS_LAND_ABORT) {
+            // reset throttle descent control
+            qp.thr_ctrl_land = false;
         } else if (s == QPOS_LAND_FINAL) {
             // remember last pos reset to handle GPS glitch in LAND_FINAL
             Vector2f rpos;
@@ -2674,6 +2687,7 @@ void QuadPlane::vtol_position_controller(void)
     }
 
     case QPOS_POSITION2:
+    case QPOS_LAND_ABORT:
     case QPOS_LAND_DESCEND: {
         setup_target_position();
         /*
@@ -2825,6 +2839,7 @@ void QuadPlane::vtol_position_controller(void)
     }
 
     case QPOS_LAND_DESCEND:
+    case QPOS_LAND_ABORT:
     case QPOS_LAND_FINAL: {
         float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
         if (poscontrol.get_state() == QPOS_LAND_FINAL) {
@@ -2832,6 +2847,10 @@ void QuadPlane::vtol_position_controller(void)
                 ahrs.set_touchdown_expected(true);
             }
         }
+        if (poscontrol.get_state() == QPOS_LAND_ABORT) {
+            set_climb_rate_cms(wp_nav->get_default_speed_up());
+            break;
+        }
         const float descent_rate_cms = landing_descent_rate_cms(height_above_ground);
         pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
         break;
@@ -3044,7 +3063,21 @@ void QuadPlane::control_auto(void)
     }
 
     if (poscontrol.get_state() > QPOS_APPROACH) {
-        if (!plane.arming.get_delay_arming()) {
+        bool should_run_motors = false;
+
+        // don't run the motors if in an arming delay
+        if (plane.arming.get_delay_arming()) {
+            should_run_motors = false;
+        }
+
+        // don't run motors if we are in the wait state for payload place
+        if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::SHUT_DOWN &&
+            plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
+            poscontrol.get_state() == QPOS_LAND_COMPLETE) {
+            should_run_motors = false;
+        }
+        
+        if (should_run_motors) {
             set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
         }
     }
@@ -3058,6 +3091,7 @@ void QuadPlane::control_auto(void)
         }
         break;
     case MAV_CMD_NAV_VTOL_LAND:
+    case MAV_CMD_NAV_PAYLOAD_PLACE:
     case MAV_CMD_NAV_LAND:
         if (is_vtol_land(id)) {
             vtol_position_controller();
@@ -3283,6 +3317,15 @@ bool QuadPlane::check_land_complete(void)
     if (land_detector(4000)) {
         poscontrol.set_state(QPOS_LAND_COMPLETE);
         gcs().send_text(MAV_SEVERITY_INFO,"Land complete");
+
+        if (plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE)) {
+            // for payload place with full landing we shutdown motors
+            // and wait for the lua script to trigger a climb (using
+            // landing abort) or disarm
+            set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
+            return false;
+        }
+
         if (plane.control_mode != &plane.mode_auto ||
             !plane.mission.continue_after_land()) {
             // disarm on land unless we have MIS_OPTIONS setup to
@@ -3384,6 +3427,23 @@ bool QuadPlane::verify_vtol_land(void)
         gcs().send_text(MAV_SEVERITY_INFO,"Land final started");
     }
 
+    // at land_final_alt begin final landing
+    if (poscontrol.get_state() == QPOS_LAND_ABORT &&
+        plane.current_loc.alt*0.01 >= land_descend_start_alt) {
+        // continue to next WP, if there is one
+        return true;
+    }
+
+    if (plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
+        (poscontrol.get_state() == QPOS_LAND_DESCEND ||
+         poscontrol.get_state() == QPOS_LAND_FINAL)) {
+        const auto &cmd = plane.mission.get_current_nav_cmd();
+        if (cmd.p1 > 0 && plane.current_loc.alt*0.01 < land_descend_start_alt - cmd.p1*0.01) {
+            gcs().send_text(MAV_SEVERITY_INFO,"Payload place aborted");
+            poscontrol.set_state(QPOS_LAND_ABORT);
+        }
+    }
+    
     if (check_land_complete() && plane.mission.continue_after_land()) {
         gcs().send_text(MAV_SEVERITY_INFO,"Mission continue");
         return true;
@@ -3730,7 +3790,7 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const
 */
 bool QuadPlane::is_vtol_land(uint16_t id) const
 {
-    if (id == MAV_CMD_NAV_VTOL_LAND) {
+    if (id == MAV_CMD_NAV_VTOL_LAND || id == MAV_CMD_NAV_PAYLOAD_PLACE) {
         if (landing_with_fixed_wing_spiral_approach()) {
             return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING;
         } else {
@@ -3879,12 +3939,13 @@ bool QuadPlane::in_vtol_land_approach(void) const
  */
 bool QuadPlane::in_vtol_land_descent(void) const
 {
+    const auto state = poscontrol.get_state();
     if (plane.control_mode == &plane.mode_qrtl &&
-        (poscontrol.get_state() == QPOS_LAND_DESCEND || poscontrol.get_state() == QPOS_LAND_FINAL)) {
+        (state == QPOS_LAND_DESCEND || state == QPOS_LAND_FINAL || state == QPOS_LAND_ABORT)) {
         return true;
     }
     if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
-        (poscontrol.get_state() == QPOS_LAND_DESCEND || poscontrol.get_state() == QPOS_LAND_FINAL)) {
+        (state == QPOS_LAND_DESCEND || state == QPOS_LAND_FINAL || state == QPOS_LAND_ABORT)) {
         return true;
     }
     return false;
@@ -4367,6 +4428,11 @@ bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const
 {
     const AP_Mission::Mission_Command cmd = plane.mission.get_current_nav_cmd();
 
+    if (cmd.id == MAV_CMD_NAV_PAYLOAD_PLACE &&
+        option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) {
+        return true;
+    }
+    
     return ((cmd.id == MAV_CMD_NAV_VTOL_LAND) &&
             (option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) ||
              cmd.p1 == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH));
@@ -4413,4 +4479,27 @@ void QuadPlane::setup_rp_fw_angle_gains(void)
     attitude_control->set_angle_P_scale(gain_scale);
 }
 
+/*
+  abort landing, used by scripting for payload place and ship landing abort
+  will return false if not in a landing descent
+ */
+bool QuadPlane::abort_landing(void)
+{
+    if (poscontrol.get_state() == QPOS_LAND_ABORT) {
+        // already aborted?
+        return false;
+    }
+
+    // special case for payload place with full landing
+    const bool payload_place_landed =
+        plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
+        poscontrol.get_state() == QPOS_LAND_COMPLETE;
+
+    if (!payload_place_landed && !in_vtol_land_descent()) {
+        return false;
+    }
+    poscontrol.set_state(QuadPlane::QPOS_LAND_ABORT);
+    return true;
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -97,6 +97,9 @@ public:
         return available() && assisted_flight;
     }
 
+    // abort landing, only valid when in a VTOL landing descent
+    bool abort_landing(void);
+
     /*
       return true if we are in a transition to fwd flight from hover
     */
@@ -172,6 +175,11 @@ public:
     // Check if servo auto trim is allowed
     bool allow_servo_auto_trim();
 
+    /*
+      are we in the descent phase of a VTOL landing?
+     */
+    bool in_vtol_land_descent(void) const;
+
 private:
     AP_AHRS &ahrs;
 
@@ -461,6 +469,7 @@ private:
         QPOS_POSITION1,
         QPOS_POSITION2,
         QPOS_LAND_DESCEND,
+        QPOS_LAND_ABORT,
         QPOS_LAND_FINAL,
         QPOS_LAND_COMPLETE
     };
@@ -491,6 +500,9 @@ private:
         float target_accel;
         uint32_t last_pos_reset_ms;
         bool overshoot;
+
+        float override_descent_rate;
+        uint32_t last_override_descent_ms;
     private:
         uint32_t last_state_change_ms;
         enum position_control_state state;
@@ -574,6 +586,9 @@ private:
 
     float last_land_final_agl;
 
+    // AHRS alt for land abort and package place, meters
+    float land_descend_start_alt;
+
     // min alt for navigation in takeoff
     AP_Float takeoff_navalt_min;
     uint32_t takeoff_last_run_ms;
@@ -603,11 +618,6 @@ private:
      */
     bool in_vtol_land_approach(void) const;
 
-    /*
-      are we in the descent phase of a VTOL landing?
-     */
-    bool in_vtol_land_descent(void) const;
-
     /*
       are we in the final landing phase of a VTOL landing?
      */