From 2bfac7ff4fdfe28f6ad2f2d187cc6b489f9f9b57 Mon Sep 17 00:00:00 2001 From: Michal Stasiak Date: Fri, 21 Oct 2016 13:33:31 +0200 Subject: [PATCH] Mission feasibility: Geofence check fix Current implementation checks against geofence only if it is in polygon form. When it's created via params, it accepts all the waypoints as the number of vertices = 0. Thus, changed the function to the one that is used to check whether geofence is breached in flight. --- src/modules/navigator/geofence.cpp | 5 +++++ src/modules/navigator/geofence.h | 2 ++ .../navigator/mission_feasibility_checker.cpp | 15 ++++++++++----- .../navigator/mission_feasibility_checker.h | 2 +- 4 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 945407a346..1b621dfffa 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -126,6 +126,11 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, } } +bool Geofence::inside(const struct mission_item_s &mission_item) +{ + return inside(mission_item.lat, mission_item.lon, mission_item.altitude); +} + bool Geofence::inside(double lat, double lon, float altitude) { float max_horizontal_distance = _param_max_hor_distance.get(); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 83910134b9..f7c295a50f 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -88,6 +88,8 @@ public: const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, const struct home_position_s home_pos, bool home_position_set); + bool inside(const struct mission_item_s &mission_item); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 1817a14dca..fa9957131e 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -87,7 +87,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p // check if all mission item commands are supported failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed); - failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); + failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { @@ -149,7 +149,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre return resLanding; } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { @@ -162,10 +162,15 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (MissionBlock::item_contains_position(&missionitem) && - !geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { + // Geofence function checks against home altitude amsl + missionitem.altitude = missionitem.altitude_is_relative + ? missionitem.altitude + home_alt + : missionitem.altitude; - mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i); + if (MissionBlock::item_contains_position(&missionitem) && + !geofence.inside(missionitem)) { + + mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1); return false; } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 6c5dda73ab..a0df970aa2 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -62,7 +62,7 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed); bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);