diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8128380973..3c7623dfd1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,27 +70,48 @@ Mission::Mission(Navigator *navigator) : mission_init(); } +void +Mission::run() +{ + if ((_mission.count > 0) && (_current_mission_index != _load_mission_index)) { + + uint32_t start_index = _current_mission_index; + uint32_t end_index = start_index + DATAMAN_CACHE_SIZE; + + end_index = math::min(end_index, static_cast(_mission.count)); + + for (uint32_t index = start_index; index < end_index; ++index) { + + _dataman_cache.load(static_cast(_mission.dataman_id), index); + } + + _load_mission_index = _current_mission_index; + } + + _dataman_cache.update(); +} + void Mission::mission_init() { // init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start - mission_s mission{}; - if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s))) { - if ((mission.timestamp != 0) - && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { - if (mission.count > 0) { - PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count); + if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), sizeof(mission_s))) { + if ((_mission.timestamp != 0) + && (_mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { + if (_mission.count > 0) { + PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", _mission.dataman_id, _mission.count); } } else { - PX4_ERR("reading mission state failed"); - // initialize mission state in dataman - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.timestamp = hrt_absolute_time(); - _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + _mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + _mission.timestamp = hrt_absolute_time(); + _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), sizeof(mission_s)); } } + + _current_mission_index = _mission.current_seq; + } void @@ -464,7 +485,7 @@ Mission::find_mission_land_start() missionitem_prev = missionitem; // store the last mission item before reading a new one bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s)); + sizeof(mission_item_s), 500_ms); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -563,6 +584,9 @@ Mission::update_mission() bool failed = true; + _dataman_cache.invalidate(); + _load_mission_index = -1; + /* Reset vehicle_roi * Missions that do not explicitly configure ROI would not override * an existing ROI setting from previous missions */ @@ -668,8 +692,8 @@ Mission::advance_mission() for (int32_t i = _current_mission_index - 1; i >= 0; i--) { struct mission_item_s missionitem = {}; - bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s)); + bool success = _dataman_cache.loadWait(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s), 100_ms); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -1619,7 +1643,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) int index_to_read = current_index + offset; int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read; - const dm_item_t dm_item = (dm_item_t)_mission.dataman_id; + const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; /* do not work on empty missions */ if (_mission.count == 0) { @@ -1646,8 +1670,8 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) struct mission_item_s mission_item_tmp; /* read mission item from datamanager */ - bool success = _dataman_client.readSync(dm_item, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), - sizeof(mission_item_s)); + bool success = _dataman_cache.loadWait(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), + sizeof(mission_item_s), 500_ms); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -1670,7 +1694,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - success = _dataman_client.writeSync(dm_item, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), + success = _dataman_client.writeSync(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), sizeof(struct mission_item_s)); if (!success) { @@ -1722,13 +1746,6 @@ Mission::save_mission_state() { mission_s mission_state = {}; - /* lock MISSION_STATE item */ - bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE); - - if (!success_lock) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); - } - /* read current state */ bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), sizeof(mission_s)); @@ -1773,11 +1790,6 @@ Mission::save_mission_state() PX4_ERR("Can't save mission state"); } } - - /* unlock MISSION_STATE item */ - if (success_lock) { - _dataman_client.unlockSync(DM_KEY_MISSION_STATE); - } } void @@ -1815,7 +1827,7 @@ Mission::check_mission_valid(bool force) { if ((!_home_inited && _navigator->home_global_position_valid()) || force) { - MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); + MissionFeasibilityChecker _missionFeasibilityChecker(_navigator, _dataman_client); _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_mission); @@ -1833,8 +1845,6 @@ Mission::check_mission_valid(bool force) void Mission::reset_mission(struct mission_s &mission) { - _dataman_client.lockSync(DM_KEY_MISSION_STATE); - if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s))) { if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { /* set current item to 0 */ @@ -1842,13 +1852,14 @@ Mission::reset_mission(struct mission_s &mission) /* reset jump counters */ if (mission.count > 0) { - const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + const dm_item_t dataman_id = (dm_item_t)mission.dataman_id; for (unsigned index = 0; index < mission.count; index++) { struct mission_item_s item; const ssize_t len = sizeof(struct mission_item_s); - bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast(&item), sizeof(mission_item_s)); + bool success = _dataman_client.readSync(dataman_id, index, reinterpret_cast(&item), sizeof(mission_item_s), + 500_ms); if (!success) { PX4_WARN("could not read mission item during reset"); @@ -1858,7 +1869,7 @@ Mission::reset_mission(struct mission_s &mission) if (item.nav_cmd == NAV_CMD_DO_JUMP) { item.do_jump_current_count = 0; - success = _dataman_client.writeSync(dm_current, index, reinterpret_cast(&item), len); + success = _dataman_client.writeSync(dataman_id, index, reinterpret_cast(&item), len); if (!success) { PX4_WARN("could not save mission item during reset"); @@ -1881,8 +1892,6 @@ Mission::reset_mission(struct mission_s &mission) _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); } - - _dataman_client.unlockSync(DM_KEY_MISSION_STATE); } bool @@ -1910,7 +1919,7 @@ Mission::index_closest_mission_item() struct mission_item_s missionitem = {}; bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s)); + sizeof(mission_item_s), 500_ms); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6cef70c4e7..2ae0c95eaf 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -74,6 +74,11 @@ public: Mission(Navigator *navigator); ~Mission() override = default; + /** + * @brief function to call regularly to do background work + */ + void run(); + void on_inactive() override; void on_inactivation() override; void on_activation() override; @@ -323,8 +328,11 @@ private: uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ mission_s _mission {}; - DatamanClient _dataman_client{}; + static constexpr uint32_t DATAMAN_CACHE_SIZE = 10; + DatamanCache _dataman_cache{"mission_dm_cache_miss", DATAMAN_CACHE_SIZE}; + DatamanClient &_dataman_client = _dataman_cache.client(); + int32_t _load_mission_index{-1}; int32_t _current_mission_index{-1}; // track location of planned mission landing diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index f992b2e6a3..0eedaf7366 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -54,14 +54,16 @@ class MissionFeasibilityChecker: public ModuleParams { private: Navigator *_navigator{nullptr}; + DatamanClient &_dataman_client; FeasibilityChecker _feasibility_checker; bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); public: - MissionFeasibilityChecker(Navigator *navigator) : + MissionFeasibilityChecker(Navigator *navigator, DatamanClient &dataman_client) : ModuleParams(nullptr), _navigator(navigator), + _dataman_client(dataman_client), _feasibility_checker() { @@ -75,6 +77,4 @@ public: * Returns true if mission is feasible and false otherwise */ bool checkMissionFeasible(const mission_s &mission); - - DatamanClient _dataman_client{}; }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 12a096ec87..3353bb4806 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -917,6 +917,8 @@ void Navigator::run() publish_mission_result(); } + _mission.run(); + perf_end(_loop_perf); } }