mirror of https://github.com/ArduPilot/ardupilot
AC_Fence: add backup fence
Also includes fixes from code review with Tridge
This commit is contained in:
parent
0fce0eb488
commit
2d17688363
|
@ -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;
|
||||
return AC_FENCE_TYPE_NONE;
|
||||
}else{
|
||||
return _enabled_fences;
|
||||
}
|
||||
|
@ -86,7 +93,7 @@ bool AC_Fence::pre_arm_check() const
|
|||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
@ -104,48 +111,64 @@ uint8_t AC_Fence::check_fence()
|
|||
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 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -170,7 +193,7 @@ void AC_Fence::record_breach(uint8_t fence_type)
|
|||
}
|
||||
|
||||
// update bitmask
|
||||
_breached_fences = _breached_fences | fence_type;
|
||||
_breached_fences |= fence_type;
|
||||
}
|
||||
|
||||
/// clear_breach - update breach bitmask, time and count
|
||||
|
@ -182,10 +205,23 @@ void AC_Fence::clear_breach(uint8_t fence_type)
|
|||
}
|
||||
|
||||
// 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;
|
||||
}
|
|
@ -10,34 +10,37 @@
|
|||
|
||||
// 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_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
|
||||
|
||||
// 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)
|
||||
|
|
|
@ -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()
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue