From 92122e5133bf0589a69faeb967bacd33d2b7c991 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" <amilcar.lucas@iav.de> Date: Wed, 6 Jan 2021 16:19:23 +0100 Subject: [PATCH] Copter: automatically enable and disable floor fence on automated takeoff and landing --- ArduCopter/Copter.h | 5 ++++- ArduCopter/fence.cpp | 17 +++++++++++++++++ ArduCopter/mode.h | 4 +++- ArduCopter/mode_auto.cpp | 4 ++++ ArduCopter/takeoff.cpp | 17 +++++++++++++++++ 5 files changed, 45 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f330191db0..fa59b371dc 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -383,7 +383,7 @@ private: RCMapper rcmap; - // intertial nav alt when we armed + // inertial nav alt when we armed float arming_altitude_m; // Failsafe @@ -744,7 +744,10 @@ private: #endif // fence.cpp +#if AC_FENCE == ENABLED void fence_check(); + void disable_fence_for_landing(void); +#endif // heli.cpp void heli_init(); diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 049d004fcc..2fc83e5879 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -83,4 +83,21 @@ void Copter::fence_check() } } +void Copter::disable_fence_for_landing(void) +{ + switch (fence.auto_enabled()) { + case AC_Fence::AutoEnable::ALWAYS_ENABLED: + fence.enable(false); + gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)"); + break; + case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: + fence.disable_floor(); + gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); + break; + default: + // fence does not auto-disable in other landing conditions + break; + } +} + #endif diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 0bb5f73757..6dbd597ce9 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -21,7 +21,7 @@ public: RTL = 6, // automatic return to launching point CIRCLE = 7, // automatic circular flight with automatic throttle LAND = 9, // automatic landing with horizontal position control - DRIFT = 11, // semi-automous position, yaw and throttle control + DRIFT = 11, // semi-autonomous position, yaw and throttle control SPORT = 13, // manual earth-frame angular rate control with manual throttle FLIP = 14, // automatically flip the vehicle on the roll axis AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains @@ -115,6 +115,8 @@ protected: // return expected input throttle setting to hover: virtual float throttle_hover() const; + void autoenable_floor_fence(void); + // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport enum AltHoldModeState { AltHold_MotorStopped, diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index a17c18d720..113553b46f 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -250,6 +250,10 @@ void ModeAuto::land_start(const Vector3f& destination) // optionally deploy landing gear copter.landinggear.deploy_for_landing(); + +#if AC_FENCE == ENABLED + copter.disable_fence_for_landing(); +#endif } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 7175c14591..f0c0b1ca17 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -76,6 +76,7 @@ void Mode::_TakeOff::stop() { _running = false; start_ms = 0; + copter.flightmode->autoenable_floor_fence(); } // returns pilot and takeoff climb rates @@ -225,3 +226,19 @@ bool Mode::is_taking_off() const } return takeoff.running(); } + +// called when takeoff is complete +void Mode::autoenable_floor_fence(void) +{ +#if AC_FENCE == ENABLED + switch(copter.fence.auto_enabled()) { + case AC_Fence::AutoEnable::ALWAYS_ENABLED: + case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: + copter.fence.enable(true); + break; + default: + // fence does not auto-enable in other takeoff conditions + break; + } +#endif +}