From d1b660b10480f90e427507a01c2491a442114a6f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Tue, 9 May 2023 13:17:16 +0200 Subject: [PATCH] rtl: integrate dataman cache --- src/modules/navigator/mission.cpp | 40 +++---- src/modules/navigator/mission.h | 12 +-- src/modules/navigator/navigator_main.cpp | 1 + src/modules/navigator/rtl.cpp | 130 ++++++++++++++++++----- src/modules/navigator/rtl.h | 23 +++- 5 files changed, 148 insertions(+), 58 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3c7623dfd1..1c56fc8f87 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -2023,14 +2023,18 @@ void Mission::publish_navigator_mission_item() bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactivation_index, unsigned &prev_pos_index) const { - struct mission_item_s missionitem = {}; for (int index = inactivation_index; index >= 0; index--) { - if (!readMissionItemAtIndex(mission, index, missionitem)) { + mission_item_s mission_item; + const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (!success) { break; } - if (MissionBlock::item_contains_position(missionitem)) { + if (MissionBlock::item_contains_position(mission_item)) { prev_pos_index = index; return true; } @@ -2039,10 +2043,16 @@ bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactiv return false; } -bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const +bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) { + const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + while (start_index < mission.count) { - if (readMissionItemAtIndex(mission, start_index, mission_item) && MissionBlock::item_contains_position(mission_item)) { + // start_index is expected to be after _current_mission_index, and the item should therefore be cached + bool success = _dataman_cache.loadWait(dm_current, start_index, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (success && MissionBlock::item_contains_position(mission_item)) { return true; } @@ -2052,19 +2062,6 @@ bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_ind return false; } -bool Mission::readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem) const -{ - bool success = false; - - if (index >= 0 && index < mission.count) { - const dm_item_t dm_current = (dm_item_t)mission.dataman_id; - const ssize_t len = sizeof(missionitem); - success = (dm_read(dm_current, index, &missionitem, len) == len); - } - - return success; -} - void Mission::cacheItem(const mission_item_s &mission_item) { switch (mission_item.nav_cmd) { @@ -2145,9 +2142,12 @@ bool Mission::cameraWasTriggering() void Mission::updateCachedItemsUpToIndex(const int end_index) { for (int i = 0; i <= end_index; i++) { - mission_item_s mission_item = {}; + mission_item_s mission_item; + const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); - if (readMissionItemAtIndex(_mission, i, mission_item)) { + if (success) { cacheItem(mission_item); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 2ae0c95eaf..be53a52ca3 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -263,17 +263,7 @@ private: * @param mission_item The mission item to populate * @return true if successful */ - bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const; - - /** - * @brief Read the mission item at the given index - * - * @param mission The mission to read from - * @param index The index to read - * @param missionitem The mission item to populate - * @return true if successful - */ - bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem) const; + bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item); /** * @brief Cache the mission items containing gimbal, camera mode and trigger commands diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 704f125dd5..9b965e799a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -919,6 +919,7 @@ void Navigator::run() _mission.run(); _geofence.run(); + _rtl.run(); perf_end(_loop_perf); } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index dd4d863716..517b0f9c39 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -67,6 +67,90 @@ RTL::RTL(Navigator *navigator) : _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); } +void RTL::run() +{ + bool success; + + switch (_dataman_state) { + + case DatamanState::UpdateRequestWait: + + if (_initiate_safe_points_updated) { + _initiate_safe_points_updated = false; + _dataman_state = DatamanState::Read; + } + + break; + + case DatamanState::Read: + + _dataman_state = DatamanState::ReadWait; + success = _dataman_client.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + _error_state = DatamanState::Read; + _dataman_state = DatamanState::Error; + } + + break; + + case DatamanState::ReadWait: + + _dataman_client.update(); + + if (_dataman_client.lastOperationCompleted(success)) { + + if (!success) { + _error_state = DatamanState::ReadWait; + _dataman_state = DatamanState::Error; + + } else if (_update_counter != _stats.update_counter) { + + _update_counter = _stats.update_counter; + _safe_points_updated = false; + + _dataman_cache.invalidate(); + + if (_dataman_cache.size() != _stats.num_items) { + _dataman_cache.resize(_stats.num_items); + } + + for (int index = 1; index <= _dataman_cache.size(); ++index) { + _dataman_cache.load(DM_KEY_SAFE_POINTS, index); + } + + _dataman_state = DatamanState::Load; + + } else { + _dataman_state = DatamanState::UpdateRequestWait; + } + } + + break; + + case DatamanState::Load: + + _dataman_cache.update(); + + if (!_dataman_cache.isLoading()) { + _dataman_state = DatamanState::UpdateRequestWait; + _safe_points_updated = true; + } + + break; + + case DatamanState::Error: + PX4_ERR("Safe points update failed! state: %" PRIu8, static_cast(_error_state)); + _dataman_state = DatamanState::UpdateRequestWait; + break; + + default: + break; + + } +} + void RTL::on_inactivation() { if (_navigator->get_precland()->is_activated()) { @@ -178,40 +262,34 @@ void RTL::find_RTL_destination() return; } - // compare to safe landing positions mission_safe_point_s closest_safe_point {}; - mission_stats_entry_s stats; - bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), - sizeof(mission_stats_entry_s)); - int num_safe_points = 0; - - if (success) { - num_safe_points = stats.num_items; - } - // check if a safe point is closer than home or landing int closest_index = 0; - for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { - mission_safe_point_s mission_safe_point; + if (_safe_points_updated) { - success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, current_seq, reinterpret_cast(&mission_safe_point), - sizeof(mission_safe_point_s)); + for (int current_seq = 1; current_seq <= _dataman_cache.size(); ++current_seq) { + mission_safe_point_s mission_safe_point; - if (!success) { - PX4_ERR("dm_read failed"); - continue; - } + bool success = _dataman_cache.loadWait(DM_KEY_SAFE_POINTS, current_seq, + reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s)); - // TODO: take altitude into account for distance measurement - dlat = mission_safe_point.lat - global_position.lat; - dlon = mission_safe_point.lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); + if (!success) { + PX4_ERR("dm_read failed"); + continue; + } - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; + // TODO: take altitude into account for distance measurement + dlat = mission_safe_point.lat - global_position.lat; + dlon = mission_safe_point.lon - global_position.lon; + double dist_squared = coord_dist_sq(dlat, dlon); + + if (dist_squared < min_dist_squared) { + closest_index = current_seq; + min_dist_squared = dist_squared; + closest_safe_point = mission_safe_point; + } } } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 6b8ddd28d2..b19aa42607 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -96,6 +96,11 @@ public: RTL_STATE_HEAD_TO_CENTER, }; + /** + * @brief function to call regularly to do background work + */ + void run(); + void on_inactivation() override; void on_inactive() override; void on_activation() override; @@ -138,6 +143,14 @@ private: RTLState _rtl_state{RTL_STATE_NONE}; + enum class DatamanState { + UpdateRequestWait, + Read, + ReadWait, + Load, + Error + }; + struct RTLPosition { double lat; double lon; @@ -157,7 +170,15 @@ private: } }; - DatamanClient _dataman_client{}; + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; + DatamanState _error_state{DatamanState::UpdateRequestWait}; + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated + bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache + DatamanCache _dataman_cache{"rtl_dm_cache_miss", 4}; + DatamanClient &_dataman_client = _dataman_cache.client(); + bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed + + mission_stats_entry_s _stats; RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)