diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index fc87bb4ad6..7c57daf815 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -434,10 +434,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) // if we were in an abort we need to explicitly move out of the abort state, as it's sticky set_flight_stage(AP_FixedWing::FlightStage::LAND); } - -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif } #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 9615082244..0217cbebe4 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -9,8 +9,16 @@ void Plane::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + uint16_t mission_id = plane.mission.get_current_nav_cmd().id; + bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND +#if HAL_QUADPLANE_ENABLED + || control_mode->mode_number() == Mode::Number::QLAND + || control_mode->mode_number() == Mode::Number::QRTL +#endif + || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); + // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(is_in_landing); if (!fence.enabled()) { // Switch back to the chosen control mode if still in diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index e6381b62df..3153d7de31 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -54,10 +54,6 @@ void ModeAuto::_exit() } #endif if (restart) { -#if AP_FENCE_ENABLED - // no longer landing to re-enable the possiblity of the fence floor - plane.fence.reset_fence_floor_enable(); -#endif plane.landing.restart_landing_sequence(); } } diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 4205960f87..f3a08aabde 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -16,9 +16,6 @@ bool ModeQLand::_enter() #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif return true; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 05a7948457..c0c69937fa 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3554,9 +3554,6 @@ bool QuadPlane::verify_vtol_land(void) poscontrol.pilot_correction_done = false; pos_control->set_lean_angle_max_cd(0); poscontrol.xy_correction.zero(); -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif