zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_Fence: add backup fence 

Also includes fixes from code review with Tridge
master
Randy Mackay 12 years ago
parent
0fce0eb488
commit
2d17688363
3 changed files with 127 additions and 77 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 152
      libraries/AC_Fence/AC_Fence.cpp
  2. 50
      libraries/AC_Fence/AC_Fence.h
  3. 2
      libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde

152
libraries/AC_Fence/AC_Fence.cpp
Unescape View File

@ -15,58 +15,65 @@ const AP_Param::GroupInfo AC_Fence::var_info[] PROGMEM = { @@ -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() @@ -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() @@ -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() @@ -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;
}

50
libraries/AC_Fence/AC_Fence.h
Unescape View File

@ -9,35 +9,38 @@ @@ -9,35 +9,38 @@
#include <AP_InertialNav.h> // 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: @@ -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: @@ -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: @@ -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)

2
libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde
Unescape View File

@ -55,7 +55,7 @@ AP_AHRS_DCM ahrs(&ins, gps); @@ -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()
{

Write Preview
Loading…
Cancel
Save