Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: Add parameters from Geofence to AC_Fence

Browse Source 
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 2020-09-10 16:28:09 +10:00 committed by Peter Barker
parent 17fb585bf1
commit 87b66b4b49
3 changed files with 209 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
libraries/AC_Fence/AC_Fence.cpp
View File

@ -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;
}
@ -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()
{

46
libraries/AC_Fence/AC_Fence.h
View File

@ -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,16 +186,21 @@ 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;

24
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -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;