Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: add support for warnings at fence margins

Browse Source
This commit is contained in:
Andy Piper 2024-12-11 09:39:48 +00:00
parent 5c9003dd02
commit ddc7bc92aa
2 changed files with 81 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AC_Fence/AC_Fence.cpp
View File

@ -142,7 +142,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
// @Param{Plane, Copter}: OPTIONS
// @DisplayName: Fence options
// @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas.
// @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas
// @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas, 2:Notify on margin breaches
// @User: Standard
AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI),
@ -556,6 +556,10 @@ bool AC_Fence::check_fence_alt_max()
}
// old breach
return false;
} else if (_curr_alt >= _alt_max - _margin) {
record_margin_breach(AC_FENCE_TYPE_ALT_MAX);
} else {
clear_margin_breach(AC_FENCE_TYPE_ALT_MAX);
}
// not breached
@ -605,6 +609,10 @@ bool AC_Fence::check_fence_alt_min()
}
// old breach
return false;
} else if (_curr_alt <= _alt_min + _margin) {
record_margin_breach(AC_FENCE_TYPE_ALT_MIN);
} else {
clear_margin_breach(AC_FENCE_TYPE_ALT_MIN);
}
// not breached
@ -665,7 +673,24 @@ bool AC_Fence::check_fence_polygon()
return true;
}
return false;
} else if (option_enabled(OPTIONS::MARGIN_BREACH)) { // check is relatively expensive
Location loc;
if (AP::ahrs().get_location(loc)) {
Location l1 = loc, l2 = loc, l3 = loc, l4 = loc;
// quite approximate
l1.offset(_margin, _margin);
l2.offset(_margin, -_margin);
l3.offset(-_margin, _margin);
l4.offset(-_margin, -_margin);
if (_poly_loader.breached(l1) || _poly_loader.breached(l2)
|| _poly_loader.breached(l3) || _poly_loader.breached(l4)) {
record_margin_breach(AC_FENCE_TYPE_POLYGON);
} else {
clear_margin_breach(AC_FENCE_TYPE_POLYGON);
}
}
}
if (was_breached) {
clear_breach(AC_FENCE_TYPE_POLYGON);
}
@ -705,6 +730,10 @@ bool AC_Fence::check_fence_circle()
return true;
}
return false;
} else if (_home_distance >= _circle_radius - _margin) {
record_margin_breach(AC_FENCE_TYPE_CIRCLE);
} else {
clear_margin_breach(AC_FENCE_TYPE_CIRCLE);
}
// not currently breached
@ -876,12 +905,48 @@ void AC_Fence::record_breach(uint8_t fence_type)
_breached_fences |= fence_type;
}
/// clear_breach - update breach bitmask, time and count
/// record_margin_breach - update margin_breach bitmask, time and count
void AC_Fence::record_margin_breach(uint8_t fence_type)
{
// if we haven't already breached a margin limit, update the breach time
if (!(_breached_fence_margins & fence_type)) {
const uint32_t now = AP_HAL::millis();
// emit a message indicated we're newly-breached, but not too often
if (option_enabled(OPTIONS::MARGIN_BREACH)
&& AP_HAL::timeout_expired(_last_margin_breach_notify_sent_ms, now, 1000U)) {
_last_margin_breach_notify_sent_ms = now;
print_fence_message("outside margin", _breached_fence_margins | fence_type);
}
}
// update bitmask
_breached_fence_margins |= fence_type;
}
/// clear_breach - update breach bitmask
void AC_Fence::clear_breach(uint8_t fence_type)
{
_breached_fences &= ~fence_type;
}
/// clear_margin_breach - update margin reach bitmask
void AC_Fence::clear_margin_breach(uint8_t fence_type)
{
if (_breached_fence_margins & fence_type) {
const uint32_t now = AP_HAL::millis();
// emit a message indicated we're newly-breached, but not too often
if (option_enabled(OPTIONS::MARGIN_BREACH)
&& AP_HAL::timeout_expired(_last_margin_breach_notify_sent_ms, now, 1000U)) {
_last_margin_breach_notify_sent_ms = now;
print_fence_message("inside margin", fence_type);
}
}
_breached_fence_margins &= ~fence_type;
}
/// get_breach_distance - returns maximum distance in meters outside
/// of the given fences. fence_type is a bitmask here.
float AC_Fence::get_breach_distance(uint8_t fence_type) const

16
libraries/AC_Fence/AC_Fence.h
View File

@ -130,6 +130,9 @@ public:
/// get_breach_count - returns number of times we have breached the fence
uint16_t get_breach_count() const { return _breach_count; }
/// get_breaches - returns bitmask of the fence types that have had their margins breached
uint8_t get_margin_breaches() const { return _breached_fence_margins; }
/// get_breach_distance - returns maximum distance in meters outside
/// of the given fences. fence_type is a bitmask here.
float get_breach_distance(uint8_t fence_type) const;
@ -177,6 +180,7 @@ public:
enum class OPTIONS {
DISABLE_MODE_CHANGE = 1U << 0,
INCLUSION_UNION = 1U << 1,
MARGIN_BREACH = 1U << 2,
};
static bool option_enabled(OPTIONS opt, const AP_Int16 &options) {
return (options.get() & int16_t(opt)) != 0;
@ -211,9 +215,15 @@ private:
/// record_breach - update breach bitmask, time and count
void record_breach(uint8_t fence_type);
/// clear_breach - update breach bitmask, time and count
/// clear_breach - update breach bitmask
void clear_breach(uint8_t fence_type);
/// record_margin_breach - update margin breach bitmask
void record_margin_breach(uint8_t fence_type);
/// clear_margin_breach - update margin breach bitmask
void clear_margin_breach(uint8_t fence_type);
// additional checks for the different fence types:
bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const;
bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const;
@ -252,10 +262,12 @@ private:
float _home_distance; // distance from home in meters (provided by main code)
// breach information
uint8_t _breached_fences; // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
uint8_t _breached_fences; // bitmask holding the fence types that were breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
uint8_t _breached_fence_margins; // bitmask holding the fence types that have margin breaches (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
uint32_t _breach_time; // time of last breach in milliseconds
uint16_t _breach_count; // number of times we have breached the fence
uint32_t _last_breach_notify_sent_ms; // last time we sent a message about newly-breaching the fences
uint32_t _last_margin_breach_notify_sent_ms; // last time we sent a message about newly-breaching the fences
uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control