diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 17642b3827..6961781dfb 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -303,6 +303,9 @@ Mission::find_offboard_land_start() void Mission::update_onboard_mission() { + /* reset triplets */ + _navigator->reset_triplets(); + if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ if (_onboard_mission.current_seq >= 0 @@ -350,6 +353,9 @@ Mission::update_offboard_mission() { bool failed = true; + /* reset triplets */ + _navigator->reset_triplets(); + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { // The following is not really a warning, but it can be useful to have this message in the log file PX4_WARN("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ba1b0830c6..44d6c9d344 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -189,6 +189,12 @@ public: */ void reset_cruising_speed(); + + /** + * Set triplets to invalid + */ + void reset_triplets(); + /** * Get the target throttle * diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 017c377128..1345ae93e9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -657,10 +657,7 @@ Navigator::task_main() /* 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; - _pos_sp_triplet_updated = true; + reset_triplets(); } if (_pos_sp_triplet_updated) { @@ -815,6 +812,17 @@ Navigator::reset_cruising_speed() _mission_cruising_speed_fw = -1.0f; } +void +Navigator::reset_triplets() +{ + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + _pos_sp_triplet_updated = true; +} + + + float Navigator::get_cruising_throttle() {