AP_Landing + Plane: Check for land complete before requesting disarm

ArduPlane/ArduPlane.cpp

@@ -549,13 +549,11 @@ void Plane::handle_auto_mode(void)
         calc_nav_roll();
         calc_nav_pitch();
         
-        if (landing.is_complete()) {
-            // during final approach constrain roll to the range
-            // allowed for level flight
-            nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
+        // allow landing to restrict the roll limits
+        nav_roll_cd = landing.constrain_roll(nav_roll_cd, g.level_roll_limit*100UL);
 
-            // we are in the final stage of a landing - force
-            // zero throttle
+        if (landing.is_complete()) {
+            // if landing is considered complete throttle is never allowed, regardless of landing type
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
         } else {
             calc_throttle();
@@ -1015,11 +1013,12 @@ void Plane::update_optical_flow(void)
 
 /*
     If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
+
+    only called from AP_Landing, when the landing library is ready to disarm
  */
 void Plane::disarm_if_autoland_complete()
 {
     if (landing.get_disarm_delay() > 0 &&
-        landing.is_complete() &&
         !is_flying() &&
         arming.arming_required() != AP_Arming::NO &&
         arming.is_armed()) {

libraries/AP_Landing/AP_Landing.cpp

@@ -336,6 +336,16 @@ bool AP_Landing::restart_landing_sequence()
     return success;
 }
 
+int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
+{
+    switch (type) {
+    case TYPE_STANDARD_GLIDE_SLOPE:
+        return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd);
+    default:
+        return desired_roll_cd;
+    }
+}
+
 /*
  * Determine how aligned heading_deg is with the wind. Return result
  * is 1.0 when perfectly aligned heading into wind, -1 when perfectly

libraries/AP_Landing/AP_Landing.h

@@ -79,6 +79,7 @@ public:
     bool is_flaring(void) const;
     bool is_on_approach(void) const;
     void handle_flight_stage_change(const bool _in_landing_stage);
+    int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
 
     // helper functions
     bool restart_landing_sequence(void);
@@ -173,6 +174,7 @@ private:
     void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm);
     int32_t type_slope_get_target_airspeed_cm(void);
     void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
+    int32_t type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
     bool type_slope_request_go_around(void);
     bool type_slope_is_flaring(void) const;
     bool type_slope_is_on_approach(void) const;

libraries/AP_Landing/AP_Landing_Slope.cpp

@@ -145,7 +145,9 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
     }
 
     // check if we should auto-disarm after a confirmed landing
-    disarm_if_autoland_complete_fn();
+    if (complete) {
+        disarm_if_autoland_complete_fn();
+    }
 
     /*
       we return false as a landing mission item never completes
@@ -344,6 +346,14 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) {
     return constrain_int32(target_airspeed_cm + head_wind_compensation_cm, target_airspeed_cm, aparm.airspeed_cruise_cm);
 }
 
+int32_t AP_Landing::type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd) {
+    if (type_slope_stage == SLOPE_STAGE_FINAL) {
+        return constrain_int32(desired_roll_cd, level_roll_limit_cd * -1, level_roll_limit_cd);
+    } else {
+        return desired_roll_cd;
+    }
+}
+
 bool AP_Landing::type_slope_is_flaring(void) const
 {
     return (type_slope_stage == SLOPE_STAGE_FINAL);