diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fa59b371dc..457b2a8be1 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -746,6 +746,7 @@ private: // fence.cpp #if AC_FENCE == ENABLED void fence_check(); + void autoenable_fence_after_takeoff(void); void disable_fence_for_landing(void); #endif diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 2fc83e5879..ee4914e546 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -83,6 +83,24 @@ void Copter::fence_check() } } +/* + called when an auto-takeoff is complete + */ +void Copter::autoenable_fence_after_takeoff(void) +{ +#if AC_FENCE == ENABLED + switch(fence.auto_enabled()) { + case AC_Fence::AutoEnable::ALWAYS_ENABLED: + case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: + fence.enable(true); + break; + default: + // fence does not auto-enable in other takeoff conditions + break; + } +#endif +} + void Copter::disable_fence_for_landing(void) { switch (fence.auto_enabled()) { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 113553b46f..a1a2bc7a8f 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1517,9 +1517,13 @@ bool ModeAuto::verify_takeoff() // have we reached our target altitude? const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); - // retract the landing gear + // if we have reached our destination if (reached_wp_dest) { + // retract the landing gear copter.landinggear.retract_after_takeoff(); + + // auto-enable the fence after takeoff + copter.autoenable_fence_after_takeoff(); } return reached_wp_dest; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 2e7664a204..2e8501ae65 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -398,6 +398,9 @@ void ModeGuided::takeoff_run() // optionally retract landing gear copter.landinggear.retract_after_takeoff(); + // takeoff complete, enable fence + copter.autoenable_fence_after_takeoff(); + // switch to position control mode but maintain current target const Vector3f target = wp_nav->get_wp_destination(); set_destination(target, false, 0, false, 0, false, wp_nav->origin_and_destination_are_terrain_alt()); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 5c781b20ed..0a0afe053d 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -40,6 +40,9 @@ bool ModeLand::init(bool ignore_checks) // optionally deploy landing gear copter.landinggear.deploy_for_landing(); + // disable the fence on landing + copter.disable_fence_for_landing(); + return true; } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index f0c0b1ca17..8eca656824 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -76,7 +76,6 @@ void Mode::_TakeOff::stop() { _running = false; start_ms = 0; - copter.flightmode->autoenable_floor_fence(); } // returns pilot and takeoff climb rates