mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
AC_Fence: Add parameters from Geofence to AC_Fence
AC_Fence: Add fence floor breach checks and calculations AC_Fence: Add event logging to enable/disable of fence floor AC_Fence: Adjust sys_status reporting to look at total fence count AC_Fence: Make retrieving of the return point accessible AC_Fence: Check whether fence is enabled or autoenable is set for arming checks Checks whether the fence is currently enabled OR if the fence is intended to be enabled automatically. These checks are used to find out enabled fences, or prearm checks
This commit is contained in:
James O'Shannessy
Peter Barker
parent
17fb585bf1
commit
87b66b4b49
@ -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<uint8_t>(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()
|
||||
{
|
||||
|
||||
@ -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<AutoEnable>(_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
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user