zr-v4/libraries/AP_Limits/AP_Limit_Geofence.cpp

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

/// @file	limits.cpp
/// @brief	Imposes limits on location (geofence), altitude and other parameters.
///         Each limit breach will trigger an action or set of actions to recover. Adapted from geofence.
/// @author Andrew Tridgell
///         Andreas Antonopoulos

#include <AP_Limit_Geofence.h>

const AP_Param::GroupInfo AP_Limit_Geofence::var_info[] PROGMEM = {
    // @Param: FNC_ON
    // @DisplayName: Enable Geofence
    // @Description: Setting this to Enabled(1) will enable the geofence. Setting this to Disabled(0) will disable the geofence
    // @Values: 0:Disabled,1:Enabled
    // @User: Standard
    AP_GROUPINFO("FNC_ON",  0,      AP_Limit_Geofence,      _enabled, 0),

    // @Param: FNC_REQ
    // @DisplayName: Require Geofence
    // @Description: Setting this to Enabled(1) will make being inside the geofence a required check before arming the vehicle.
    // @Values: 0:Disabled,1:Enabled
    // @User: Standard
    AP_GROUPINFO("FNC_REQ", 1,      AP_Limit_Geofence,      _required, 0),

    // @Param: FNC_SMPL
    // @DisplayName: Require Geofence
    // @Description: "Simple" geofence (enabled - 1) is based on a radius from the home position, "Complex" (disabled - 0) define a complex fence by lat/long positions
    // @Values: 0:Disabled,1:Enabled
    // @User: Standard
    AP_GROUPINFO("FNC_SMPL", 2,     AP_Limit_Geofence,      _simple, 0),

    // @Param: FNC_RAD
    // @DisplayName: Require Geofence
    // @Description: Radius of fenced area in meters. A value of 20 creates a 20-meter radius circle (40-meter diameter) from the home point.
    // @Units: Meters
    // @Range: 0 32767
    // @Increment: 1
    // @User: Standard
    AP_GROUPINFO("FNC_RAD", 3,      AP_Limit_Geofence,      _radius, 0),

    AP_GROUPINFO("FNC_TOT", 4,      AP_Limit_Geofence,      _fence_total, 0),
    AP_GROUPEND


};


AP_Limit_Geofence::AP_Limit_Geofence(uint32_t efs, uint8_t f_wp_s, uint8_t max_fp, GPS *&gps, struct Location *h_loc, struct Location *c_loc) :
    AP_Limit_Module(AP_LIMITS_GEOFENCE),
    _gps(gps),
    _current_loc(c_loc),
    _home(h_loc),
    _eeprom_fence_start(efs),
    _fence_wp_size(f_wp_s),
    _max_fence_points(max_fp),
    _boundary_uptodate(false)
{
    update_boundary();
}




bool AP_Limit_Geofence::triggered() {

    // reset trigger before checking
    _triggered = false;

    // never trigger while disabled
    if (!_enabled) return false;

    // if Geofence is required and we don't know where we are, trigger.
    if (_required && (!_gps || !_gps->fix || !_home || !_current_loc)) {

        // TRIGGER
        _triggered = true;
    }

    uint32_t distance;

    if (_simple && _current_loc && _home) {     // simple mode, pointers to current and home exist.
        distance = (uint32_t) get_distance_meters(_current_loc, _home);
        if (distance > 0 &&  distance > (uint16_t) _radius) {

            // TRIGGER
            _triggered = true;
        }
    }
    else {

        // COMPLEX GEOFENCE  mode

        // check boundary and update if necessary
        if (!_boundary_uptodate) {
            update_boundary();
        }

        // if boundary is correct, and current_loc exists check if we are inside the fence.
        if (boundary_correct() && _current_loc) {
            Vector2l location;
            location.x = _current_loc->lat;
            location.y = _current_loc->lng;
            if (Polygon_outside(location, &_boundary[1], _fence_total-1)) {            // trigger if outside

                // TRIGGER
                _triggered = true;
            }
        } else {         // boundary incorrect

            // If geofence is required and our boundary fence is incorrect, we trigger.
            if (_required) {

                // TRIGGER
                _triggered = true;
            }
        }
    }
    return _triggered;
}



uint32_t get_distance_meters(struct Location *loc1, struct Location *loc2)  // distance in meters between two locations
{
    if (!loc1 || !loc2)
        return -1;         // pointers missing (dangling)
    if(loc1->lat == 0 || loc1->lng == 0)
        return -1;         // do not trigger a false positive on erroneous location data
    if(loc2->lat == 0 || loc2->lng == 0)
        return -1;         // do not trigger a false positive on erroneous location data

    float dlat      = (float)(loc2->lat - loc1->lat);
    float dlong = (float)(loc2->lng - loc1->lng);
    return (sqrt(sq(dlat) + sq(dlong)) * .01113195);
}



AP_Int8 AP_Limit_Geofence::fence_total() {
    return _fence_total;
}

// save a fence point
void AP_Limit_Geofence::set_fence_point_with_index(Vector2l &point, uint8_t i)
{
    uint32_t mem;

    if (i >= (unsigned)fence_total()) {
        // not allowed
        return;
    }

    mem = _eeprom_fence_start + (i * _fence_wp_size);

    eeprom_write_dword((uint32_t *)mem, point.x);
    mem += sizeof(uint32_t);
    eeprom_write_dword((uint32_t *)mem, point.y);

    _boundary_uptodate = false;
}

/*
 *  fence boundaries fetch/store
 */
Vector2l AP_Limit_Geofence::get_fence_point_with_index(uint8_t i)
{
    uint32_t mem;
    Vector2l ret;

    if (i > (unsigned) fence_total()) {
        return Vector2l(0,0);
    }

    // read fence point
    mem = _eeprom_fence_start + (i * _fence_wp_size);
    ret.x = eeprom_read_dword((uint32_t *)mem);
    mem += sizeof(uint32_t);
    ret.y = eeprom_read_dword((uint32_t *)mem);

    return ret;
}

void AP_Limit_Geofence::update_boundary() {
    if (!_simple && _fence_total > 0) {

        for (uint8_t i = 0; i < (uint8_t) _fence_total; i++) {
            _boundary[i] = get_fence_point_with_index(i);
        }

        _boundary_uptodate = true;

    }
}

bool AP_Limit_Geofence::boundary_correct() {

    if (Polygon_complete(&_boundary[1], _fence_total - 1) &&
        !Polygon_outside(_boundary[0], &_boundary[1], _fence_total - 1)) {
        return true;
    } else return false;
}