From 11c5e6ff617c4870e98b281154156d95098a68cf Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 09:29:08 +0100 Subject: [PATCH 01/10] mission_base: Do not initialize mission from dataman. only listen on mission topic --- src/modules/navigator/mission_base.cpp | 38 ++++++++------------------ 1 file changed, 11 insertions(+), 27 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 360ed16ad469..25cdfa75b6e1 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -52,38 +52,22 @@ MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed _dataman_cache_size_signed(dataman_cache_size_signed) { _dataman_cache.resize(abs(dataman_cache_size_signed)); - _is_current_planned_mission_item_valid = (initMission() == PX4_OK); - updateDatamanCache(); + // Reset _mission here, and listen on changes on the uorb topic instead of initialize from dataman. + _mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + _mission.fence_dataman_id = DM_KEY_FENCE_POINTS_0; + _mission.safepoint_dataman_id = DM_KEY_SAFE_POINTS_0; + _mission.count = 0; + _mission.current_seq = 0; + _mission.land_start_index = -1; + _mission.land_index = -1; + _mission.mission_id = 0; + _mission.geofence_id = 0; + _mission.safe_points_id = 0; _mission_pub.advertise(); } -int MissionBase::initMission() -{ - mission_s mission; - int ret_val{PX4_ERROR}; - - bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), - sizeof(mission_s)); - - if (success) { - if (isMissionValid(mission)) { - _mission = mission; - ret_val = PX4_OK; - - } else { - resetMission(); - } - - } else { - PX4_ERR("Could not initialize Mission: Dataman read failed"); - resetMission(); - } - - return ret_val; -} - void MissionBase::updateDatamanCache() { From c28ad866a57ae6bf1546d9f51796c1addb5adbf6 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 09:31:34 +0100 Subject: [PATCH 02/10] mission_base: clean up mission check evaluation --- src/modules/navigator/mission_base.cpp | 39 ++++++++++++++------------ src/modules/navigator/mission_base.h | 7 ++--- 2 files changed, 24 insertions(+), 22 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 25cdfa75b6e1..185bba939006 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -104,29 +104,25 @@ void MissionBase::updateMavlinkMission() _mission = new_mission; - _is_current_planned_mission_item_valid = isMissionValid(_mission); - /* Relevant mission items updated externally*/ if (mission_data_changed) { onMissionUpdate(mission_items_changed); } + + _is_current_planned_mission_item_valid = isMissionValid(); } } void MissionBase::onMissionUpdate(bool has_mission_items_changed) { - _is_current_planned_mission_item_valid = _mission.count > 0; - if (has_mission_items_changed) { _dataman_cache.invalidate(); _load_mission_index = -1; - check_mission_valid(); - - // only warn if the check failed on merit - if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) { - PX4_WARN("mission check failed"); + if (_navigator->home_global_position_valid()) { + _initialized_mission_checked = true; + check_mission_valid(); } } @@ -159,8 +155,9 @@ MissionBase::on_inactive() /* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */ if (_navigator->home_global_position_valid() && !_initialized_mission_checked) { - check_mission_valid(); _initialized_mission_checked = true; + check_mission_valid(); + _is_current_planned_mission_item_valid = isMissionValid(); } if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { @@ -662,7 +659,7 @@ MissionBase::checkMissionRestart() && ((_mission.current_seq + 1) == _mission.count)) { setMissionIndex(0); _inactivation_index = -1; // reset - _is_current_planned_mission_item_valid = isMissionValid(_mission); + _is_current_planned_mission_item_valid = isMissionValid(); resetMissionJumpCounter(); _navigator->reset_cruising_speed(); _navigator->reset_vroi(); @@ -688,6 +685,12 @@ MissionBase::check_mission_valid() _navigator->get_mission_result()->failure = false; set_mission_result(); + + // only warn if the check failed on merit + if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) { + PX4_WARN("mission check failed"); + } + } } @@ -860,16 +863,16 @@ void MissionBase::publish_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } -bool MissionBase::isMissionValid(mission_s &mission) const +bool MissionBase::isMissionValid() const { bool ret_val{false}; - if (((mission.current_seq < mission.count) || (mission.count == 0U && mission.current_seq <= 0)) && - (mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 - || mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && - (mission.timestamp != 0u)) { + if (((_mission.current_seq < _mission.count) || (_mission.count == 0U && _mission.current_seq <= 0)) && + (_mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || + _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && + (_mission.timestamp != 0u) && + (_navigator->get_mission_result()->valid)) { ret_val = true; - } return ret_val; @@ -1125,7 +1128,7 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa void MissionBase::resetMission() { /* we do not need to reset mission if is already.*/ - if (_mission.count == 0u && isMissionValid(_mission)) { + if (_mission.count == 0u) { return; } diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 660c918686f2..a501166e0114 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -207,13 +207,12 @@ class MissionBase : public MissionBlock, public ModuleParams int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps, bool mission_direction_backward = false); /** - * @brief Is Mission Parameters Valid + * @brief Is Mission Valid * - * @param mission Mission struct - * @return true is mission parameters are valid + * @return true is mission is valid * @return false otherwise */ - bool isMissionValid(mission_s &mission) const; + bool isMissionValid() const; /** * On mission update From 0f36a87dcf8800b169338a49de249b567be4112c Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 10:40:34 +0100 Subject: [PATCH 03/10] geofence: publish status of loaded geofence --- msg/CMakeLists.txt | 1 + msg/GeofenceStatus.msg | 7 +++++++ src/modules/navigator/geofence.cpp | 25 +++++++++++++++++++++++++ src/modules/navigator/geofence.h | 5 ++++- 4 files changed, 37 insertions(+), 1 deletion(-) create mode 100644 msg/GeofenceStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 62544cffc07b..2f8bb98c80e0 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -99,6 +99,7 @@ set(msg_files FollowTargetStatus.msg GeneratorStatus.msg GeofenceResult.msg + GeofenceStatus.msg GimbalControls.msg GimbalDeviceAttitudeStatus.msg GimbalDeviceInformation.msg diff --git a/msg/GeofenceStatus.msg b/msg/GeofenceStatus.msg new file mode 100644 index 000000000000..d32b9010c312 --- /dev/null +++ b/msg/GeofenceStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 geofence_id # loaded geofence id +uint8 status # Current geofence status + +uint8 GF_STATUS_LOADING = 0 +uint8 GF_STATUS_READY = 1 diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4f48d768a57d..55917331a614 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -81,6 +81,8 @@ Geofence::Geofence(Navigator *navigator) : if (_navigator != nullptr) { updateFence(); } + + _geofence_status_pub.advertise(); } Geofence::~Geofence() @@ -101,6 +103,14 @@ void Geofence::run() if (_initiate_fence_updated) { _initiate_fence_updated = false; _dataman_state = DatamanState::Read; + + geofence_status_s status; + status.timestamp = hrt_absolute_time(); + status.geofence_id = _opaque_id; + status.status = geofence_status_s::GF_STATUS_LOADING; + + _geofence_status_pub.publish(status); + } break; @@ -147,6 +157,14 @@ void Geofence::run() } else { _dataman_state = DatamanState::UpdateRequestWait; + _fence_updated = true; + + geofence_status_s status; + status.timestamp = hrt_absolute_time(); + status.geofence_id = _opaque_id; + status.status = geofence_status_s::GF_STATUS_READY; + + _geofence_status_pub.publish(status); } } @@ -160,6 +178,13 @@ void Geofence::run() _dataman_state = DatamanState::UpdateRequestWait; _updateFence(); _fence_updated = true; + + geofence_status_s status; + status.timestamp = hrt_absolute_time(); + status.geofence_id = _opaque_id; + status.status = geofence_status_s::GF_STATUS_READY; + + _geofence_status_pub.publish(status); } break; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index f2f057dc9a77..71d0a6bdf7ec 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -171,7 +172,7 @@ class Geofence : public ModuleParams mission_stats_entry_s _stats; DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _error_state{DatamanState::UpdateRequestWait}; - DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4}; + DatamanCache _dataman_cache{"geofence_dm_cache_miss", 0}; DatamanClient &_dataman_client = _dataman_cache.client(); float _altitude_min{0.0f}; @@ -185,6 +186,8 @@ class Geofence : public ModuleParams bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed + uORB::Publication _geofence_status_pub{ORB_ID(geofence_status)}; + /** * implementation of updateFence() */ From 47cfb1d4f0fdf4cc5a5a80b5675d5f36da222d6a Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 10:55:13 +0100 Subject: [PATCH 04/10] mission_base: Only run mission feasibility if the geofence module is ready --- src/modules/navigator/mission_base.cpp | 32 ++++++++++++++++++++++---- src/modules/navigator/mission_base.h | 6 ++++- 2 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 185bba939006..2ce9f3a596bc 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -120,9 +120,12 @@ void MissionBase::onMissionUpdate(bool has_mission_items_changed) _dataman_cache.invalidate(); _load_mission_index = -1; - if (_navigator->home_global_position_valid()) { - _initialized_mission_checked = true; + if (canRunMissionFeasibility()) { + _mission_checked = true; check_mission_valid(); + + } else { + _mission_checked = false; } } @@ -148,14 +151,15 @@ MissionBase::on_inactive() _land_detected_sub.update(); _vehicle_status_sub.update(); _global_pos_sub.update(); + _geofence_status_sub.update(); parameters_update(); updateMavlinkMission(); - /* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */ - if (_navigator->home_global_position_valid() && !_initialized_mission_checked) { - _initialized_mission_checked = true; + /* Check the mission */ + if (!_mission_checked && canRunMissionFeasibility()) { + _mission_checked = true; check_mission_valid(); _is_current_planned_mission_item_valid = isMissionValid(); } @@ -240,12 +244,22 @@ MissionBase::on_active() _land_detected_sub.update(); _vehicle_status_sub.update(); _global_pos_sub.update(); + _geofence_status_sub.update(); parameters_update(); updateMavlinkMission(); updateDatamanCache(); + /* Check the mission */ + if (!_mission_checked && canRunMissionFeasibility()) { + _mission_checked = true; + check_mission_valid(); + _is_current_planned_mission_item_valid = isMissionValid(); + update_mission(); + set_mission_items(); + } + // check if heading alignment is necessary, and add it to the current mission item if necessary if (_align_heading_necessary && is_mission_item_reached_or_completed()) { @@ -1347,3 +1361,11 @@ bool MissionBase::checkMissionDataChanged(mission_s new_mission) (new_mission.mission_id != _mission.mission_id) || (new_mission.current_seq != _mission.current_seq)); } + +bool MissionBase::canRunMissionFeasibility() +{ + return _navigator->home_global_position_valid() && + (_geofence_status_sub.get().timestamp > 0) && + (_geofence_status_sub.get().geofence_id == _mission.geofence_id) && + (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY); +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index a501166e0114..d2275cdabb14 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -308,7 +309,7 @@ class MissionBase : public MissionBlock, public ModuleParams bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ - bool _initialized_mission_checked{false}; /**< Flag indicating if the initialized mission has been checked by the mission validator*/ + bool _mission_checked{false}; /**< Flag indicating if the mission has been checked by the mission validator*/ bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/ mission_s _mission; /**< Currently active mission*/ float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ @@ -443,6 +444,8 @@ class MissionBase : public MissionBlock, public ModuleParams */ bool checkMissionDataChanged(mission_s new_mission); + bool canRunMissionFeasibility(); + int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ @@ -460,4 +463,5 @@ class MissionBase : public MissionBlock, public ModuleParams ) uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionData _geofence_status_sub{ORB_ID(geofence_status)}; }; From 37d8f56328808ae2a3d643c167f085ca41fd9c73 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 10:57:24 +0100 Subject: [PATCH 05/10] mission_base: check mission feasibility again, if geofence has changed. --- src/modules/navigator/mission_base.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 2ce9f3a596bc..8a1b04710937 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -102,6 +102,11 @@ void MissionBase::updateMavlinkMission() static_cast(new_mission.count) - 1); } + if (new_mission.geofence_id != _mission.geofence_id) { + // New geofence data, need to check mission again. + _mission_checked = false; + } + _mission = new_mission; /* Relevant mission items updated externally*/ From 5a3378cbeb282b027c36c44ebfd1be77b848d74e Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 12:26:29 +0100 Subject: [PATCH 06/10] MissionResult uorb: fix wrong int types --- msg/MissionResult.msg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/msg/MissionResult.msg b/msg/MissionResult.msg index aaa840762d99..f70326be3b26 100644 --- a/msg/MissionResult.msg +++ b/msg/MissionResult.msg @@ -1,8 +1,8 @@ uint64 timestamp # time since system start (microseconds) -uint16 mission_id # Id for the mission for which the result was generated -uint16 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) -uint64 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) +uint32 mission_id # Id for the mission for which the result was generated +uint32 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) +uint32 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) int32 seq_reached # Sequence of the mission item which has been reached, default -1 uint16 seq_current # Sequence of the current mission item From e3092a5a77ff52be524aca5e8a286a23c46ee4be Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 13:25:23 +0100 Subject: [PATCH 07/10] MissionFeasibiltyChecker: Do not delete uorb data on reset. --- .../MissionFeasibility/FeasibilityChecker.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index ef829d62f594..b27ef969aaeb 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,13 +46,6 @@ FeasibilityChecker::FeasibilityChecker() : void FeasibilityChecker::reset() { - - _is_landed = false; - _home_alt_msl = NAN; - _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - _vehicle_type = VehicleType::RotaryWing; - _mission_validity_failed = false; _takeoff_failed = false; _land_pattern_validity_failed = false; @@ -86,10 +79,16 @@ void FeasibilityChecker::updateData() if (home.valid_hpos) { _home_lat_lon = matrix::Vector2d(home.lat, home.lon); + + } else { + _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); } if (home.valid_alt) { _home_alt_msl = home.alt; + + } else { + _home_alt_msl = NAN; } } From 1ed1ffd5c763ec12dc87aab6340706c0192169a2 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 13:28:47 +0100 Subject: [PATCH 08/10] mission_base: Run feasibility checker only after first global position has been published --- src/modules/navigator/mission_base.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 8a1b04710937..1d98a033784e 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -1369,8 +1369,9 @@ bool MissionBase::checkMissionDataChanged(mission_s new_mission) bool MissionBase::canRunMissionFeasibility() { - return _navigator->home_global_position_valid() && - (_geofence_status_sub.get().timestamp > 0) && + return _navigator->home_global_position_valid() && // Need to have a home position checked + _navigator->get_global_position()->timestamp > 0 && // Need to have a position, for first waypoint check + (_geofence_status_sub.get().timestamp > 0) && // Geofence data must be loaded (_geofence_status_sub.get().geofence_id == _mission.geofence_id) && (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY); } From 4636069b2db70f628d447b0e835d54d2d708fea6 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 6 Mar 2024 14:29:42 +0100 Subject: [PATCH 09/10] missionFeasibilityChecker: Fix tests --- .../MissionFeasibility/FeasibilityCheckerTest.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index fa5882690537..0de54f33766c 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -65,6 +65,18 @@ class TestFeasibilityChecker : public FeasibilityChecker orb_publish(ORB_ID(home_position), home_pub, &home); } + void publishInvalidHome() + { + home_position_s home = {}; + home.alt = 0.f; + home.valid_alt = false; + home.lat = 0.; + home.lon = 0.; + home.valid_hpos = false; + orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home); + orb_publish(ORB_ID(home_position), home_pub, &home); + } + void publishCurrentPosition(double lat, double lon) { vehicle_global_position_s gpos = {}; @@ -122,6 +134,7 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity) ASSERT_EQ(ret, false); checker.reset(); + checker.publishInvalidHome(); mission_item.nav_cmd = NAV_CMD_TAKEOFF; mission_item.altitude_is_relative = true; ret = checker.processNextItem(mission_item, 0, 5); @@ -190,6 +203,7 @@ TEST_F(FeasibilityCheckerTest, check_below_home) // this is done to invalidate the home position checker.reset(); + checker.publishInvalidHome(); checker.publishLanded(true); checker.processNextItem(mission_item, 0, 1); From d3c87bd2fa6b8a4ff2997dcd8e6d27325e7b12ee Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 8 Mar 2024 09:44:42 +0100 Subject: [PATCH 10/10] mission_base: Force mission validity check on activation --- src/modules/navigator/mission_base.cpp | 12 +++++++----- src/modules/navigator/mission_base.h | 5 +++-- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 1d98a033784e..495639ba33a1 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -207,7 +207,7 @@ MissionBase::on_activation() _mission_has_been_activated = true; _system_disarmed_while_inactive = false; - check_mission_valid(); + check_mission_valid(true); update_mission(); @@ -687,11 +687,13 @@ MissionBase::checkMissionRestart() } void -MissionBase::check_mission_valid() +MissionBase::check_mission_valid(bool forced) { - if ((_navigator->get_mission_result()->mission_id != _mission.mission_id) - || (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) - || (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) { + // Allow forcing it, since we currently not rechecking if parameters have changed. + if (forced || + (_navigator->get_mission_result()->mission_id != _mission.mission_id) || + (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) || + (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) { _navigator->get_mission_result()->mission_id = _mission.mission_id; _navigator->get_mission_result()->geofence_id = _mission.geofence_id; diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index d2275cdabb14..3f7e2ce91d92 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -342,9 +342,10 @@ class MissionBase : public MissionBlock, public ModuleParams void updateMavlinkMission(); /** - * Check whether a mission is ready to go + * @brief Check whether a mission is ready to go + * @param[in] forced flag if the check has to be run irregardles of any updates. */ - void check_mission_valid(); + void check_mission_valid(bool forced = false); /** * Reset mission