diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 12a40909a0..55f001c0a1 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -329,8 +329,9 @@ private: (ParamFloat) _param_mis_yaw_err ) - int _local_pos_sub{-1}; /**< local position subscription */ - int _vehicle_status_sub{-1}; /**< local position subscription */ + int _local_pos_sub{-1}; + int _mission_sub{-1}; + int _vehicle_status_sub{-1}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 06c6cf45a1..469792dbd4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -101,6 +101,7 @@ Navigator::Navigator() : _handle_reverse_delay = param_find("VT_B_REV_DEL"); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _mission_sub = orb_subscribe(ORB_ID(mission)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); reset_triplets(); @@ -110,6 +111,7 @@ Navigator::~Navigator() { perf_free(_loop_perf); orb_unsubscribe(_local_pos_sub); + orb_unsubscribe(_mission_sub); orb_unsubscribe(_vehicle_status_sub); } @@ -144,13 +146,15 @@ Navigator::run() params_update(); /* wakeup source(s) */ - px4_pollfd_struct_t fds[2] {}; + px4_pollfd_struct_t fds[3] {}; /* Setup of loop */ fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; fds[1].fd = _vehicle_status_sub; fds[1].events = POLLIN; + fds[2].fd = _mission_sub; + fds[2].events = POLLIN; /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); @@ -168,18 +172,19 @@ Navigator::run() PX4_ERR("poll error %d, %d", pret, errno); px4_usleep(10000); continue; - - } else { - if (fds[0].revents & POLLIN) { - /* success, local pos is available */ - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - } } perf_begin(_loop_perf); + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus); + if (fds[2].revents & POLLIN) { + // copy mission to clear any update + mission_s mission; + orb_copy(ORB_ID(mission), _mission_sub, &mission); + } + /* gps updated */ if (_gps_pos_sub.updated()) { _gps_pos_sub.copy(&_gps_pos);