From d53dccc319b2d1985f09ddc626d3bb94d10e90b8 Mon Sep 17 00:00:00 2001 From: daniele Date: Wed, 25 Mar 2020 16:45:13 +0100 Subject: [PATCH] reposition (goto): add check if goto location is outside the geofence Before this check we were free to fly outside of a geofence with a goto command. With this check we ignore the command if the location of the goto is outside the fence. If this is the case we send back a mavlink warning to the ground station. --- src/modules/navigator/navigator_main.cpp | 29 +++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a049a8f998..525cff48be 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -228,7 +228,31 @@ Navigator::run() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { - if (true) { + bool reposition_valid = true; + + if (have_geofence_position_data && + ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) { + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + + vehicle_global_position_s test_reposition_validity {}; + test_reposition_validity.lat = cmd.param5; + test_reposition_validity.lon = cmd.param6; + + if (PX4_ISFINITE(cmd.param7)) { + test_reposition_validity.alt = cmd.param7; + + } else { + test_reposition_validity.alt = get_global_position()->alt; + } + + reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos, + home_position_valid()); + } + } + + if (reposition_valid) { position_setpoint_triplet_s *rep = get_reposition_triplet(); position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); @@ -299,6 +323,9 @@ Navigator::run() rep->current.timestamp = hrt_absolute_time(); rep->next.valid = false; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence"); } // CMD_DO_REPOSITION is acknowledged by commander