diff --git a/libraries/AP_Limits/AP_Limit_Geofence.cpp b/libraries/AP_Limits/AP_Limit_Geofence.cpp index c939f80961..96fc46089f 100644 --- a/libraries/AP_Limits/AP_Limit_Geofence.cpp +++ b/libraries/AP_Limits/AP_Limit_Geofence.cpp @@ -9,37 +9,37 @@ #include 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_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_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_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), + // @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_GROUPINFO("FNC_TOT", 4, AP_Limit_Geofence, _fence_total, 0), AP_GROUPEND @@ -47,16 +47,16 @@ const AP_Param::GroupInfo AP_Limit_Geofence::var_info[] PROGMEM = { 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) + 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(); + update_boundary(); } @@ -64,81 +64,81 @@ AP_Limit_Geofence::AP_Limit_Geofence(uint32_t efs, uint8_t f_wp_s, uint8_t max_f bool AP_Limit_Geofence::triggered() { - // reset trigger before checking - _triggered = false; + // reset trigger before checking + _triggered = false; - // never trigger while disabled - if (!_enabled) return 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)) { + // if Geofence is required and we don't know where we are, trigger. + if (_required && (!_gps || !_gps->fix || !_home || !_current_loc)) { - // TRIGGER - _triggered = true; - } + // TRIGGER + _triggered = true; + } - uint32_t distance; + 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) { + 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 { + // TRIGGER + _triggered = true; + } + } + else { - // COMPLEX GEOFENCE mode + // COMPLEX GEOFENCE mode - // check boundary and update if necessary - if (!_boundary_uptodate) { - update_boundary(); - } + // 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 + // 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 + // TRIGGER + _triggered = true; + } + } else { // boundary incorrect - // If geofence is required and our boundary fence is incorrect, we trigger. - if (_required) { + // If geofence is required and our boundary fence is incorrect, we trigger. + if (_required) { - // TRIGGER - _triggered = true; - } - } - } - return _triggered; + // 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 + 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); + 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; +AP_Int8 AP_Limit_Geofence::fence_total() { + return _fence_total; } // save a fence point @@ -161,7 +161,7 @@ void AP_Limit_Geofence::set_fence_point_with_index(Vector2l &point, uint8_t i) } /* - fence boundaries fetch/store + * fence boundaries fetch/store */ Vector2l AP_Limit_Geofence::get_fence_point_with_index(uint8_t i) { @@ -182,23 +182,23 @@ Vector2l AP_Limit_Geofence::get_fence_point_with_index(uint8_t i) } void AP_Limit_Geofence::update_boundary() { - if (!_simple && _fence_total > 0) { + if (!_simple && _fence_total > 0) { - for (uint8_t i = 0; i < (uint8_t) _fence_total; i++) { - _boundary[i] = get_fence_point_with_index(i); - } + for (uint8_t i = 0; i < (uint8_t) _fence_total; i++) { + _boundary[i] = get_fence_point_with_index(i); + } - _boundary_uptodate = true; + _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; + if (Polygon_complete(&_boundary[1], _fence_total - 1) && + !Polygon_outside(_boundary[0], &_boundary[1], _fence_total - 1)) { + return true; + } else return false; }