diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index adb1e7f0d7db..bfd494f6c9ce 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -191,12 +191,14 @@ Mission::on_active() bool mission_sub_updated = _mission_sub.updated(); if (mission_sub_updated) { + _navigator->reset_triplets(); update_mission(); } /* reset the current mission if needed */ if (need_to_reset_mission(true)) { reset_mission(_mission); + _navigator->reset_triplets(); update_mission(); _navigator->reset_cruising_speed(); mission_sub_updated = true; @@ -442,9 +444,6 @@ Mission::update_mission() bool failed = true; - /* reset triplets */ - _navigator->reset_triplets(); - /* Reset vehicle_roi * Missions that do not explicitly configure ROI would not override * an existing ROI setting from previous missions */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a30a813fb721..c273b9eac825 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -620,7 +620,6 @@ Navigator::run() case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: - _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = nullptr; _can_loiter_at_sp = false; break; @@ -720,16 +719,8 @@ Navigator::print_status() void Navigator::publish_position_setpoint_triplet() { - // do not publish an invalid setpoint - if (!_pos_sp_triplet.current.valid) { - return; - } - _pos_sp_triplet.timestamp = hrt_absolute_time(); - - /* lazily publish the position setpoint triplet only once available */ _pos_sp_triplet_pub.publish(_pos_sp_triplet); - _pos_sp_triplet_updated = false; }