2016-02-17 21:25:06 -04:00
# pragma once
2013-04-26 06:47:07 -03:00
# include <inttypes.h>
2015-08-11 03:28:41 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_Param/AP_Param.h>
# include <AP_Math/AP_Math.h>
2016-06-25 03:43:01 -03:00
# include <AC_Fence/AC_PolyFence_loader.h>
2013-04-26 06:47:07 -03:00
// bit masks for enabled fence types. Used for TYPE parameter
2013-05-01 05:05:04 -03:00
# 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)
2016-06-23 03:24:48 -03:00
# define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence
2020-09-10 03:28:09 -03:00
# define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL
2013-04-26 06:47:07 -03:00
// valid actions should a fence be breached
2013-05-01 05:05:04 -03:00
# define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action
# define AC_FENCE_ACTION_RTL_AND_LAND 1 // return to launch and, if that fails, land
2018-07-24 01:19:12 -03:00
# define AC_FENCE_ACTION_ALWAYS_LAND 2 // always land
2019-01-24 06:00:42 -04:00
# 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
2021-01-30 22:34:40 -04:00
# define AC_FENCE_ACTION_SMART_RTL_OR_LAND 5 // SmartRTL, if that fails, Land
2020-09-10 03:28:09 -03:00
# 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
2019-01-24 06:00:42 -04:00
2013-04-26 06:47:07 -03:00
// default boundaries
2013-05-03 02:57:37 -03:00
# define AC_FENCE_ALT_MAX_DEFAULT 100.0f // default max altitude is 100m
2017-02-03 00:18:25 -04:00
# define AC_FENCE_ALT_MIN_DEFAULT -10.0f // default maximum depth in meters
2014-07-13 23:21:39 -03:00
# define AC_FENCE_CIRCLE_RADIUS_DEFAULT 300.0f // default circular fence radius is 300m
2013-05-01 05:05:04 -03:00
# define AC_FENCE_ALT_MAX_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further up
2020-09-10 03:28:09 -03:00
# define AC_FENCE_ALT_MIN_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further down
2013-05-01 05:05:04 -03:00
# define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further out
2013-08-15 04:05:38 -03:00
# define AC_FENCE_MARGIN_DEFAULT 2.0f // default distance in meters that autopilot's should maintain from the fence to avoid a breach
2013-05-01 05:05:04 -03:00
// give up distance
# define AC_FENCE_GIVE_UP_DISTANCE 100.0f // distance outside the fence at which we should give up and just land. Note: this is not used by library directly but is intended to be used by the main code
2014-04-26 23:08:37 -03:00
# 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
2013-04-26 06:47:07 -03:00
class AC_Fence
{
public :
2020-09-10 03:28:09 -03:00
enum class AutoEnable
{
ALWAYS_DISABLED = 0 ,
ALWAYS_ENABLED = 1 ,
ENABLE_DISABLE_FLOOR_ONLY = 2 ,
ONLY_WHEN_ARMED = 3
} ;
2019-01-30 20:37:22 -04:00
AC_Fence ( ) ;
2017-08-29 20:06:10 -03:00
/* Do not allow copies */
AC_Fence ( const AC_Fence & other ) = delete ;
AC_Fence & operator = ( const AC_Fence & ) = delete ;
2013-04-26 06:47:07 -03:00
2019-08-28 04:22:16 -03:00
void init ( ) {
_poly_loader . init ( ) ;
}
// get singleton instance
static AC_Fence * get_singleton ( ) { return _singleton ; }
2020-09-10 03:28:09 -03:00
/// enable - allows fence to be enabled/disabled.
2017-07-20 13:42:28 -03:00
void enable ( bool value ) ;
2013-04-26 06:47:07 -03:00
2021-01-06 08:15:59 -04:00
/// auto_enabled - automaticaly enable/disable fence depending of flight status
2020-09-10 03:28:09 -03:00
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 ( ) ;
2021-01-13 19:35:48 -04:00
/// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met
void auto_enable_fence_after_takeoff ( ) ;
/// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing.
void auto_disable_fence_for_landing ( ) ;
2013-04-26 06:47:07 -03:00
/// enabled - returns true if fence is enabled
2013-05-01 05:05:04 -03:00
bool enabled ( ) const { return _enabled ; }
2013-04-26 06:47:07 -03:00
2020-09-10 03:28:09 -03:00
/// present - returns true if fence is present
bool present ( ) const ;
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 get_enabled_fences ( ) const ;
2013-04-26 06:47:07 -03:00
2019-08-28 04:22:16 -03:00
// should be called @10Hz to handle loading from eeprom
void update ( ) {
_poly_loader . update ( ) ;
}
2013-04-26 06:47:07 -03:00
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
2017-03-24 01:38:19 -03:00
bool pre_arm_check ( const char * & fail_msg ) const ;
2013-04-26 06:47:07 -03:00
///
/// methods to check we are within the boundaries and recover
///
2017-12-15 05:35:22 -04:00
/// check - returns the fence type that has been breached (if any)
uint8_t check ( ) ;
2013-04-26 06:47:07 -03:00
2016-05-19 05:31:36 -03:00
// returns true if the destination is within fence (used to reject waypoints outside the fence)
2022-02-25 01:06:26 -04:00
bool check_destination_within_fence ( const class Location & loc ) ;
2016-04-28 04:29:35 -03:00
2013-04-26 06:47:07 -03:00
/// get_breaches - returns bit mask of the fence types that have been breached
uint8_t get_breaches ( ) const { return _breached_fences ; }
/// get_breach_time - returns time the fence was breached
uint32_t get_breach_time ( ) const { return _breach_time ; }
/// get_breach_count - returns number of times we have breached the fence
2013-05-01 05:05:04 -03:00
uint16_t get_breach_count ( ) const { return _breach_count ; }
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 get_breach_distance ( uint8_t fence_type ) const ;
2013-04-26 06:47:07 -03:00
/// get_action - getter for user requested action on limit breach
uint8_t get_action ( ) const { return _action . get ( ) ; }
2013-08-15 04:05:38 -03:00
/// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin)
2017-02-03 00:18:25 -04:00
float get_safe_alt_max ( ) const { return _alt_max - _margin ; }
2017-08-24 06:48:17 -03:00
/// get_safe_alt_min - returns the minimum safe altitude (i.e. alt_min + margin)
2017-02-03 00:18:25 -04:00
float get_safe_alt_min ( ) const { return _alt_min + _margin ; }
2014-04-26 23:08:37 -03:00
2016-06-19 22:14:14 -03:00
/// get_radius - returns the fence radius in meters
float get_radius ( ) const { return _circle_radius . get ( ) ; }
/// get_margin - returns the fence margin in meters
float get_margin ( ) const { return _margin . get ( ) ; }
2020-09-10 03:28:09 -03:00
/// 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 ; }
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
/// should be called whenever the pilot changes the flight mode
/// has no effect if no breaches have occurred
void manual_recovery_start ( ) ;
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 sys_status_present ( ) const ;
bool sys_status_enabled ( ) const ;
bool sys_status_failed ( ) const ;
2017-12-12 01:49:14 -04:00
2019-08-28 04:22:16 -03:00
AC_PolyFence_loader & polyfence ( ) ;
const AC_PolyFence_loader & polyfence ( ) const ;
2019-01-30 21:54:13 -04:00
2019-08-28 04:22:16 -03:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
2019-05-29 10:02:04 -03:00
2013-04-26 06:47:07 -03:00
private :
2019-01-30 21:54:13 -04:00
static AC_Fence * _singleton ;
2017-12-15 05:35:22 -04:00
2020-09-10 03:28:09 -03:00
/// check_fence_alt_max - true if max alt fence has been newly breached
2017-12-15 05:35:22 -04:00
bool check_fence_alt_max ( ) ;
2017-12-07 20:53:18 -04:00
2020-09-10 03:28:09 -03:00
/// check_fence_alt_min - true if min alt fence has been newly breached
bool check_fence_alt_min ( ) ;
2017-12-07 20:10:16 -04:00
/// check_fence_polygon - true if polygon fence has been newly breached
bool check_fence_polygon ( ) ;
2017-12-07 20:18:29 -04:00
/// check_fence_circle - true if circle fence has been newly breached
bool check_fence_circle ( ) ;
2013-04-26 06:47:07 -03:00
/// record_breach - update breach bitmask, time and count
void record_breach ( uint8_t fence_type ) ;
/// clear_breach - update breach bitmask, time and count
void clear_breach ( uint8_t fence_type ) ;
2017-12-26 22:53:39 -04:00
// additional checks for the different fence types:
2017-12-13 22:54:03 -04:00
bool pre_arm_check_polygon ( const char * & fail_msg ) const ;
2017-12-26 22:53:39 -04:00
bool pre_arm_check_circle ( const char * & fail_msg ) const ;
bool pre_arm_check_alt ( const char * & fail_msg ) const ;
2017-12-13 22:54:03 -04:00
2013-04-26 06:47:07 -03:00
// parameters
2021-01-06 08:13:57 -04:00
AP_Int8 _enabled ; // fence enable/disable control
2020-09-10 03:28:09 -03:00
AP_Int8 _auto_enabled ; // top level flag for auto enabling fence
2013-04-26 06:47:07 -03:00
AP_Int8 _enabled_fences ; // bit mask holding which fences are enabled
AP_Int8 _action ; // recovery action specified by user
2013-05-01 05:05:04 -03:00
AP_Float _alt_max ; // altitude upper limit in meters
2017-02-03 00:18:25 -04:00
AP_Float _alt_min ; // altitude lower limit in meters
2013-05-01 05:05:04 -03:00
AP_Float _circle_radius ; // circle fence radius in meters
2013-08-15 04:05:38 -03:00
AP_Float _margin ; // distance in meters that autopilot's should maintain from the fence to avoid a breach
2016-06-23 03:24:48 -03:00
AP_Int8 _total ; // number of polygon points saved in eeprom
2020-09-10 03:28:09 -03:00
AP_Int8 _ret_rally ; // return to fence return point or rally point/home
2021-02-15 20:41:56 -04:00
AP_Int16 _ret_altitude ; // return to this altitude
2013-05-01 05:05:04 -03:00
// 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
2020-09-10 03:28:09 -03:00
float _alt_min_backup ; // backup altitude lower limit in meters used to refire the breach if the vehicle continues to move further away
2013-05-01 05:05:04 -03:00
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
2020-09-10 03:28:09 -03:00
float _alt_min_breach_distance ; // distance below the altitude min
2013-08-15 04:05:38 -03:00
float _circle_breach_distance ; // distance beyond the circular fence
2013-04-26 06:47:07 -03:00
// other internal variables
2020-09-10 03:28:09 -03:00
bool _floor_enabled ; // fence floor is enabled
2013-05-01 05:05:04 -03:00
float _home_distance ; // distance from home in meters (provided by main code)
2020-09-10 03:28:09 -03:00
float _curr_alt ;
2017-12-15 05:35:22 -04:00
2013-04-26 06:47:07 -03:00
// 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)
uint32_t _breach_time ; // time of last breach in milliseconds
uint16_t _breach_count ; // number of times we have breached the fence
2021-02-12 23:01:25 -04:00
uint32_t _last_breach_notify_sent_ms ; // last time we sent a message about newly-breaching the fences
2014-04-26 23:08:37 -03:00
uint32_t _manual_recovery_start_ms ; // system time in milliseconds that pilot re-took manual control
2016-06-23 03:24:48 -03:00
2021-02-12 23:01:25 -04:00
2019-05-29 10:02:04 -03:00
AC_PolyFence_loader _poly_loader { _total } ; // polygon fence
2013-04-26 06:47:07 -03:00
} ;
2019-01-30 21:54:13 -04:00
namespace AP {
AC_Fence * fence ( ) ;
} ;