diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 26 |
1 files changed, 22 insertions, 4 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fe1f4893e..b1658a6b8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -92,8 +92,7 @@ Navigator *g_navigator; } Navigator::Navigator() : - - /* state machine transition table */ + SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -122,8 +121,10 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false) + _update_triplet(false), + _param_loiter_radius(this, "LOITER_RAD") { + updateParams(); } Navigator::~Navigator() @@ -189,6 +190,13 @@ Navigator::vehicle_control_mode_update() } void +Navigator::params_update() +{ + parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); +} + +void Navigator::task_main_trampoline(int argc, char *argv[]) { navigator::g_navigator->task_main(); @@ -226,6 +234,7 @@ Navigator::task_main() _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _param_update_sub - orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); @@ -233,6 +242,7 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); + params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -241,7 +251,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[5]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -254,6 +264,8 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; + fds[5].fd = _param_update_sub; + fds[5].events = POLLIN; while (!_task_should_exit) { @@ -278,6 +290,12 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* parameters updated */ + if (fds[5].revents & POLLIN) { + params_update(); + updateParams(); + } + /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); |