diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5572d9b47422..a85c5e17aeb6 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -258,8 +258,11 @@ void Geofence::_updateFence() // check if current position is inside the fence and vehicle is armed const bool current_position_check_okay = checkCurrentPositionRequirementsForGeofence(polygon); + //check if current mission point inside the geofence + const bool current_mission_check_okay = checkMissionRequirementsForGeofence(polygon); + // discard the polygon if at least one check fails by not incrementing the counter in that case - if (home_check_okay && current_position_check_okay) { + if (home_check_okay && current_position_check_okay && current_mission_check_okay) { ++_num_polygons; } @@ -275,6 +278,30 @@ void Geofence::_updateFence() } } +bool Geofence::checkMissionRequirementsForGeofence(const PolygonInfo &polygon) +{ + mission_s mission; + _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission),sizeof(mission_s)); + bool checks_pass = false; + //check all mission against all geofence + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem = {}; + _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + //missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; + checks_pass = checkPointAgainstPolygonCircle(polygon,missionitem.lat, missionitem.lon, missionitem.altitude); + if (!checks_pass) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence invalid, against mission\t"); + events::send(events::ID("navigator_geofence_invalid_against_mission"), {events::Log::Critical, events::LogInternal::Warning}, + "Geofence invalid, against mission"); + + + break; + } + } + return checks_pass; +} + bool Geofence::checkHomeRequirementsForGeofence(const PolygonInfo &polygon) { bool checks_pass = true; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 3f2ecbba6167..408250df9f1d 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -206,6 +206,12 @@ class Geofence : public ModuleParams */ bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude); + /** + * Check polygon or circle geofence fullfills the requirements relative to the current mission. + * @return true if checks pass + */ + bool checkMissionRequirementsForGeofence(const PolygonInfo &polygon); + /** * Check if a single point is within a polygon or circle * @return true if within polygon or circle