diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp new file mode 100644 index 0000000000..298cf0e926 --- /dev/null +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -0,0 +1,191 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AC_Fence::var_info[] PROGMEM = { + // @Param: ENABLE + // @DisplayName: Fence enable/disable + // @Description: Allows you to enable (1) or disable (0) the fence functionality + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0), + + // @Param: TYPE + // @DisplayName: Fence Type + // @Description: Enabled fence types held as bitmask + // @Values: 0:None,1:MaxAltitude,2: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), + + // @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 + // @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 + // @User: Standard + AP_GROUPINFO("ALT_MAX", 3, AC_Fence, _alt_max_cm, AC_FENCE_ALT_MAX_DEFAULT), + + // @Param: RADIUS + // @DisplayName: Circular Fence Radius + // @Description: circle fence radius in cm + // @Units: centimeters + // @Range: 0 65536 + // @User: Standard + AP_GROUPINFO("RADIUS", 4, AC_Fence, _radius_cm, AC_FENCE_RADIUS_DEFAULT), + + AP_GROUPEND +}; + +/// Default constructor. +AC_Fence::AC_Fence(AP_InertialNav* inav, GPS** gps_ptr) : + _inav(inav), + _gps_ptr(gps_ptr) +{ + 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( _radius_cm < 0 ) { + _radius_cm.set_and_save(AC_FENCE_RADIUS_DEFAULT); + } +} + +/// get_enabled_fences - returns bitmask of enabled fences +uint8_t AC_Fence::get_enabled_fences() +{ + if(!_enabled) { + return false; + }else{ + return _enabled_fences; + } +} + +/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully +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) { + return true; + } + + // check no limits are currently breached + 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()) { + return false; + } + + // if we got this far everything must be ok + return true; +} + +/// check_fence - returns the fence type that has been breached (if any) +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) { + return AC_FENCE_TYPE_NONE; + } + + // get current position + Vector3f curr = _inav->get_position(); + + // 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; + } + }else{ + clear_breach(AC_FENCE_TYPE_ALT_MAX); + } + } + + // 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) { + // 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; + } + }else{ + clear_breach(AC_FENCE_TYPE_BIG_CIRCLE); + } + } + + // return any new breaches that have occurred + return ret; + + // To-Do: add min alt and polygon check + //outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1); +} + +/// 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 + if( _breached_fences == AC_FENCE_TYPE_NONE ) { + _breach_time = hal.scheduler->millis(); + } + + // update breach count + if( _breach_count < 65500) { + _breach_count++; + } + + // update bitmask + _breached_fences = _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 ) { + return; + } + + // update bitmask + _breached_fences = _breached_fences & ~fence_type; + + // if all breaches cleared, clear the breach time + if( _breached_fences == AC_FENCE_TYPE_NONE ) { + _breach_time = 0; + } +} \ No newline at end of file diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h new file mode 100644 index 0000000000..4378ad7fb3 --- /dev/null +++ b/libraries/AC_Fence/AC_Fence.h @@ -0,0 +1,100 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#ifndef AC_FENCE_H +#define AC_FENCE_H + +#include +#include +#include +#include +#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 + +// 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 + +class AC_Fence +{ +public: + + /// Constructor + AC_Fence(AP_InertialNav* inav, GPS** gps_ptr); + + /// 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; } + + /// get_enabled_fences - returns bitmask of enabled fences + uint8_t get_enabled_fences(); + + /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully + bool pre_arm_check() const; + + /// + /// methods to check we are within the boundaries and recover + /// + + /// check_fence - returns the fence type that has been breached (if any) + uint8_t check_fence(); + + /// 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 + uint32_t get_breach_count() const { return _breach_count; } + + /// get_action - getter for user requested action on limit breach + uint8_t get_action() const { return _action.get(); } + + /// + /// 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; } + + static const struct AP_Param::GroupInfo var_info[]; + +private: + + /// 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); + + // pointers to other objects we depend upon + AP_InertialNav* _inav; + GPS** _gps_ptr; // pointer to pointer to gps + + // parameters + 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 + + // other internal variables + float _home_distance_cm; // distance from home in cm (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) + uint32_t _breach_time; // time of last breach in milliseconds + uint16_t _breach_count; // number of times we have breached the fence +}; +#endif // AC_FENCE_H 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 new file mode 100644 index 0000000000..0562cb7ad6 --- /dev/null +++ b/libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde @@ -0,0 +1,72 @@ +/* + * Example of AC_WPNav library . + * DIYDrones.com + */ + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include +#include +#include + +#include // ArduPilot GPS library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega Barometer Library +#include +#include // ArduPilot Mega Magnetometer Library +#include +#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include +#include +#include // PID library +#include // PID library +#include // ArduPilot general purpose FIFO buffer +#include // Inertial Navigation library +#include // Fence library + +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; + +// INS and Baro declaration +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 + +#define A_LED_PIN 27 +#define C_LED_PIN 25 +AP_InertialSensor_MPU6000 ins; +AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi); + +#else + +#define A_LED_PIN 37 +#define C_LED_PIN 35 +AP_ADC_ADS7844 adc; +AP_InertialSensor_Oilpan ins(&adc); +AP_Baro_BMP085 baro; +#endif + +// GPS declaration +GPS *gps; +AP_GPS_Auto auto_gps(&gps); + +AP_Compass_HMC5843 compass; +AP_AHRS_DCM ahrs(&ins, gps); + +// Inertial Nav declaration +AP_InertialNav inertial_nav(&ahrs, &ins, &baro, &gps); + +// Fence +AC_Fence fence(&inertial_nav, &gps); + +void setup() +{ + hal.console->println("AC_Fence library test"); +} + +void loop() +{ + // call update function + hal.console->printf_P(PSTR("hello")); + hal.scheduler->delay(1); +} + +AP_HAL_MAIN(); diff --git a/libraries/AC_Fence/examples/AC_Fence_test/Makefile b/libraries/AC_Fence/examples/AC_Fence_test/Makefile new file mode 100644 index 0000000000..f5daf25151 --- /dev/null +++ b/libraries/AC_Fence/examples/AC_Fence_test/Makefile @@ -0,0 +1 @@ +include ../../../../mk/apm.mk diff --git a/libraries/AC_Fence/examples/AC_Fence_test/nocore.inoflag b/libraries/AC_Fence/examples/AC_Fence_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AC_Fence/keywords.txt b/libraries/AC_Fence/keywords.txt new file mode 100644 index 0000000000..54b4e294f2 --- /dev/null +++ b/libraries/AC_Fence/keywords.txt @@ -0,0 +1,14 @@ +AC_Fence KEYWORD1 +enable KEYWORD2 +enabled KEYWORD2 +get_enabled_fences KEYWORD2 +pre_arm_check KEYWORD2 +check_fence KEYWORD2 +get_breaches KEYWORD2 +get_breach_time KEYWORD2 +get_breach_count KEYWORD2 +get_action KEYWORD2 +set_home_distance KEYWORD2 + + +