2015-08-11 03:28:41 -03:00
# include "AC_Fence.h"
2019-02-14 02:40:09 -04:00
2022-07-19 08:32:28 -03:00
# if AP_FENCE_ENABLED
2022-03-04 12:42:09 -04:00
2022-03-04 16:25:54 -04:00
# include <AP_Vehicle/AP_Vehicle_Type.h>
# ifndef AC_FENCE_DUMMY_METHODS_ENABLED
2022-07-25 13:51:32 -03:00
# define AC_FENCE_DUMMY_METHODS_ENABLED (!(APM_BUILD_TYPE(APM_BUILD_Rover) | APM_BUILD_COPTER_OR_HELI | APM_BUILD_TYPE(APM_BUILD_ArduPlane) | APM_BUILD_TYPE(APM_BUILD_ArduSub) | (AP_FENCE_ENABLED == 1)))
2022-03-04 16:25:54 -04:00
# endif
# if !AC_FENCE_DUMMY_METHODS_ENABLED
2019-02-14 02:40:09 -04:00
# include <AP_AHRS/AP_AHRS.h>
# include <AP_HAL/AP_HAL.h>
2020-06-01 23:27:28 -03:00
# include <AP_Logger/AP_Logger.h>
2022-08-14 23:17:34 -03:00
# include <GCS_MAVLink/GCS.h>
2013-04-26 06:47:07 -03:00
extern const AP_HAL : : HAL & hal ;
2020-03-26 21:51:14 -03:00
# if APM_BUILD_TYPE(APM_BUILD_Rover)
2018-10-26 00:29:02 -03:00
# define AC_FENCE_TYPE_DEFAULT AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON
2021-03-03 07:49:47 -04:00
# elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
# define AC_FENCE_TYPE_DEFAULT AC_FENCE_TYPE_POLYGON
2018-10-26 00:29:02 -03:00
# else
# define AC_FENCE_TYPE_DEFAULT AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON
# endif
2022-08-06 17:43:38 -03:00
// 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_MARGIN_DEFAULT 2.0f // default distance in meters that autopilot's should maintain from the fence to avoid a breach
# define AC_FENCE_MANUAL_RECOVERY_TIME_MIN 10000 // pilot has 10seconds to recover during which time the autopilot will not attempt to re-take control
# if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
2023-08-18 15:30:22 -03:00
# define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 100.0 // after fence is broken we recreate the fence 100m further out
2024-06-26 06:09:08 -03:00
# define AC_FENCE_OPTIONS_DEFAULT OPTIONS::DISABLE_MODE_CHANGE
2022-08-06 17:43:38 -03:00
# else
# define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out
2024-06-26 06:09:08 -03:00
# define AC_FENCE_OPTIONS_DEFAULT 0
2022-08-06 17:43:38 -03:00
# endif
2023-12-04 12:58:40 -04:00
//#define AC_FENCE_DEBUG
2022-08-06 17:43:38 -03:00
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AC_Fence : : var_info [ ] = {
2023-12-04 12:58:40 -04:00
2013-04-26 06:47:07 -03:00
// @Param: ENABLE
// @DisplayName: Fence enable/disable
2023-12-04 12:58:40 -04:00
// @Description: Allows you to enable (1) or disable (0) the fence functionality. Fences can still be enabled and disabled via mavlink or an RC option, but these changes are not persisted.
2013-04-26 06:47:07 -03:00
// @Values: 0:Disabled,1:Enabled
// @User: Standard
2021-01-06 08:13:57 -04:00
AP_GROUPINFO ( " ENABLE " , 0 , AC_Fence , _enabled , 0 ) ,
2013-04-26 06:47:07 -03:00
// @Param: TYPE
// @DisplayName: Fence Type
2023-12-04 12:58:40 -04:00
// @Description: Configured fence types held as bitmask. Max altitide, Circle and Polygon fences will be immediately enabled if configured. Min altitude fence will only be enabled once the minimum altitude is reached.
2023-07-22 03:40:57 -03:00
// @Bitmask{Rover}: 1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons
// @Bitmask{Copter, Plane, Sub}: 0:Max altitude,1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons,3:Min altitude
2013-04-26 06:47:07 -03:00
// @User: Standard
2023-12-04 12:58:40 -04:00
AP_GROUPINFO ( " TYPE " , 1 , AC_Fence , _configured_fences , AC_FENCE_TYPE_DEFAULT ) ,
2013-04-26 06:47:07 -03:00
// @Param: ACTION
2013-11-26 09:18:05 -04:00
// @DisplayName: Fence Action
// @Description: What action should be taken when fence is breached
2021-01-30 22:34:40 -04:00
// @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
2021-04-26 00:23:33 -03:00
// @Values{Rover}: 0:Report Only,1:RTL or Hold,2:Hold,3:SmartRTL or RTL or Hold,4:SmartRTL or Hold
2020-09-10 03:28:09 -03:00
// @Values{Plane}: 0:Report Only,1:RTL,6:Guided,7:GuidedThrottlePass
2019-01-24 06:00:42 -04:00
// @Values: 0:Report Only,1:RTL or Land
2013-04-26 06:47:07 -03:00
// @User: Standard
AP_GROUPINFO ( " ACTION " , 2 , AC_Fence , _action , AC_FENCE_ACTION_RTL_AND_LAND ) ,
2020-09-10 03:28:09 -03:00
// @Param{Copter, Plane, Sub}: ALT_MAX
2013-04-26 06:47:07 -03:00
// @DisplayName: Fence Maximum Altitude
// @Description: Maximum altitude allowed before geofence triggers
2017-05-02 10:38:01 -03:00
// @Units: m
2013-05-01 05:05:04 -03:00
// @Range: 10 1000
// @Increment: 1
2013-04-26 06:47:07 -03:00
// @User: Standard
2020-09-10 03:28:09 -03:00
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 ) ,
2013-04-26 06:47:07 -03:00
// @Param: RADIUS
// @DisplayName: Circular Fence Radius
2013-05-01 05:05:04 -03:00
// @Description: Circle fence radius which when breached will cause an RTL
2017-05-02 10:38:01 -03:00
// @Units: m
2013-07-26 22:28:33 -03:00
// @Range: 30 10000
2013-04-26 06:47:07 -03:00
// @User: Standard
2013-05-01 05:05:04 -03:00
AP_GROUPINFO ( " RADIUS " , 4 , AC_Fence , _circle_radius , AC_FENCE_CIRCLE_RADIUS_DEFAULT ) ,
2013-08-15 04:05:38 -03:00
// @Param: MARGIN
// @DisplayName: Fence Margin
// @Description: Distance that autopilot's should maintain from the fence to avoid a breach
2017-05-02 10:38:01 -03:00
// @Units: m
2013-08-15 04:05:38 -03:00
// @Range: 1 10
// @User: Standard
AP_GROUPINFO ( " MARGIN " , 5 , AC_Fence , _margin , AC_FENCE_MARGIN_DEFAULT ) ,
2016-06-23 03:24:48 -03:00
// @Param: TOTAL
// @DisplayName: Fence polygon point total
// @Description: Number of polygon points saved in eeprom (do not update manually)
// @Range: 1 20
// @User: Standard
AP_GROUPINFO ( " TOTAL " , 6 , AC_Fence , _total , 0 ) ,
2020-09-10 03:28:09 -03:00
// @Param{Copter, Plane, Sub}: ALT_MIN
2017-02-03 00:18:25 -04:00
// @DisplayName: Fence Minimum Altitude
// @Description: Minimum altitude allowed before geofence triggers
2017-05-02 10:38:01 -03:00
// @Units: m
2017-02-03 00:18:25 -04:00
// @Range: -100 100
// @Increment: 1
// @User: Standard
2017-07-17 05:39:26 -03:00
AP_GROUPINFO_FRAME ( " ALT_MIN " , 7 , AC_Fence , _alt_min , AC_FENCE_ALT_MIN_DEFAULT , AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE ) ,
2020-09-10 03:28:09 -03:00
// @Param{Plane}: RET_RALLY
// @DisplayName: Fence Return to Rally
2021-01-06 08:15:59 -04:00
// @Description: Should the vehicle return to fence return point or rally point
2020-09-10 03:28:09 -03:00
// @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 ) ,
2023-12-04 12:58:40 -04:00
// @Param{Plane, Copter}: AUTOENABLE
2020-09-10 03:28:09 -03:00
// @DisplayName: Fence Auto-Enable
2023-12-04 12:58:40 -04:00
// @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences on arming, except the minimum altitude fence (which is enabled when the minimum altitude is reached), but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings.
// @Values{Plane, Copter}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed
2020-09-10 03:28:09 -03:00
// @Range: 0 3
// @Increment: 1
// @User: Standard
2023-12-04 12:58:40 -04:00
AP_GROUPINFO_FRAME ( " AUTOENABLE " , 10 , AC_Fence , _auto_enabled , static_cast < uint8_t > ( AutoEnable : : ALWAYS_DISABLED ) , AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI ) ,
2017-02-03 00:18:25 -04:00
2024-06-26 06:09:08 -03:00
// @Param{Plane, Copter}: OPTIONS
2022-08-12 16:32:28 -03:00
// @DisplayName: Fence options
2023-12-04 12:58:40 -04:00
// @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.
2023-07-22 19:03:08 -03:00
// @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas
2022-08-12 16:32:28 -03:00
// @User: Standard
2024-07-21 08:29:04 -03:00
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 ) ,
2022-08-12 16:32:28 -03:00
2013-04-26 06:47:07 -03:00
AP_GROUPEND
} ;
/// Default constructor.
2019-01-30 20:37:22 -04:00
AC_Fence : : AC_Fence ( )
2013-04-26 06:47:07 -03:00
{
2019-01-30 21:54:13 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if ( _singleton ! = nullptr ) {
AP_HAL : : panic ( " Fence must be singleton " ) ;
}
# endif
_singleton = this ;
2013-04-26 06:47:07 -03:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2023-12-04 12:58:40 -04:00
if ( _enabled ) {
_enabled_fences = _configured_fences . get ( ) & ~ AC_FENCE_TYPE_ALT_MIN ;
}
}
// get a user-friendly list of fences
void AC_Fence : : get_fence_names ( uint8_t fences , ExpandingString & msg )
{
if ( ! fences ) {
return ;
}
static const char * FENCE_NAMES [ ] = {
" Max Alt " ,
" Circle " ,
" Polygon " ,
" Min Alt " ,
} ;
uint8_t i = 0 ;
uint8_t nfences = 0 ;
while ( fences ! = 0 ) {
if ( fences & 0x1 ) {
if ( nfences > 0 ) {
if ( ! ( fences & ~ 1U ) ) {
msg . printf ( " and " ) ;
} else {
msg . printf ( " , " ) ;
}
}
msg . printf ( " %s " , FENCE_NAMES [ i ] ) ;
nfences + + ;
}
fences > > = 1 ;
i + + ;
}
msg . printf ( " fence " ) ;
if ( nfences > 1 ) {
msg . printf ( " s " ) ;
}
}
// print a message about the passed in fences
void AC_Fence : : print_fence_message ( const char * message , uint8_t fences ) const
{
if ( ! fences ) {
return ;
}
char msg [ MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1 ] ;
ExpandingString e ( msg , MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1 ) ;
AC_Fence : : get_fence_names ( fences , e ) ;
GCS_SEND_TEXT ( MAV_SEVERITY_NOTICE , " %s %s " , e . get_writeable_string ( ) , message ) ;
}
// should be called @10Hz to handle loading from eeprom
void AC_Fence : : update ( )
{
_poly_loader . update ( ) ;
// if someone changes the parameter we want to enable or disable everything
if ( _enabled ! = _last_enabled | | _auto_enabled ! = _last_auto_enabled ) {
// reset the auto mask since we just reconfigured all of fencing
_auto_enable_mask = AC_FENCE_ALL_FENCES ;
_last_enabled = _enabled ;
_last_auto_enabled = _auto_enabled ;
if ( _enabled ) {
_enabled_fences = _configured_fences . get ( ) & ~ AC_FENCE_TYPE_ALT_MIN ;
} else {
_enabled_fences = 0 ;
}
}
# ifdef AC_FENCE_DEBUG
static uint32_t last_msg_count = 0 ;
if ( get_enabled_fences ( ) & & last_msg_count + + % 10 = = 0 ) {
2024-03-11 09:40:31 -03:00
print_fence_message ( " active " , get_enabled_fences ( ) ) ;
print_fence_message ( " breached " , get_breaches ( ) ) ;
2023-12-04 12:58:40 -04:00
}
# endif
2013-04-26 06:47:07 -03:00
}
2024-07-21 08:29:04 -03:00
// enable or disable configured fences present in fence_types
// also updates the bitmask of auto enabled fences if update_auto_mask is true
// returns a bitmask of fences that were changed
2023-12-04 12:58:40 -04:00
uint8_t AC_Fence : : enable ( bool value , uint8_t fence_types , bool update_auto_mask )
2017-07-20 13:42:28 -03:00
{
2023-12-04 12:58:40 -04:00
uint8_t fences = _configured_fences . get ( ) & fence_types ;
uint8_t enabled_fences = _enabled_fences ;
if ( value ) {
enabled_fences | = fences ;
} else {
enabled_fences & = ~ fences ;
}
uint8_t fences_to_change = _enabled_fences ^ enabled_fences ;
if ( ! fences_to_change ) {
return 0 ;
}
2024-11-24 22:53:22 -04:00
// fences that were manually changed are no longer eligible for auto-enablement or disablement
if ( update_auto_mask ) {
_auto_enable_mask & = ~ fences_to_change ;
}
2023-07-13 21:58:04 -03:00
# if HAL_LOGGING_ENABLED
2023-12-04 12:58:40 -04:00
AP : : logger ( ) . Write_Event ( value ? LogEvent : : FENCE_ENABLE : LogEvent : : FENCE_DISABLE ) ;
if ( fences_to_change & AC_FENCE_TYPE_ALT_MAX ) {
AP : : logger ( ) . Write_Event ( value ? LogEvent : : FENCE_ALT_MAX_ENABLE : LogEvent : : FENCE_ALT_MAX_DISABLE ) ;
}
if ( fences_to_change & AC_FENCE_TYPE_CIRCLE ) {
AP : : logger ( ) . Write_Event ( value ? LogEvent : : FENCE_CIRCLE_ENABLE : LogEvent : : FENCE_CIRCLE_DISABLE ) ;
}
if ( fences_to_change & AC_FENCE_TYPE_ALT_MIN ) {
AP : : logger ( ) . Write_Event ( value ? LogEvent : : FENCE_ALT_MIN_ENABLE : LogEvent : : FENCE_ALT_MIN_DISABLE ) ;
}
if ( fences_to_change & AC_FENCE_TYPE_POLYGON ) {
AP : : logger ( ) . Write_Event ( value ? LogEvent : : FENCE_POLYGON_ENABLE : LogEvent : : FENCE_POLYGON_DISABLE ) ;
2020-06-01 23:27:28 -03:00
}
2023-07-13 21:58:04 -03:00
# endif
2023-12-04 12:58:40 -04:00
_enabled_fences = enabled_fences ;
2017-07-20 13:42:28 -03:00
if ( ! value ) {
2023-12-04 12:58:40 -04:00
clear_breach ( fences_to_change ) ;
2020-09-10 03:28:09 -03:00
}
2023-12-04 12:58:40 -04:00
return fences_to_change ;
2020-09-10 03:28:09 -03:00
}
/// enable/disable fence floor only
void AC_Fence : : enable_floor ( )
{
2023-12-04 12:58:40 -04:00
enable ( true , AC_FENCE_TYPE_ALT_MIN ) ;
2020-09-10 03:28:09 -03:00
}
void AC_Fence : : disable_floor ( )
{
2023-12-04 12:58:40 -04:00
enable ( false , AC_FENCE_TYPE_ALT_MIN ) ;
}
/*
called on arming
*/
void AC_Fence : : auto_enable_fence_on_arming ( void )
{
if ( auto_enabled ( ) ! = AC_Fence : : AutoEnable : : ONLY_WHEN_ARMED ) {
return ;
2017-07-20 13:42:28 -03:00
}
2023-12-04 12:58:40 -04:00
const uint8_t fences = enable ( true , _auto_enable_mask & ~ AC_FENCE_TYPE_ALT_MIN , false ) ;
print_fence_message ( " auto-enabled " , fences ) ;
2020-09-10 03:28:09 -03:00
}
2024-03-11 09:40:31 -03:00
/*
called on disarming
*/
void AC_Fence : : auto_disable_fence_on_disarming ( void )
{
if ( auto_enabled ( ) ! = AC_Fence : : AutoEnable : : ONLY_WHEN_ARMED ) {
return ;
}
const uint8_t fences = enable ( false , _auto_enable_mask , false ) ;
print_fence_message ( " auto-disabled " , fences ) ;
}
2021-01-13 19:35:48 -04:00
/*
called when an auto - takeoff is complete
*/
void AC_Fence : : auto_enable_fence_after_takeoff ( void )
{
2024-07-17 19:00:11 -03:00
if ( auto_enabled ( ) ! = AutoEnable : : ENABLE_ON_AUTO_TAKEOFF & &
auto_enabled ( ) ! = AutoEnable : : ENABLE_DISABLE_FLOOR_ONLY ) {
2024-05-31 10:12:38 -03:00
return ;
2021-01-13 19:35:48 -04:00
}
2024-05-31 10:12:38 -03:00
const uint8_t fences = enable ( true , _auto_enable_mask , false ) ;
print_fence_message ( " auto-enabled " , fences ) ;
2021-01-13 19:35:48 -04:00
}
2024-05-31 10:12:38 -03:00
// return fences that should be auto-disabled when requested
uint8_t AC_Fence : : get_auto_disable_fences ( void ) const
2021-01-13 19:35:48 -04:00
{
2024-05-31 10:12:38 -03:00
uint8_t auto_disable = 0 ;
2021-01-13 19:35:48 -04:00
switch ( auto_enabled ( ) ) {
2024-05-31 10:12:38 -03:00
case AC_Fence : : AutoEnable : : ENABLE_ON_AUTO_TAKEOFF :
auto_disable = _auto_enable_mask ;
2021-01-13 19:35:48 -04:00
break ;
case AC_Fence : : AutoEnable : : ENABLE_DISABLE_FLOOR_ONLY :
2023-12-04 12:58:40 -04:00
case AC_Fence : : AutoEnable : : ONLY_WHEN_ARMED :
2024-06-22 08:02:21 -03:00
default : // when auto disable is not set we still need to disable the altmin fence on landing
2024-05-31 10:12:38 -03:00
auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN ;
2021-01-13 19:35:48 -04:00
break ;
}
2024-05-31 10:12:38 -03:00
return auto_disable ;
2021-01-13 19:35:48 -04:00
}
2023-12-04 12:58:40 -04:00
uint8_t AC_Fence : : present ( ) const
2020-09-10 03:28:09 -03:00
{
2023-12-04 12:58:40 -04:00
uint8_t mask = AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX ;
if ( _poly_loader . total_fence_count ( ) > 0 ) {
mask | = AC_FENCE_TYPE_POLYGON ;
2020-09-10 03:28:09 -03:00
}
2020-12-18 05:08:56 -04:00
2023-12-04 12:58:40 -04:00
return _configured_fences . get ( ) & mask ;
2017-07-20 13:42:28 -03:00
}
2013-04-26 06:47:07 -03:00
/// get_enabled_fences - returns bitmask of enabled fences
2013-05-01 05:05:04 -03:00
uint8_t AC_Fence : : get_enabled_fences ( ) const
2013-04-26 06:47:07 -03:00
{
2023-12-04 12:58:40 -04:00
return _enabled_fences & present ( ) ;
2013-04-26 06:47:07 -03:00
}
2017-12-13 22:54:03 -04:00
// additional checks for the polygon fence:
2024-06-22 08:02:21 -03:00
bool AC_Fence : : pre_arm_check_polygon ( char * failure_msg , const uint8_t failure_msg_len ) const
2017-12-13 22:54:03 -04:00
{
2023-12-04 12:58:40 -04:00
if ( ! ( _configured_fences & AC_FENCE_TYPE_POLYGON ) ) {
2017-12-13 22:54:03 -04:00
// not enabled; all good
return true ;
}
2019-08-28 04:22:16 -03:00
if ( ! _poly_loader . loaded ( ) ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Polygon fence(s) invalid " ) ;
2017-12-13 22:54:03 -04:00
return false ;
}
2020-07-21 14:00:57 -03:00
if ( ! _poly_loader . check_inclusion_circle_margin ( _margin ) ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Polygon fence margin is less than inclusion circle radius " ) ;
2020-07-21 14:00:57 -03:00
return false ;
}
2017-12-13 22:54:03 -04:00
return true ;
}
2017-12-26 22:53:39 -04:00
// additional checks for the circle fence:
2024-06-22 08:02:21 -03:00
bool AC_Fence : : pre_arm_check_circle ( char * failure_msg , const uint8_t failure_msg_len ) const
2017-12-26 22:53:39 -04:00
{
if ( _circle_radius < 0 ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Invalid Circle FENCE_RADIUS value " ) ;
2017-12-26 22:53:39 -04:00
return false ;
}
2020-07-21 14:00:57 -03:00
if ( _circle_radius < _margin ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Circle FENCE_MARGIN is less than FENCE_RADIUS " ) ;
2020-07-21 14:00:57 -03:00
return false ;
}
2017-12-26 22:53:39 -04:00
return true ;
}
// additional checks for the alt fence:
2024-06-22 08:02:21 -03:00
bool AC_Fence : : pre_arm_check_alt ( char * failure_msg , const uint8_t failure_msg_len ) const
2017-12-26 22:53:39 -04:00
{
if ( _alt_max < 0.0f ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Invalid FENCE_ALT_MAX value " ) ;
2017-12-26 22:53:39 -04:00
return false ;
}
2020-09-10 03:28:09 -03:00
if ( _alt_min < - 100.0f ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Invalid FENCE_ALT_MIN value " ) ;
2020-09-10 03:28:09 -03:00
return false ;
}
2017-12-26 22:53:39 -04:00
return true ;
}
2017-12-13 22:54:03 -04:00
2013-04-26 06:47:07 -03:00
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
2024-06-22 08:02:21 -03:00
bool AC_Fence : : pre_arm_check ( char * failure_msg , const uint8_t failure_msg_len ) const
2013-04-26 06:47:07 -03:00
{
2021-05-18 12:20:49 -03:00
// if fences are enabled but none selected fail pre-arm check
2023-12-04 12:58:40 -04:00
if ( _enabled & & ! present ( ) ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Fences enabled, but none selected " ) ;
2021-05-18 12:20:49 -03:00
return false ;
}
2024-07-23 21:12:43 -03:00
// if AUTOENABLE = 1 or 2 warn now, but fail in a later release
// PARAMETER_CONVERSION - Added: Jul-2024 for ArduPilot-4.6
if ( _auto_enabled = = 1 | | _auto_enabled = = 2 ) {
static uint32_t last_autoenable_warn_ms ;
const uint32_t now_ms = AP_HAL : : millis ( ) ;
if ( now_ms - last_autoenable_warn_ms > 60000 ) {
GCS_SEND_TEXT ( MAV_SEVERITY_WARNING , " FENCE_AUTOENABLE is %u, will be removed in 4.7, use 3 " , unsigned ( _auto_enabled ) ) ;
last_autoenable_warn_ms = now_ms ;
}
}
2021-05-18 12:20:49 -03:00
2013-04-26 06:47:07 -03:00
// if not enabled or not fence set-up always return true
2023-12-04 12:58:40 -04:00
if ( ( ! enabled ( ) & & ! _auto_enabled ) | | ! _configured_fences ) {
2013-04-26 06:47:07 -03:00
return true ;
}
2017-12-07 19:25:52 -04:00
// if we have horizontal limits enabled, check we can get a
2019-01-30 20:37:22 -04:00
// relative position from the AHRS
2023-12-04 12:58:40 -04:00
if ( ( _configured_fences & AC_FENCE_TYPE_CIRCLE ) | |
( _configured_fences & AC_FENCE_TYPE_POLYGON ) ) {
2017-12-07 19:25:52 -04:00
Vector2f position ;
2019-03-05 23:04:58 -04:00
if ( ! AP : : ahrs ( ) . get_relative_position_NE_home ( position ) ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Fence requires position " ) ;
2017-12-07 19:25:52 -04:00
return false ;
}
2013-04-26 06:47:07 -03:00
}
2024-06-22 08:02:21 -03:00
if ( ! pre_arm_check_polygon ( failure_msg , failure_msg_len ) ) {
2017-12-13 22:54:03 -04:00
return false ;
}
2024-06-22 08:02:21 -03:00
if ( ! pre_arm_check_circle ( failure_msg , failure_msg_len ) ) {
2017-12-26 22:53:39 -04:00
return false ;
}
2024-06-22 08:02:21 -03:00
if ( ! pre_arm_check_alt ( failure_msg , failure_msg_len ) ) {
2017-12-26 22:53:39 -04:00
return false ;
}
2019-08-28 04:22:16 -03:00
// check no limits are currently breached
if ( _breached_fences ) {
2024-06-22 08:02:21 -03:00
char msg [ MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1 ] ;
ExpandingString e ( msg , MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1 ) ;
AC_Fence : : get_fence_names ( _breached_fences , e ) ;
2024-07-28 02:59:02 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Vehicle breaching %s " , e . get_writeable_string ( ) ) ;
2019-08-28 04:22:16 -03:00
return false ;
}
2017-08-24 06:48:17 -03:00
// validate FENCE_MARGIN parameter range
if ( _margin < 0.0f ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " Invalid FENCE_MARGIN value " ) ;
2017-08-24 06:48:17 -03:00
return false ;
}
if ( _alt_max < _alt_min ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " FENCE_ALT_MAX < FENCE_ALT_MIN " ) ;
2017-08-24 06:48:17 -03:00
return false ;
}
if ( _alt_max - _alt_min < = 2.0f * _margin ) {
2024-06-22 08:02:21 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " FENCE_MARGIN too big " ) ;
2017-08-24 06:48:17 -03:00
return false ;
}
2013-04-26 06:47:07 -03:00
// if we got this far everything must be ok
return true ;
}
2019-08-28 04:22:16 -03:00
/// returns true if we have freshly breached the maximum altitude
/// fence; also may set up a fallback fence which, if breached, will
/// cause the altitude fence to be freshly breached
2017-12-15 05:35:22 -04:00
bool AC_Fence : : check_fence_alt_max ( )
2017-12-07 20:53:18 -04:00
{
// altitude fence check
2023-12-04 12:58:40 -04:00
if ( ! ( get_enabled_fences ( ) & AC_FENCE_TYPE_ALT_MAX ) ) {
2017-12-07 20:53:18 -04:00
// not enabled; no breach
return false ;
}
2023-12-04 12:58:40 -04:00
float alt ;
AP : : ahrs ( ) . get_relative_position_D_home ( alt ) ;
_curr_alt = - alt ; // translate Down to Up
2017-12-15 05:35:22 -04:00
2017-12-07 20:53:18 -04:00
// check if we are over the altitude fence
2020-09-10 03:28:09 -03:00
if ( _curr_alt > = _alt_max ) {
2017-12-07 20:53:18 -04:00
// record distance above breach
2017-12-15 05:35:22 -04:00
_alt_max_breach_distance = _curr_alt - _alt_max ;
2017-12-07 20:53:18 -04:00
// check for a new breach or a breach of the backup fence
if ( ! ( _breached_fences & AC_FENCE_TYPE_ALT_MAX ) | |
2017-12-15 05:35:22 -04:00
( ! is_zero ( _alt_max_backup ) & & _curr_alt > = _alt_max_backup ) ) {
2017-12-07 20:53:18 -04:00
// new breach
record_breach ( AC_FENCE_TYPE_ALT_MAX ) ;
// create a backup fence 20m higher up
2017-12-15 05:35:22 -04:00
_alt_max_backup = _curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE ;
2021-01-06 08:15:59 -04:00
// new breach
2017-12-07 20:53:18 -04:00
return true ;
}
2021-01-06 08:15:59 -04:00
// old breach
2017-12-07 20:53:18 -04:00
return false ;
}
// not breached
2021-01-06 08:15:59 -04:00
// clear max alt breach if present
2017-12-07 20:53:18 -04:00
if ( ( _breached_fences & AC_FENCE_TYPE_ALT_MAX ) ! = 0 ) {
clear_breach ( AC_FENCE_TYPE_ALT_MAX ) ;
_alt_max_backup = 0.0f ;
_alt_max_breach_distance = 0.0f ;
}
return false ;
}
2020-09-10 03:28:09 -03:00
/// 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
2023-12-04 12:58:40 -04:00
if ( ! ( get_enabled_fences ( ) & AC_FENCE_TYPE_ALT_MIN ) ) {
2021-01-06 08:15:59 -04:00
// not enabled; no breach
2020-09-10 03:28:09 -03:00
return false ;
}
2023-12-04 12:58:40 -04:00
float alt ;
AP : : ahrs ( ) . get_relative_position_D_home ( alt ) ;
_curr_alt = - alt ; // translate Down to Up
2020-09-10 03:28:09 -03:00
// check if we are under the altitude fence
2017-08-24 06:48:17 -03:00
if ( _curr_alt < = _alt_min ) {
2020-09-10 03:28:09 -03:00
// 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 ) | |
2017-08-24 06:48:17 -03:00
( ! is_zero ( _alt_min_backup ) & & _curr_alt < = _alt_min_backup ) ) {
2020-09-10 03:28:09 -03:00
// 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
2021-01-06 08:15:59 -04:00
// clear min alt breach if present
2020-09-10 03:28:09 -03:00
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 ;
}
2023-12-04 12:58:40 -04:00
/// auto enable fence floor
bool AC_Fence : : auto_enable_fence_floor ( )
{
// altitude fence check
2024-05-31 10:12:38 -03:00
if ( ! ( _configured_fences & AC_FENCE_TYPE_ALT_MIN ) // not configured
| | ( get_enabled_fences ( ) & AC_FENCE_TYPE_ALT_MIN ) // already enabled
| | ! ( _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN ) // has been manually disabled
| | ( ! _enabled & & ( auto_enabled ( ) = = AC_Fence : : AutoEnable : : ALWAYS_DISABLED
| | auto_enabled ( ) = = AutoEnable : : ENABLE_ON_AUTO_TAKEOFF ) ) ) {
2023-12-04 12:58:40 -04:00
// not enabled
return false ;
}
float alt ;
AP : : ahrs ( ) . get_relative_position_D_home ( alt ) ;
_curr_alt = - alt ; // translate Down to Up
// check if we are over the altitude fence
2024-05-31 10:12:38 -03:00
if ( ! floor_enabled ( ) & & _curr_alt > = _alt_min + _margin ) {
2024-05-31 04:20:13 -03:00
enable ( true , AC_FENCE_TYPE_ALT_MIN , false ) ;
2024-08-07 00:17:19 -03:00
GCS_SEND_TEXT ( MAV_SEVERITY_NOTICE , " Min Alt fence enabled (auto enable) " ) ;
2023-12-04 12:58:40 -04:00
return true ;
}
return false ;
}
2019-08-28 04:22:16 -03:00
// check_fence_polygon - returns true if the poly fence is freshly
// breached. That includes being inside exclusion zones and outside
// inclusions zones
2017-12-07 20:10:16 -04:00
bool AC_Fence : : check_fence_polygon ( )
2019-03-06 00:45:00 -04:00
{
2024-03-11 09:40:31 -03:00
if ( ! ( get_enabled_fences ( ) & AC_FENCE_TYPE_POLYGON ) ) {
// not enabled; no breach
2024-05-19 04:39:50 -03:00
clear_breach ( AC_FENCE_TYPE_POLYGON ) ;
2024-03-11 09:40:31 -03:00
return false ;
}
2019-03-06 00:45:00 -04:00
const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON ;
2024-03-11 09:40:31 -03:00
if ( _poly_loader . breached ( ) ) {
2019-03-06 00:45:00 -04:00
if ( ! was_breached ) {
record_breach ( AC_FENCE_TYPE_POLYGON ) ;
return true ;
}
return false ;
}
if ( was_breached ) {
clear_breach ( AC_FENCE_TYPE_POLYGON ) ;
}
return false ;
}
2019-08-28 04:22:16 -03:00
/// check_fence_circle - returns true if the circle fence (defined via
/// parameters) has been freshly breached. May also set up a backup
/// fence outside the fence and return a fresh breach if that backup
2023-10-11 04:41:49 -03:00
/// fence is breached.
2017-12-07 20:18:29 -04:00
bool AC_Fence : : check_fence_circle ( )
{
2023-12-04 12:58:40 -04:00
if ( ! ( get_enabled_fences ( ) & AC_FENCE_TYPE_CIRCLE ) ) {
2017-12-07 20:18:29 -04:00
// not enabled; no breach
return false ;
}
2017-12-13 23:46:48 -04:00
Vector2f home ;
2019-01-30 20:37:22 -04:00
if ( AP : : ahrs ( ) . get_relative_position_NE_home ( home ) ) {
2017-12-13 23:46:48 -04:00
// we (may) remain breached if we can't update home
_home_distance = home . length ( ) ;
}
2017-12-07 20:18:29 -04:00
// check if we are outside the fence
if ( _home_distance > = _circle_radius ) {
// record distance outside the fence
_circle_breach_distance = _home_distance - _circle_radius ;
// check for a new breach or a breach of the backup fence
if ( ! ( _breached_fences & AC_FENCE_TYPE_CIRCLE ) | |
( ! is_zero ( _circle_radius_backup ) & & _home_distance > = _circle_radius_backup ) ) {
// new breach
2023-08-18 15:30:22 -03:00
// create a backup fence 20m or 100m further out
2017-12-07 20:18:29 -04:00
record_breach ( AC_FENCE_TYPE_CIRCLE ) ;
_circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE ;
return true ;
}
return false ;
}
// not currently breached
// clear circle breach if present
if ( _breached_fences & AC_FENCE_TYPE_CIRCLE ) {
clear_breach ( AC_FENCE_TYPE_CIRCLE ) ;
_circle_radius_backup = 0.0f ;
_circle_breach_distance = 0.0f ;
}
return false ;
}
2017-12-07 20:10:16 -04:00
2017-12-15 05:35:22 -04:00
/// check - returns bitmask of fence types breached (if any)
2024-05-31 10:12:38 -03:00
uint8_t AC_Fence : : check ( bool disable_auto_fences )
2013-04-26 06:47:07 -03:00
{
2017-12-15 06:06:56 -04:00
uint8_t ret = 0 ;
2024-05-31 10:12:38 -03:00
uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences ( ) : 0 ;
uint8_t fences_to_disable = disabled_fences & _enabled_fences ;
2013-04-26 06:47:07 -03:00
2022-09-10 18:03:02 -03:00
// clear any breach from a non-enabled fence
2023-12-04 12:58:40 -04:00
clear_breach ( ~ _configured_fences ) ;
2024-05-31 10:12:38 -03:00
// clear any breach from disabled fences
clear_breach ( fences_to_disable ) ;
// report on any fences that were auto-disabled
if ( fences_to_disable ) {
print_fence_message ( " auto-disabled " , fences_to_disable ) ;
}
2022-09-10 18:03:02 -03:00
2013-04-26 06:47:07 -03:00
// return immediately if disabled
2023-12-04 12:58:40 -04:00
if ( ( ! enabled ( ) & & ! _auto_enabled & & ! ( _configured_fences & AC_FENCE_TYPE_ALT_MIN ) ) | | ! _configured_fences ) {
2017-12-15 06:06:56 -04:00
return 0 ;
2013-04-26 06:47:07 -03:00
}
2024-05-31 10:12:38 -03:00
// disable the (temporarily) disabled fences
enable ( false , disabled_fences , false ) ;
2017-12-07 20:53:18 -04:00
// maximum altitude fence check
2024-05-31 10:12:38 -03:00
if ( ! ( disabled_fences & AC_FENCE_TYPE_ALT_MAX ) & & check_fence_alt_max ( ) ) {
2017-12-07 20:53:18 -04:00
ret | = AC_FENCE_TYPE_ALT_MAX ;
2013-04-26 06:47:07 -03:00
}
2023-12-04 12:58:40 -04:00
// minimum altitude fence check, do this before auto-disabling (e.g. because falling)
// so that any action can be taken
2024-05-31 10:12:38 -03:00
if ( ! ( disabled_fences & AC_FENCE_TYPE_ALT_MIN ) & & check_fence_alt_min ( ) ) {
2020-09-10 03:28:09 -03:00
ret | = AC_FENCE_TYPE_ALT_MIN ;
}
2024-05-31 10:12:38 -03:00
// auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required)
if ( ! ( disabled_fences & AC_FENCE_TYPE_ALT_MIN ) ) {
2023-12-04 12:58:40 -04:00
auto_enable_fence_floor ( ) ;
}
2013-04-26 06:47:07 -03:00
// circle fence check
2024-05-31 10:12:38 -03:00
if ( ! ( disabled_fences & AC_FENCE_TYPE_CIRCLE ) & & check_fence_circle ( ) ) {
2017-12-07 20:18:29 -04:00
ret | = AC_FENCE_TYPE_CIRCLE ;
2013-04-26 06:47:07 -03:00
}
2016-06-23 03:24:48 -03:00
// polygon fence check
2024-05-31 10:12:38 -03:00
if ( ! ( disabled_fences & AC_FENCE_TYPE_POLYGON ) & & check_fence_polygon ( ) ) {
2017-12-07 20:10:16 -04:00
ret | = AC_FENCE_TYPE_POLYGON ;
2016-06-23 03:24:48 -03:00
}
2023-12-04 12:58:40 -04:00
// check if pilot is attempting to recover manually
// this is done last so that _breached_fences is correct
if ( _manual_recovery_start_ms ! = 0 ) {
// we ignore any fence breaches during the manual recovery period which is about 10 seconds
if ( ( AP_HAL : : millis ( ) - _manual_recovery_start_ms ) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN ) {
return 0 ;
}
// recovery period has passed so reset manual recovery time
// and continue with fence breach checks
_manual_recovery_start_ms = 0 ;
}
2013-04-26 06:47:07 -03:00
// return any new breaches that have occurred
return ret ;
}
2016-05-19 05:31:36 -03:00
// returns true if the destination is within fence (used to reject waypoints outside the fence)
2019-01-01 22:54:09 -04:00
bool AC_Fence : : check_destination_within_fence ( const Location & loc )
2016-04-28 04:29:35 -03:00
{
2020-09-10 03:28:09 -03:00
// Altitude fence check - Fence Ceiling
2016-07-02 05:14:17 -03:00
if ( ( get_enabled_fences ( ) & AC_FENCE_TYPE_ALT_MAX ) ) {
int32_t alt_above_home_cm ;
2019-03-14 22:45:12 -03:00
if ( loc . get_alt_cm ( Location : : AltFrame : : ABOVE_HOME , alt_above_home_cm ) ) {
2016-07-02 05:14:17 -03:00
if ( ( alt_above_home_cm * 0.01f ) > _alt_max ) {
return false ;
}
}
2016-04-28 04:29:35 -03:00
}
2020-09-10 03:28:09 -03:00
// 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 ;
}
}
}
2016-04-28 04:29:35 -03:00
// Circular fence check
2016-07-02 05:14:17 -03:00
if ( ( get_enabled_fences ( ) & AC_FENCE_TYPE_CIRCLE ) ) {
2019-02-24 20:12:59 -04:00
if ( AP : : ahrs ( ) . get_home ( ) . get_distance ( loc ) > _circle_radius ) {
2016-07-02 05:14:17 -03:00
return false ;
}
}
// polygon fence check
2019-05-29 10:02:04 -03:00
if ( ( get_enabled_fences ( ) & AC_FENCE_TYPE_POLYGON ) ) {
if ( _poly_loader . breached ( loc ) ) {
return false ;
2016-07-02 05:14:17 -03:00
}
2016-04-28 04:29:35 -03:00
}
return true ;
}
2013-04-26 06:47:07 -03:00
/// record_breach - update breach bitmask, time and count
void AC_Fence : : record_breach ( uint8_t fence_type )
{
// if we haven't already breached a limit, update the breach time
2017-12-15 06:06:56 -04:00
if ( ! _breached_fences ) {
2021-02-12 23:01:25 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
_breach_time = now ;
// emit a message indicated we're newly-breached, but not too often
if ( now - _last_breach_notify_sent_ms > 1000 ) {
_last_breach_notify_sent_ms = now ;
2023-09-02 02:21:34 -03:00
GCS_SEND_MESSAGE ( MSG_FENCE_STATUS ) ;
2021-02-12 23:01:25 -04:00
}
2013-04-26 06:47:07 -03:00
}
// update breach count
2013-05-01 05:05:04 -03:00
if ( _breach_count < 65500 ) {
2013-04-26 06:47:07 -03:00
_breach_count + + ;
}
// update bitmask
2013-05-01 05:05:04 -03:00
_breached_fences | = fence_type ;
2013-04-26 06:47:07 -03:00
}
/// clear_breach - update breach bitmask, time and count
void AC_Fence : : clear_breach ( uint8_t fence_type )
{
2013-05-01 05:05:04 -03:00
_breached_fences & = ~ fence_type ;
}
2013-04-26 06:47:07 -03:00
2019-08-08 00:16:54 -03:00
/// get_breach_distance - returns maximum distance in meters outside
/// of the given fences. fence_type is a bitmask here.
2013-05-01 05:05:04 -03:00
float AC_Fence : : get_breach_distance ( uint8_t fence_type ) const
{
2019-08-08 00:16:54 -03:00
float max = 0.0f ;
2020-09-10 03:28:09 -03:00
2019-08-08 00:16:54 -03:00
if ( fence_type & AC_FENCE_TYPE_ALT_MAX ) {
max = MAX ( _alt_max_breach_distance , max ) ;
}
2020-09-10 03:28:09 -03:00
if ( fence_type & AC_FENCE_TYPE_ALT_MIN ) {
2021-01-06 08:15:59 -04:00
max = MAX ( _alt_min_breach_distance , max ) ;
2020-09-10 03:28:09 -03:00
}
2019-08-08 00:16:54 -03:00
if ( fence_type & AC_FENCE_TYPE_CIRCLE ) {
max = MAX ( _circle_breach_distance , max ) ;
}
return max ;
2013-07-24 11:05:21 -03:00
}
2014-04-26 23:08:37 -03:00
/// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds
/// has no effect if no breaches have occurred
void AC_Fence : : manual_recovery_start ( )
{
// return immediate if we haven't breached a fence
2017-12-15 06:06:56 -04:00
if ( ! _breached_fences ) {
2014-04-26 23:08:37 -03:00
return ;
}
// record time pilot began manual recovery
2015-11-19 23:05:31 -04:00
_manual_recovery_start_ms = AP_HAL : : millis ( ) ;
2023-12-04 12:58:40 -04:00
2024-08-07 00:17:19 -03:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " Manual recovery started " ) ;
2014-04-26 23:08:37 -03:00
}
2016-06-23 03:24:48 -03:00
2018-12-04 00:32:42 -04:00
// methods for mavlink SYS_STATUS message (send_sys_status)
2017-12-27 00:31:32 -04:00
bool AC_Fence : : sys_status_present ( ) const
2017-12-12 01:49:14 -04:00
{
2020-09-10 03:28:09 -03:00
return present ( ) ;
2017-12-12 01:49:14 -04:00
}
2017-12-27 00:31:32 -04:00
bool AC_Fence : : sys_status_enabled ( ) const
2017-12-12 01:49:14 -04:00
{
2017-12-27 00:31:32 -04:00
if ( ! sys_status_present ( ) ) {
2017-12-12 01:49:14 -04:00
return false ;
}
if ( _action = = AC_FENCE_ACTION_REPORT_ONLY ) {
return false ;
}
2020-12-18 05:08:56 -04:00
// Fence is only enabled when the flag is enabled
2023-12-04 12:58:40 -04:00
return enabled ( ) ;
2017-12-12 01:49:14 -04:00
}
2017-12-27 00:31:32 -04:00
bool AC_Fence : : sys_status_failed ( ) const
2017-12-12 01:49:14 -04:00
{
2017-12-27 00:31:32 -04:00
if ( ! sys_status_present ( ) ) {
2017-12-12 01:49:14 -04:00
// not failed if not present; can fail if present but not enabled
return false ;
}
if ( get_breaches ( ) ! = 0 ) {
return true ;
}
return false ;
}
2019-01-30 21:54:13 -04:00
2020-09-10 03:28:09 -03:00
AC_PolyFence_loader & AC_Fence : : polyfence ( )
{
2019-08-28 04:22:16 -03:00
return _poly_loader ;
}
2020-09-10 03:28:09 -03:00
const AC_PolyFence_loader & AC_Fence : : polyfence ( ) const
{
2019-08-28 04:22:16 -03:00
return _poly_loader ;
}
2022-03-04 16:25:54 -04:00
# else // build type is not appropriate; provide a dummy implementation:
const AP_Param : : GroupInfo AC_Fence : : var_info [ ] = { AP_GROUPEND } ;
AC_Fence : : AC_Fence ( ) { } ;
2023-12-04 12:58:40 -04:00
uint8_t AC_Fence : : enable ( bool value , uint8_t fence_types , bool update_auto_enable ) { return 0 ; }
2022-03-04 16:25:54 -04:00
2023-12-04 12:58:40 -04:00
void AC_Fence : : enable_floor ( ) { }
void AC_Fence : : disable_floor ( ) { }
void AC_Fence : : update ( ) { }
2022-03-04 16:25:54 -04:00
2023-12-04 12:58:40 -04:00
void AC_Fence : : auto_enable_fence_after_takeoff ( ) { }
void AC_Fence : : auto_enable_fence_on_arming ( ) { }
2024-03-11 09:40:31 -03:00
void AC_Fence : : auto_disable_fence_on_disarming ( ) { }
2022-03-04 16:25:54 -04:00
2023-12-04 12:58:40 -04:00
uint8_t AC_Fence : : present ( ) const { return 0 ; }
2022-03-04 16:25:54 -04:00
uint8_t AC_Fence : : get_enabled_fences ( ) const { return 0 ; }
2024-06-22 08:02:21 -03:00
bool AC_Fence : : pre_arm_check ( char * failure_msg , const uint8_t failure_msg_len ) const { return true ; }
2022-03-04 16:25:54 -04:00
2024-05-31 10:12:38 -03:00
uint8_t AC_Fence : : check ( bool disable_auto_fences ) { return 0 ; }
2022-03-04 16:25:54 -04:00
bool AC_Fence : : check_destination_within_fence ( const Location & loc ) { return true ; }
float AC_Fence : : get_breach_distance ( uint8_t fence_type ) const { return 0.0 ; }
2023-12-04 12:58:40 -04:00
void AC_Fence : : get_fence_names ( uint8_t fences , ExpandingString & msg ) { }
void AC_Fence : : print_fence_message ( const char * msg , uint8_t fences ) const { }
2022-03-04 16:25:54 -04:00
void AC_Fence : : manual_recovery_start ( ) { }
bool AC_Fence : : sys_status_present ( ) const { return false ; }
bool AC_Fence : : sys_status_enabled ( ) const { return false ; }
bool AC_Fence : : sys_status_failed ( ) const { return false ; }
AC_PolyFence_loader & AC_Fence : : polyfence ( )
{
return _poly_loader ;
}
const AC_PolyFence_loader & AC_Fence : : polyfence ( ) const
{
return _poly_loader ;
}
# endif // #if AC_FENCE_DUMMY_METHODS_ENABLED
2019-01-30 21:54:13 -04:00
// singleton instance
AC_Fence * AC_Fence : : _singleton ;
2020-09-10 03:28:09 -03:00
namespace AP
{
2019-01-30 21:54:13 -04:00
AC_Fence * fence ( )
{
return AC_Fence : : get_singleton ( ) ;
}
}
2022-03-04 12:42:09 -04:00
2022-07-19 08:32:28 -03:00
# endif // AP_FENCE_ENABLED