From 9f0cbc4a9d0d808a6472262425b9e35f91f98979 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 9 Jun 2023 16:45:16 +0200 Subject: [PATCH] Mission resume: make logic lighter and more separated Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 49 ++++++++++++++++--------------- src/modules/navigator/mission.h | 19 ++++++------ 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 41cea4b928df..28e5a6323dca 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -200,17 +200,17 @@ Mission::on_activation() if (_inactivation_index > 0 && cameraWasTriggering() && getPreviousPositionItemIndex(_mission, _inactivation_index - 1, resume_index)) { - // the mission we are resuming had camera triggering enabled. In order to not lose any images + // The mission we are resuming had camera triggering enabled. In order to not lose any images // we restart the mission at the previous position item. - // we will replay the cached commands once we reach the previous position item + // We will replay the cached commands once we reach the previous position item and have yaw aligned. set_current_mission_index(resume_index); - printf("reset to previous position item (resume_index %i)\n", resume_index); + + _align_heading_necessary = true; } else { set_mission_items(); } - _replay_cached_gimbal_items_at_next_waypoint = haveCachedItems(); _inactivation_index = -1; // reset // reset cruise speed @@ -250,21 +250,16 @@ Mission::on_active() set_mission_items(); } - if (_replay_cached_gimbal_items_at_next_waypoint && is_mission_item_reached_or_completed() - && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) { - replayCachedGimbalItems(); - _replay_cached_gimbal_items_at_next_waypoint = false; - + // 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()) { mission_item_s next_position_mission_item = {}; + // add yaw alignment requirement on t he current mission item if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_position_mission_item) && !PX4_ISFINITE(_mission_item.yaw)) { - // we are at the waypoint from which we want to resume the mission, first make sure that we are facing the next waypoint _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, next_position_mission_item.lat, next_position_mission_item.lon)); _mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode - // replay camera commands at the next waypoint to ensure the gimbal and vehicle are aligned for shooting - _replay_cached_camera_items_at_next_waypoint = cameraWasTriggering(); } mission_apply_limitation(_mission_item); @@ -273,13 +268,17 @@ Mission::on_active() reset_mission_item_reached(); _navigator->set_position_setpoint_triplet_updated(); + _align_heading_necessary = false; + } - return; + // replay gimbal and camera commands immediately after resuming mission + if (haveCachedGimbalOrCameraItems()) { + replayCachedGimbalCameraItems(); } - if (_replay_cached_camera_items_at_next_waypoint && is_mission_item_reached_or_completed()) { - replayCachedCameraItems(); - _replay_cached_camera_items_at_next_waypoint = false; + // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set + if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { + replayCachedTriggerItems(); } /* lets check if we reached the current mission item */ @@ -2006,7 +2005,6 @@ bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactiv if (MissionBlock::item_contains_position(missionitem)) { prev_pos_index = index; - printf("prev_pos_index %i, inactivation_index: %i\n", prev_pos_index, inactivation_index); return true; } } @@ -2067,25 +2065,29 @@ void Mission::cacheItem(const mission_item_s &mission_item) } } -void Mission::replayCachedGimbalItems() +void Mission::replayCachedGimbalCameraItems() { if (_last_gimbal_configure_item.nav_cmd > 0) { issue_command(_last_gimbal_configure_item); + _last_gimbal_configure_item = {}; // delete cached item } if (_last_gimbal_control_item.nav_cmd > 0) { issue_command(_last_gimbal_control_item); + _last_gimbal_control_item = {}; // delete cached item } -} -void Mission::replayCachedCameraItems() -{ if (_last_camera_mode_item.nav_cmd > 0) { issue_command(_last_camera_mode_item); + _last_camera_mode_item = {}; // delete cached item } +} +void Mission::replayCachedTriggerItems() +{ if (_last_camera_trigger_item.nav_cmd > 0) { issue_command(_last_camera_trigger_item); + _last_camera_trigger_item = {}; // delete cached item } } @@ -2097,12 +2099,11 @@ void Mission::resetItemCache() _last_camera_trigger_item = {}; } -bool Mission::haveCachedItems() +bool Mission::haveCachedGimbalOrCameraItems() { return _last_gimbal_configure_item.nav_cmd > 0 || _last_gimbal_control_item.nav_cmd > 0 || - _last_camera_mode_item.nav_cmd > 0 || - _last_camera_trigger_item.nav_cmd > 0; + _last_camera_mode_item.nav_cmd > 0; } bool Mission::cameraWasTriggering() diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 3cf8bba387c5..94e4a57f6183 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -271,7 +271,7 @@ class Mission : public MissionBlock, public ModuleParams bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem); /** - * @brief Cache the mission items containing gimbal and camera commands + * @brief Cache the mission items containing gimbal, camera mode and trigger commands * * @param mission_item The mission item to cache if applicable */ @@ -285,15 +285,15 @@ class Mission : public MissionBlock, public ModuleParams void updateCachedItemsUpToIndex(int end_index); /** - * @brief Replay the cached gimbal items + * @brief Replay the cached gimbal and camera mode items */ - void replayCachedGimbalItems(); + void replayCachedGimbalCameraItems(); /** - * @brief Replay the cached camera items + * @brief Replay the cached trigger items * */ - void replayCachedCameraItems(); + void replayCachedTriggerItems(); /** * @brief Reset the item cache @@ -301,11 +301,11 @@ class Mission : public MissionBlock, public ModuleParams void resetItemCache(); /** - * @brief Check if there are cached items + * @brief Check if there are cached gimbal or camera mode items to be replayed * * @return true if there are cached items */ - bool haveCachedItems(); + bool haveCachedGimbalOrCameraItems(); /** * @brief Check if the camera was triggering @@ -366,9 +366,8 @@ class Mission : public MissionBlock, public ModuleParams uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ bool _execution_mode_changed{false}; - bool _replay_cached_gimbal_items_at_next_waypoint = false; - bool _replay_cached_camera_items_at_next_waypoint = false; - int _inactivation_index = -1; + int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not loose images. + bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment. mission_item_s _last_gimbal_configure_item {}; mission_item_s _last_gimbal_control_item {};