diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 194400c3e7..3f7abb31a6 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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()) { diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index a48f40c96d..c098148796 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/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 diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 7bed3654ff..f9de245c23 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/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 ¤t_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; diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 051df28c29..8f10103b18 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/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);