diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 9728ad1279..7616c5c658 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -33,18 +33,19 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Description: What action should be taken when fence is breached // @Values{Copter}: 0:Report Only,1:RTL or Land,2:Always Land,3:SmartRTL or RTL or Land,4:Brake or Land,5:SmartRTL or Land // @Values{Rover}: 0:Report Only,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold + // @Values{Plane}: 0:Report Only,1:RTL,6:Guided,7:GuidedThrottlePass // @Values: 0:Report Only,1:RTL or Land // @User: Standard AP_GROUPINFO("ACTION", 2, AC_Fence, _action, AC_FENCE_ACTION_RTL_AND_LAND), - // @Param{Copter, Sub}: ALT_MAX + // @Param{Copter, Plane, Sub}: ALT_MAX // @DisplayName: Fence Maximum Altitude // @Description: Maximum altitude allowed before geofence triggers // @Units: m // @Range: 10 1000 // @Increment: 1 // @User: Standard - AP_GROUPINFO_FRAME("ALT_MAX", 3, AC_Fence, _alt_max, AC_FENCE_ALT_MAX_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), + AP_GROUPINFO_FRAME("ALT_MAX", 3, AC_Fence, _alt_max, AC_FENCE_ALT_MAX_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE), // @Param: RADIUS // @DisplayName: Circular Fence Radius @@ -69,14 +70,41 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO("TOTAL", 6, AC_Fence, _total, 0), - // @Param: ALT_MIN + // @Param{Copter, Plane, Sub}: ALT_MIN // @DisplayName: Fence Minimum Altitude // @Description: Minimum altitude allowed before geofence triggers // @Units: m // @Range: -100 100 // @Increment: 1 // @User: Standard - AP_GROUPINFO_FRAME("ALT_MIN", 7, AC_Fence, _alt_min, AC_FENCE_ALT_MIN_DEFAULT, AP_PARAM_FRAME_SUB), + AP_GROUPINFO_FRAME("ALT_MIN", 7, AC_Fence, _alt_min, AC_FENCE_ALT_MIN_DEFAULT, AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_PLANE), + + // @Param{Plane}: RET_RALLY + // @DisplayName: Fence Return to Rally + // @Description: Should the vehichle return to fence return point or rally point + // @Values: 0:Fence Return Point,1:Nearest Rally Point + // @Range: 0 1 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO_FRAME("RET_RALLY", 8, AC_Fence, _ret_rally, 0, AP_PARAM_FRAME_PLANE), + + // @Param{Plane}: RET_ALT + // @DisplayName: Fence Return Altitude + // @Description: Altitude the vehicle will transit to when a fence breach occurs + // @Units: m + // @Range: 0 32767 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO_FRAME("RET_ALT", 9, AC_Fence, _ret_altitude, 0.0f, AP_PARAM_FRAME_PLANE), + + // @Param: AUTOENABLE + // @DisplayName: Fence Auto-Enable + // @Description: Auto-enable of fence + // @Values{Plane}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed + // @Range: 0 3 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED)), AP_GROUPEND }; @@ -103,14 +131,45 @@ void AC_Fence::enable(bool value) } _enabled = value; if (!value) { - clear_breach(AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON); + clear_breach(AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON); + } else { + enable_floor(); } } +/// enable/disable fence floor only +void AC_Fence::enable_floor() +{ + if (!_floor_enabled) { + // Floor is currently disabled, enable it + AP::logger().Write_Event(LogEvent::FENCE_FLOOR_ENABLE); + } + _floor_enabled = true; +} + +void AC_Fence::disable_floor() +{ + if (_floor_enabled) { + // Floor is currently enabled, disable it + AP::logger().Write_Event(LogEvent::FENCE_FLOOR_DISABLE); + } + _floor_enabled = true; + clear_breach(AC_FENCE_TYPE_ALT_MIN); +} + +bool AC_Fence::present() const +{ + // A fence is present if it is enabled and there are points present + if (_poly_loader.total_fence_count() == 0) { + return false; + } + return true; +} + /// get_enabled_fences - returns bitmask of enabled fences uint8_t AC_Fence::get_enabled_fences() const { - if (!_enabled) { + if (!_enabled && !_auto_enabled) { return 0; } return _enabled_fences; @@ -159,6 +218,11 @@ bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const fail_msg = "Invalid FENCE_ALT_MAX value"; return false; } + + if (_alt_min < -100.0f) { + fail_msg = "Invalid FENCE_ALT_MIN value"; + return false; + } return true; } @@ -169,7 +233,7 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const fail_msg = nullptr; // if not enabled or not fence set-up always return true - if (!_enabled || !_enabled_fences) { + if ((!_enabled && !_auto_enabled) || !_enabled_fences) { return true; } @@ -221,7 +285,7 @@ bool AC_Fence::check_fence_alt_max() _curr_alt = -_curr_alt; // translate Down to Up // check if we are over the altitude fence - if(_curr_alt >= _alt_max) { + if (_curr_alt >= _alt_max) { // record distance above breach _alt_max_breach_distance = _curr_alt - _alt_max; @@ -254,6 +318,54 @@ bool AC_Fence::check_fence_alt_max() return false; } +/// returns true if we have freshly breached the minimum altitude +/// fence; also may set up a fallback fence which, if breached, will +/// cause the altitude fence to be freshly breached +bool AC_Fence::check_fence_alt_min() +{ + // altitude fence check + if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MIN)) { + // not enabled + return false; + } + + AP::ahrs().get_relative_position_D_home(_curr_alt); + _curr_alt = -_curr_alt; // translate Down to Up + + // check if we are under the altitude fence + if (_curr_alt < _alt_min) { + + // record distance below breach + _alt_min_breach_distance = _alt_min - _curr_alt; + + // check for a new breach or a breach of the backup fence + if (!(_breached_fences & AC_FENCE_TYPE_ALT_MIN) || + (!is_zero(_alt_min_backup) && _curr_alt < _alt_min_backup)) { + + // new breach + record_breach(AC_FENCE_TYPE_ALT_MIN); + + // create a backup fence 20m lower down + _alt_min_backup = _curr_alt - AC_FENCE_ALT_MIN_BACKUP_DISTANCE; + // new breach + return true; + } + // old breach + return false; + } + + // not breached + + // clear alt breach if present + if ((_breached_fences & AC_FENCE_TYPE_ALT_MIN) != 0) { + clear_breach(AC_FENCE_TYPE_ALT_MIN); + _alt_min_backup = 0.0f; + _alt_min_breach_distance = 0.0f; + } + + return false; +} + // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones @@ -329,7 +441,7 @@ uint8_t AC_Fence::check() uint8_t ret = 0; // return immediately if disabled - if (!_enabled || !_enabled_fences) { + if ((!_enabled && !_auto_enabled) || !_enabled_fences) { return 0; } @@ -352,6 +464,11 @@ uint8_t AC_Fence::check() ret |= AC_FENCE_TYPE_ALT_MAX; } + // minimum altitude fence check + if (_floor_enabled && check_fence_alt_min()) { + ret |= AC_FENCE_TYPE_ALT_MIN; + } + // circle fence check if (check_fence_circle()) { ret |= AC_FENCE_TYPE_CIRCLE; @@ -369,7 +486,7 @@ uint8_t AC_Fence::check() // returns true if the destination is within fence (used to reject waypoints outside the fence) bool AC_Fence::check_destination_within_fence(const Location& loc) { - // Altitude fence check + // Altitude fence check - Fence Ceiling if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) { int32_t alt_above_home_cm; if (loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm)) { @@ -379,6 +496,16 @@ bool AC_Fence::check_destination_within_fence(const Location& loc) } } + // Altitude fence check - Fence Floor + if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) { + int32_t alt_above_home_cm; + if (loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm)) { + if ((alt_above_home_cm * 0.01f) < _alt_min) { + return false; + } + } + } + // Circular fence check if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) { if (AP::ahrs().get_home().get_distance(loc) > _circle_radius) { @@ -431,9 +558,13 @@ void AC_Fence::clear_breach(uint8_t fence_type) float AC_Fence::get_breach_distance(uint8_t fence_type) const { float max = 0.0f; + if (fence_type & AC_FENCE_TYPE_ALT_MAX) { max = MAX(_alt_max_breach_distance, max); } + if (fence_type & AC_FENCE_TYPE_ALT_MIN) { + max = MAX(_alt_max_breach_distance, max); + } if (fence_type & AC_FENCE_TYPE_CIRCLE) { max = MAX(_circle_breach_distance, max); } @@ -456,7 +587,7 @@ void AC_Fence::manual_recovery_start() // methods for mavlink SYS_STATUS message (send_sys_status) bool AC_Fence::sys_status_present() const { - return _enabled; + return present(); } bool AC_Fence::sys_status_enabled() const @@ -482,17 +613,20 @@ bool AC_Fence::sys_status_failed() const return false; } -AC_PolyFence_loader &AC_Fence::polyfence() { +AC_PolyFence_loader &AC_Fence::polyfence() +{ return _poly_loader; } -const AC_PolyFence_loader &AC_Fence::polyfence() const { +const AC_PolyFence_loader &AC_Fence::polyfence() const +{ return _poly_loader; } // singleton instance AC_Fence *AC_Fence::_singleton; -namespace AP { +namespace AP +{ AC_Fence *fence() { diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 4b244395e5..682e0da6d1 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -12,6 +12,7 @@ #define AC_FENCE_TYPE_ALT_MAX 1 // high alt fence which usually initiates an RTL #define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL) #define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence +#define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL // valid actions should a fence be breached #define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action @@ -20,12 +21,15 @@ #define AC_FENCE_ACTION_SMART_RTL 3 // smartRTL, if that fails, RTL, it that still fails, land #define AC_FENCE_ACTION_BRAKE 4 // brake, if that fails, land #define AC_FENCE_ACTION_SMART_RTL_OR_LAND 5 // SmartRTL, if that fails, Land +#define AC_FENCE_ACTION_GUIDED 6 // guided mode, with target waypoint as fence return point +#define AC_FENCE_ACTION_GUIDED_THROTTLE_PASS 7 // guided mode, but pilot retains manual throttle control // default boundaries #define AC_FENCE_ALT_MAX_DEFAULT 100.0f // default max altitude is 100m #define AC_FENCE_ALT_MIN_DEFAULT -10.0f // default maximum depth in meters #define AC_FENCE_CIRCLE_RADIUS_DEFAULT 300.0f // default circular fence radius is 300m #define AC_FENCE_ALT_MAX_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further up +#define AC_FENCE_ALT_MIN_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further down #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further out #define AC_FENCE_MARGIN_DEFAULT 2.0f // default distance in meters that autopilot's should maintain from the fence to avoid a breach @@ -36,6 +40,15 @@ class AC_Fence { public: + + enum class AutoEnable + { + ALWAYS_DISABLED = 0, + ALWAYS_ENABLED = 1, + ENABLE_DISABLE_FLOOR_ONLY = 2, + ONLY_WHEN_ARMED = 3 + }; + AC_Fence(); /* Do not allow copies */ @@ -49,12 +62,24 @@ public: // get singleton instance static AC_Fence *get_singleton() { return _singleton; } - /// enable - allows fence to be enabled/disabled. Note: this does not update the eeprom saved value + /// enable - allows fence to be enabled/disabled. void enable(bool value); + /// auto_enabled - + AutoEnable auto_enabled() { return static_cast(_auto_enabled.get()); } + + /// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value + void enable_floor(); + + /// disable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value + void disable_floor(); + /// enabled - returns true if fence is enabled bool enabled() const { return _enabled; } + /// present - returns true if fence is present + bool present() const; + /// get_enabled_fences - returns bitmask of enabled fences uint8_t get_enabled_fences() const; @@ -104,6 +129,12 @@ public: /// get_margin - returns the fence margin in meters float get_margin() const { return _margin.get(); } + /// get_return_rally - returns whether returning to fence return point or rally point + uint8_t get_return_rally() const { return _ret_rally; } + + /// get_return_rally - returns whether returning to fence return point or rally point + float get_return_altitude() const { return _ret_altitude; } + /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds /// should be called whenever the pilot changes the flight mode /// has no effect if no breaches have occurred @@ -122,9 +153,12 @@ public: private: static AC_Fence *_singleton; - /// check_fence_alt_max - true if alt fence has been newly breached + /// check_fence_alt_max - true if max alt fence has been newly breached bool check_fence_alt_max(); + /// check_fence_alt_min - true if min alt fence has been newly breached + bool check_fence_alt_min(); + /// check_fence_polygon - true if polygon fence has been newly breached bool check_fence_polygon(); @@ -143,7 +177,8 @@ private: bool pre_arm_check_alt(const char* &fail_msg) const; // parameters - AP_Int8 _enabled; // top level enable/disable control + AP_Int8 _enabled; // fence enable/disable control + AP_Int8 _auto_enabled; // top level flag for auto enabling fence AP_Int8 _enabled_fences; // bit mask holding which fences are enabled AP_Int8 _action; // recovery action specified by user AP_Float _alt_max; // altitude upper limit in meters @@ -151,18 +186,23 @@ private: AP_Float _circle_radius; // circle fence radius in meters AP_Float _margin; // distance in meters that autopilot's should maintain from the fence to avoid a breach AP_Int8 _total; // number of polygon points saved in eeprom + AP_Int8 _ret_rally; // return to fence return point or rally point/home + AP_Float _ret_altitude; // return to this altitude // backup fences float _alt_max_backup; // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away + float _alt_min_backup; // backup altitude lower limit in meters used to refire the breach if the vehicle continues to move further away float _circle_radius_backup; // backup circle fence radius in meters used to refire the breach if the vehicle continues to move further away // breach distances float _alt_max_breach_distance; // distance above the altitude max + float _alt_min_breach_distance; // distance below the altitude min float _circle_breach_distance; // distance beyond the circular fence // other internal variables + bool _floor_enabled; // fence floor is enabled float _home_distance; // distance from home in meters (provided by main code) - float _curr_alt; + float _curr_alt; // breach information diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h index 790b196b40..bc12e2499a 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.h +++ b/libraries/AC_Fence/AC_PolyFence_loader.h @@ -161,6 +161,23 @@ public: // call @10Hz to check for fence load being valid void update(); +#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT + // get_return_point - returns latitude/longitude of return point. + // This works with storage - the returned vector is absolute + // lat/lon. + bool get_return_point(Vector2l &ret) WARN_IF_UNUSED; +#endif + + // return total number of fences - polygons and circles + uint16_t total_fence_count() const { + return (get_exclusion_polygon_count() + + get_inclusion_polygon_count() + + get_exclusion_circle_count() + + get_inclusion_circle_count()); + } + + + private: // multi-thread access support HAL_Semaphore _loaded_fence_sem; @@ -373,13 +390,6 @@ private: // methods to write specific types of fencepoint out: bool write_eos_to_storage(uint16_t &offset); -#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT - // get_return_point - returns latitude/longitude of return point. - // This works with storage - the returned vector is absolute - // lat/lon. - bool get_return_point(Vector2l &ret) WARN_IF_UNUSED; -#endif - // _total - reference to FENCE_TOTAL parameter. This is used // solely for compatability with the FENCE_POINT protocol AP_Int8 &_total;