diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7d1c10d051..0227a2ed072 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -392,17 +392,7 @@ Navigator::task_main() home_position_update(); } - /* global position updated */ - if (fds[0].revents & POLLIN) { - global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { - have_geofence_position_data = true; - } - } - - bool updated; orb_check(_vehicle_command_sub, &updated); - if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); @@ -412,6 +402,14 @@ Navigator::task_main() } } + /* global position updated */ + if (fds[0].revents & POLLIN) { + global_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; + } + } + /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data &&