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; // simple mode, pointers to current and home exist. if (_simple && _current_loc && _home) { distance = (uint32_t) get_distance(*_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; // trigger if outside if (Polygon_outside(location, &_boundary[1], _fence_total-1)) { // 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; }
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), _eeprom_fence_start(efs), _fence_wp_size(f_wp_s), _max_fence_points(max_fp), _boundary_uptodate(false), _current_loc(c_loc), _home(h_loc), _gps(gps) { update_boundary(); }
AP_Limit_Geofence::AP_Limit_Geofence(uint16_t efs, uint8_t f_wp_s, GPS *&gps, const struct Location *h_loc, const struct Location *c_loc) : AP_Limit_Module(AP_LIMITS_GEOFENCE), _gps(gps), _current_loc(c_loc), _home(h_loc), _fence_wp_size(f_wp_s), _boundary_uptodate(false) { AP_Param::setup_object_defaults(this, var_info); update_boundary(); }