diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 1a2c4395ae..8cc2c2e9e3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -244,39 +244,13 @@ void Geofence::_updateFence() } } -bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) + +bool Geofence::checkPointAgainstAllGeofences(double lat, double lon, float altitude) { - return checkAll(global_position.lat, global_position.lon, global_position.alt); -} - -bool Geofence::checkAll(double lat, double lon, float altitude) -{ - bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude); - - inside_fence = inside_fence && isBelowMaxAltitude(altitude); - - // to be inside the geofence both fences have to report being inside - // as they both report being inside when not enabled - inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude); - + const bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude) && isBelowMaxAltitude(altitude) + && isInsidePolygonOrCircle(lat, lon, altitude); return inside_fence; } -} - -bool Geofence::check(const vehicle_global_position_s &global_position, const sensor_gps_s &gps_position) -{ - if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { - return checkAll(global_position); - - } else { - return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, gps_position.altitude_msl_m); - } -} - -bool Geofence::check(const struct mission_item_s &mission_item) -{ - return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude); -} bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 329edf6ca8..5b9f755d9b 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -81,28 +81,6 @@ public: */ void updateFence(); - /** - * Return whether the system obeys the geofence. - * - * @return true: system is obeying fence, false: system is violating fence - */ - bool check(const vehicle_global_position_s &global_position, const sensor_gps_s &gps_position); - - /** - * Return whether a mission item obeys the geofence. - * - * @return true: system is obeying fence, false: system is violating fence - */ - bool check(const struct mission_item_s &mission_item); - - /** - * Check if a point passes the Geofence test. - * In addition to checkPolygons(), this takes all additional parameters into account. - * - * @return false for a geofence violation - */ - bool checkAll(double lat, double lon, float altitude); - bool isCloserThanMaxDistToHome(double lat, double lon, float altitude); bool isBelowMaxAltitude(float altitude); @@ -210,10 +188,6 @@ private: */ bool checkPolygons(double lat, double lon, float altitude); - - - bool checkAll(const vehicle_global_position_s &global_position); - /** * Check if a single point is within a polygon * @return true if within polygon diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 75ba23391e..b67a85224f 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -137,7 +137,8 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, // Geofence function checks against home altitude amsl missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; - if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) { + if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().checkPointAgainstAllGeofences( + missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1); events::send(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info}, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 373721f89c..675cf8c286 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1469,8 +1469,8 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos) if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN)) { - if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon)) { - return _geofence.check(pos, _gps_pos); + if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon) && PX4_ISFINITE(pos.alt)) { + return _geofence.checkPointAgainstAllGeofences(pos.lat, pos.lon, pos.alt); } }