AC_Fence: first implemenation of library

libraries/AC_Fence/AC_Fence.cpp

@@ -0,0 +1,191 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL.h>
+#include <AC_Fence.h>
+
+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;
+    }
+}

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 <inttypes.h>
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_Math.h>
+#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
+
+// 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

libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde

@@ -0,0 +1,72 @@
+/*
+ *       Example of AC_WPNav library .
+ *       DIYDrones.com
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
+#include <AP_GPS.h>             // ArduPilot GPS library
+#include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_Baro.h>            // ArduPilot Mega Barometer Library
+#include <Filter.h>
+#include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
+#include <AP_Declination.h>
+#include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_AHRS.h>
+#include <AP_Airspeed.h>
+#include <AC_PID.h>             // PID library
+#include <APM_PI.h>             // PID library
+#include <AP_Buffer.h>          // ArduPilot general purpose FIFO buffer
+#include <AP_InertialNav.h>     // Inertial Navigation library
+#include <AC_Fence.h>           // 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();

libraries/AC_Fence/examples/AC_Fence_test/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk

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
+
+
+