From 2d176883635f9892421dfb162660547a12810879 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 May 2013 17:05:04 +0900 Subject: [PATCH] AC_Fence: add backup fence Also includes fixes from code review with Tridge --- libraries/AC_Fence/AC_Fence.cpp | 152 +++++++++++------- libraries/AC_Fence/AC_Fence.h | 50 +++--- .../examples/AC_Fence_test/AC_Fence_test.pde | 2 +- 3 files changed, 127 insertions(+), 77 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 298cf0e926..352df39d06 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -15,58 +15,65 @@ const AP_Param::GroupInfo AC_Fence::var_info[] PROGMEM = { // @Param: TYPE // @DisplayName: Fence Type // @Description: Enabled fence types held as bitmask - // @Values: 0:None,1:MaxAltitude,2:Circle + // @Values: 0:None,1:Altitude,2:Circle,3:Altitude and Circle // @User: Standard - AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_BIG_CIRCLE), + AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE), // @Param: ACTION // @DisplayName: Action to perform when the limit is breached // @Description: What to do on fence breach - // @Values: 0:Report Only,1:Bounce,3:Return-to-Launch,4:Move to location + // @Values: 0:Report Only,1:RTL or Land // @User: Standard AP_GROUPINFO("ACTION", 2, AC_Fence, _action, AC_FENCE_ACTION_RTL_AND_LAND), // @Param: ALT_MAX // @DisplayName: Fence Maximum Altitude // @Description: Maximum altitude allowed before geofence triggers - // @Units: centimeters - // @Range: 1000 100000 - // @Increment: 100 + // @Units: Meters + // @Range: 10 1000 + // @Increment: 1 // @User: Standard - AP_GROUPINFO("ALT_MAX", 3, AC_Fence, _alt_max_cm, AC_FENCE_ALT_MAX_DEFAULT), + AP_GROUPINFO("ALT_MAX", 3, AC_Fence, _alt_max, AC_FENCE_ALT_MAX_DEFAULT), // @Param: RADIUS // @DisplayName: Circular Fence Radius - // @Description: circle fence radius in cm - // @Units: centimeters - // @Range: 0 65536 + // @Description: Circle fence radius which when breached will cause an RTL + // @Units: Meters + // @Range: 0 10000 // @User: Standard - AP_GROUPINFO("RADIUS", 4, AC_Fence, _radius_cm, AC_FENCE_RADIUS_DEFAULT), + AP_GROUPINFO("RADIUS", 4, AC_Fence, _circle_radius, AC_FENCE_CIRCLE_RADIUS_DEFAULT), AP_GROUPEND }; /// Default constructor. -AC_Fence::AC_Fence(AP_InertialNav* inav, GPS** gps_ptr) : +AC_Fence::AC_Fence(AP_InertialNav* inav) : _inav(inav), - _gps_ptr(gps_ptr) + _alt_max_backup(0), + _circle_radius_backup(0), + _alt_max_breach_distance(0), + _circle_breach_distance(0), + _home_distance(0), + _breached_fences(AC_FENCE_TYPE_NONE), + _breach_time(0), + _breach_count(0) { AP_Param::setup_object_defaults(this, var_info); // check for silly fence values - if( _alt_max_cm < 0 ) { - _alt_max_cm.set_and_save(AC_FENCE_ALT_MAX_DEFAULT); + if (_alt_max < 0) { + _alt_max.set_and_save(AC_FENCE_ALT_MAX_DEFAULT); } - if( _radius_cm < 0 ) { - _radius_cm.set_and_save(AC_FENCE_RADIUS_DEFAULT); + if (_circle_radius < 0) { + _circle_radius.set_and_save(AC_FENCE_CIRCLE_RADIUS_DEFAULT); } } /// get_enabled_fences - returns bitmask of enabled fences -uint8_t AC_Fence::get_enabled_fences() +uint8_t AC_Fence::get_enabled_fences() const { - if(!_enabled) { - return false; + if (!_enabled) { + return AC_FENCE_TYPE_NONE; }else{ return _enabled_fences; } @@ -76,17 +83,17 @@ uint8_t AC_Fence::get_enabled_fences() bool AC_Fence::pre_arm_check() const { // if not enabled or not fence set-up always return true - if(!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { + if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { return true; } // check no limits are currently breached - if(_breached_fences != AC_FENCE_TYPE_NONE) { + if (_breached_fences != AC_FENCE_TYPE_NONE) { return false; } // if we have horizontal limits enabled, check inertial nav position is ok - if((_enabled_fences & (AC_FENCE_TYPE_CIRCLE|AC_FENCE_TYPE_BIG_CIRCLE))>0 && !_inav->position_ok()) { + if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE)!=0 && !_inav->position_ok()) { return false; } @@ -100,52 +107,68 @@ uint8_t AC_Fence::check_fence() uint8_t ret = AC_FENCE_TYPE_NONE; // return immediately if disabled - if(!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { + if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { return AC_FENCE_TYPE_NONE; } - // get current position - Vector3f curr = _inav->get_position(); + // get current altitude in meters + float curr_alt = _inav->get_position().z * 0.01f; + + // altitude fence check + if ((_enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) { + + // check if we are over the altitude fence + if( curr_alt >= _alt_max ) { + + // record distance above breach + _alt_max_breach_distance = curr_alt - _alt_max; + + // check for a new breach or a breach of the backup fence + if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) == 0 || (_alt_max_backup != 0 && curr_alt >= _alt_max_backup)) { - // check max altitude - if( (_enabled_fences & AC_FENCE_TYPE_ALT_MAX) > 0 ) { - if(curr.z >= _alt_max_cm ) { - // ensure it's a new breach - if((_breached_fences & AC_FENCE_TYPE_ALT_MAX) == 0) { // record that we have breached the upper limit record_breach(AC_FENCE_TYPE_ALT_MAX); ret = ret | AC_FENCE_TYPE_ALT_MAX; + + // create a backup fence 20m higher up + _alt_max_backup = curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE; } }else{ - clear_breach(AC_FENCE_TYPE_ALT_MAX); + // clear alt breach if present + if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) != 0) { + clear_breach(AC_FENCE_TYPE_ALT_MAX); + _alt_max_backup = 0; + _alt_max_breach_distance = 0; + } } } // circle fence check - if( (_enabled_fences & AC_FENCE_TYPE_CIRCLE) > 0 ) { - if( _home_distance_cm >= _radius_cm ) { - // ensure it's a new breach - if((_breached_fences & AC_FENCE_TYPE_CIRCLE) == 0) { + if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) != 0 ) { + + // 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) == 0 || (_circle_radius_backup != 0 && _home_distance >= _circle_radius_backup)) { + // record that we have breached the circular distance limit record_breach(AC_FENCE_TYPE_CIRCLE); ret = ret | AC_FENCE_TYPE_CIRCLE; - } - }else{ - clear_breach(AC_FENCE_TYPE_CIRCLE); - } - } - // big circle fence check - if( (_enabled_fences & AC_FENCE_TYPE_BIG_CIRCLE) > 0 ) { - if( _home_distance_cm >= _radius_cm * 2 ) { - // ensure it's a new breach - if((_breached_fences & AC_FENCE_TYPE_BIG_CIRCLE) == 0) { - // record that we have breached the circular distance limit - record_breach(AC_FENCE_TYPE_BIG_CIRCLE); - ret = ret | AC_FENCE_TYPE_BIG_CIRCLE; + // create a backup fence 20m further out + _circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE; } }else{ - clear_breach(AC_FENCE_TYPE_BIG_CIRCLE); + // clear circle breach if present + if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) != 0) { + clear_breach(AC_FENCE_TYPE_CIRCLE); + _circle_radius_backup = 0; + _circle_breach_distance = 0; + } } } @@ -160,32 +183,45 @@ uint8_t AC_Fence::check_fence() void AC_Fence::record_breach(uint8_t fence_type) { // if we haven't already breached a limit, update the breach time - if( _breached_fences == AC_FENCE_TYPE_NONE ) { + if (_breached_fences == AC_FENCE_TYPE_NONE) { _breach_time = hal.scheduler->millis(); } // update breach count - if( _breach_count < 65500) { + if (_breach_count < 65500) { _breach_count++; } // update bitmask - _breached_fences = _breached_fences | fence_type; + _breached_fences |= fence_type; } /// clear_breach - update breach bitmask, time and count void AC_Fence::clear_breach(uint8_t fence_type) { // return immediately if this fence type was not breached - if( (_breached_fences & fence_type) == 0 ) { + if ((_breached_fences & fence_type) == 0) { return; } // update bitmask - _breached_fences = _breached_fences & ~fence_type; + _breached_fences &= ~fence_type; +} - // if all breaches cleared, clear the breach time - if( _breached_fences == AC_FENCE_TYPE_NONE ) { - _breach_time = 0; +/// get_breach_distance - returns distance in meters outside of the given fence +float AC_Fence::get_breach_distance(uint8_t fence_type) const +{ + switch (fence_type) { + case AC_FENCE_TYPE_ALT_MAX: + return _alt_max_breach_distance; + break; + case AC_FENCE_TYPE_CIRCLE: + return _circle_breach_distance; + break; + case AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE: + return max(_alt_max_breach_distance,_circle_breach_distance); } + + // we don't recognise the fence type so just return 0 + return 0; } \ No newline at end of file diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 4378ad7fb3..1bfcb6789b 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -9,35 +9,38 @@ #include // Inertial Navigation library // bit masks for enabled fence types. Used for TYPE parameter -#define AC_FENCE_TYPE_NONE 0 // fence disabled -#define AC_FENCE_TYPE_ALT_MAX 1 // max alt fence enabled -#define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence -#define AC_FENCE_TYPE_BIG_CIRCLE 4 // circular horizonal fence double the normal radius used to force land when all hope of RTL is lost +#define AC_FENCE_TYPE_NONE 0 // fence disabled +#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) // 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 -#define AC_FENCE_ACTION_RTL_AND_LAND 1 // return to launch and, if that fails, land +#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 // default boundaries -#define AC_FENCE_ALT_MAX_DEFAULT 15000 // default max altitude is 150m -#define AC_FENCE_RADIUS_DEFAULT 30000 // default circular fence radius is 300m -#define AC_FENCE_SAFETY_MARGIN_DEFAULT 200.0f // default distance within the fence to move to after bouncing off a wall +#define AC_FENCE_ALT_MAX_DEFAULT 150.0f // default max altitude is 150m +#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_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further out + +// 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 class AC_Fence { public: /// Constructor - AC_Fence(AP_InertialNav* inav, GPS** gps_ptr); + AC_Fence(AP_InertialNav* inav); /// enable - allows fence to be enabled/disabled. Note: this does not update the eeprom saved value void enable(bool true_false) { _enabled = true_false; } /// enabled - returns true if fence is enabled - bool enabled() { return _enabled; } + bool enabled() const { return _enabled; } /// get_enabled_fences - returns bitmask of enabled fences - uint8_t get_enabled_fences(); + uint8_t get_enabled_fences() const; /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully bool pre_arm_check() const; @@ -56,7 +59,10 @@ public: uint32_t get_breach_time() const { return _breach_time; } /// get_breach_count - returns number of times we have breached the fence - uint32_t get_breach_count() const { return _breach_count; } + uint16_t get_breach_count() const { return _breach_count; } + + /// get_breach_distance - returns distance in meters outside of the given fence + float get_breach_distance(uint8_t fence_type) const; /// get_action - getter for user requested action on limit breach uint8_t get_action() const { return _action.get(); } @@ -65,8 +71,8 @@ public: /// time saving methods to piggy-back on main code's calculations /// - /// set_home_distance - update home distance - required for circular horizontal fence monitoring - void set_home_distance(float distance_cm) { _home_distance_cm = distance_cm; } + /// set_home_distance - update vehicle's distance from home in meters - required for circular horizontal fence monitoring + void set_home_distance(float distance) { _home_distance = distance; } static const struct AP_Param::GroupInfo var_info[]; @@ -86,11 +92,19 @@ private: AP_Int8 _enabled; // top level enable/disable control AP_Int8 _enabled_fences; // bit mask holding which fences are enabled AP_Int8 _action; // recovery action specified by user - AP_Float _alt_max_cm; // altitude upper limit in cm - AP_Float _radius_cm; // circle fence radius in cm + AP_Float _alt_max; // altitude upper limit in meters + AP_Float _circle_radius; // circle fence radius in meters + + // 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 _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 _circle_breach_distance; // distance above the altitude max // other internal variables - float _home_distance_cm; // distance from home in cm (provided by main code) + 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) diff --git a/libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde b/libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde index 0562cb7ad6..6c6d2a5450 100644 --- a/libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde +++ b/libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde @@ -55,7 +55,7 @@ AP_AHRS_DCM ahrs(&ins, gps); AP_InertialNav inertial_nav(&ahrs, &ins, &baro, &gps); // Fence -AC_Fence fence(&inertial_nav, &gps); +AC_Fence fence(&inertial_nav); void setup() {