diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 185bba9390..2ce9f3a596 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 a501166e01..d2275cdabb 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 @@ protected: 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 @@ private: */ 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 @@ private: ) uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionData _geofence_status_sub{ORB_ID(geofence_status)}; };