Example #1
0
// check_fence_polygon - returns true if the polygon fence is freshly breached
bool AC_Fence::check_fence_polygon()
{
    if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) {
        // not enabled; no breach
        return false;
    }

    // check consistency of number of points
    if (_boundary_num_points != _total) {
        // Fence is currently not completely loaded.  Can't breach it?!
        _boundary_loaded = false;
        load_polygon_from_eeprom();
        return false;
    }
    if (!_boundary_valid) {
        // fence isn't valid - can't breach it?!
        return false;
    }

    // check if vehicle is outside the polygon fence
    Vector2f position;
    if (!_ahrs.get_relative_position_NE_origin(position)) {
        // we have no idea where we are; can't breach the fence
        return false;
    }

    position = position * 100.0f;  // m to cm
    if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) {
        // check if this is a new breach
        if (_breached_fences & AC_FENCE_TYPE_POLYGON) {
            // not a new breach
            return false;
        }
        // record that we have breached the polygon
        record_breach(AC_FENCE_TYPE_POLYGON);
        return true;
    }

    // inside boundary; clear breach if present
    if (_breached_fences & AC_FENCE_TYPE_POLYGON) {
        clear_breach(AC_FENCE_TYPE_POLYGON);
    }

    return false;
}
Example #2
0
/// check_fence - returns the fence type that has been breached (if any)
///     curr_alt is the altitude above home in meters
uint8_t AC_Fence::check_fence(float curr_alt)
{
    uint8_t ret = AC_FENCE_TYPE_NONE;

    // return immediately if disabled
    if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) {
        return AC_FENCE_TYPE_NONE;
    }

    // check if pilot is attempting to recover manually
    if (_manual_recovery_start_ms != 0) {
        // we ignore any fence breaches during the manual recovery period which is about 10 seconds
        if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
            return AC_FENCE_TYPE_NONE;
        } else {
            // recovery period has passed so reset manual recovery time and continue with fence breach checks
            _manual_recovery_start_ms = 0;
        }
    }

    // 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 || (!is_zero(_alt_max_backup) && curr_alt >= _alt_max_backup)) {

                // record that we have breached the upper limit
                record_breach(AC_FENCE_TYPE_ALT_MAX);
                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.0f;
                _alt_max_breach_distance = 0.0f;
            }
        }
    }

    // circle fence check
    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 || (!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) {

                // record that we have breached the circular distance limit
                record_breach(AC_FENCE_TYPE_CIRCLE);
                ret |= AC_FENCE_TYPE_CIRCLE;

                // create a backup fence 20m further out
                _circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE;
            }
        }else{
            // clear circle breach if present
            if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) != 0) {
                clear_breach(AC_FENCE_TYPE_CIRCLE);
                _circle_radius_backup = 0.0f;
                _circle_breach_distance = 0.0f;
            }
        }
    }

    // polygon fence check
    if ((_enabled_fences & AC_FENCE_TYPE_POLYGON) != 0 ) {
        // check consistency of number of points
        if (_boundary_num_points != _total) {
            _boundary_loaded = false;
        }
        // load fence if necessary
        if (!_boundary_loaded) {
            load_polygon_from_eeprom();
        } else if (_boundary_valid) {
            // check if vehicle is outside the polygon fence
            const Vector3f& position = _inav.get_position();
            if (_poly_loader.boundary_breached(Vector2f(position.x, position.y), _boundary_num_points, _boundary, true)) {
                // check if this is a new breach
                if ((_breached_fences & AC_FENCE_TYPE_POLYGON) == 0) {
                    // record that we have breached the polygon
                    record_breach(AC_FENCE_TYPE_POLYGON);
                    ret |= AC_FENCE_TYPE_POLYGON;
                }
            } else {
                // clear breach if present
                if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
                    clear_breach(AC_FENCE_TYPE_POLYGON);
                }
            }
        }
    }

    // return any new breaches that have occurred
    return ret;
}