diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 26 |
1 files changed, 16 insertions, 10 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a867dd0da..df620e5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -137,6 +137,7 @@ Navigator::Navigator() : _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -289,8 +290,9 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -426,12 +428,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { @@ -439,11 +444,13 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ + _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { @@ -451,9 +458,11 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: @@ -467,9 +476,9 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -497,8 +506,8 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, + SCHED_PRIORITY_DEFAULT + 20, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); @@ -630,9 +639,6 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } void |