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();
}
