diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fec7efdebd..eade1446bf 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -171,6 +171,8 @@ Mission::on_inactivation() /* reset so current mission item gets restarted if mission was paused */ _work_item_type = WORK_ITEM_TYPE_DEFAULT; + + _inactivation_index = _current_mission_index; } void @@ -188,15 +190,28 @@ Mission::on_activation() // we already reset the mission items _execution_mode_changed = false; - set_mission_items(); + // reset the cache and fill it with the camera and gimbal items up to the previous item + if (_current_mission_index > 0) { + resetItemCache(); + updateCachedItemsUpToIndex(_current_mission_index - 1); + } - // unpause triggering if it was paused - vehicle_command_s cmd = {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // unpause trigger - cmd.param1 = -1.0f; - cmd.param3 = 0.0f; - _navigator->publish_vehicle_cmd(&cmd); + unsigned resume_index; + + 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 + // we restart the mission at 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); + + _align_heading_necessary = true; + + } else { + set_mission_items(); + } + + _inactivation_index = -1; // reset // reset cruise speed _navigator->reset_cruising_speed(); @@ -235,6 +250,37 @@ Mission::on_active() 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()) { + mission_item_s next_position_mission_item = {}; + + // add yaw alignment requirement on the current mission item + if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_position_mission_item) + && !PX4_ISFINITE(_mission_item.yaw)) { + _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 + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + reset_mission_item_reached(); + + _navigator->set_position_setpoint_triplet_updated(); + _align_heading_necessary = false; + } + + // replay gimbal and camera commands immediately after resuming mission + if (haveCachedGimbalOrCameraItems()) { + replayCachedGimbalCameraItems(); + } + + // 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 */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { /* If we just completed a takeoff which was inserted before the right waypoint, @@ -305,6 +351,11 @@ Mission::set_current_mission_index(uint16_t index) _current_mission_index = index; + // we start from the first item so can reset the cache + if (_current_mission_index == 0) { + resetItemCache(); + } + // a mission index is set manually which has the higher priority than the closest mission item // as it is set by the user _mission_waypoints_changed = false; @@ -380,6 +431,7 @@ Mission::set_execution_mode(const uint8_t mode) // handle switch from reverse to forward mission if (_current_mission_index < 0) { _current_mission_index = 0; + resetItemCache(); // reset cache as we start from the beginning } else if (_current_mission_index < _mission.count - 1) { ++_current_mission_index; @@ -578,6 +630,14 @@ Mission::update_mission() _current_mission_index = 0; } + // we start from the first item so can reset the cache + if (_current_mission_index == 0) { + resetItemCache(); + } + + // reset as when we update mission we don't want to proceed at previous index + _inactivation_index = -1; + // find and store landing start marker (if available) find_mission_land_start(); @@ -1932,3 +1992,136 @@ void Mission::publish_navigator_mission_item() _navigator_mission_item_pub.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)) { + break; + } + + if (MissionBlock::item_contains_position(missionitem)) { + prev_pos_index = index; + return true; + } + } + + return false; +} + +bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const +{ + while (start_index < mission.count) { + if (readMissionItemAtIndex(mission, start_index, mission_item) && MissionBlock::item_contains_position(mission_item)) { + return true; + } + + start_index++; + } + + 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) { + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + _last_gimbal_configure_item = mission_item; + break; + + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + _last_gimbal_control_item = mission_item; + break; + + case NAV_CMD_SET_CAMERA_MODE: + _last_camera_mode_item = mission_item; + break; + + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_DO_TRIGGER_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + _last_camera_trigger_item = mission_item; + break; + + default: + break; + } +} + +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 + } + + 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 + } +} + +void Mission::resetItemCache() +{ + _last_gimbal_configure_item = {}; + _last_gimbal_control_item = {}; + _last_camera_mode_item = {}; + _last_camera_trigger_item = {}; +} + +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; +} + +bool Mission::cameraWasTriggering() +{ + return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL + && (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST + && _last_camera_trigger_item.params[0] > FLT_EPSILON); +} + +void Mission::updateCachedItemsUpToIndex(const int end_index) +{ + for (int i = 0; i <= end_index; i++) { + mission_item_s mission_item = {}; + + if (readMissionItemAtIndex(_mission, i, mission_item)) { + cacheItem(mission_item); + } + } +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 0cdddae4fb..4854ee0f05 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -240,6 +240,80 @@ private: void publish_navigator_mission_item(); + + /** + * @brief Get the index associated with the last item that contains a position + * @param mission The mission to search + * @param start_index The index to start searching from + * @param prev_pos_index The index of the previous position item containing a position + * @return true if a previous position item was found + */ + bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index) const; + + /** + * @brief Get the next item after start_index that contains a position + * + * @param mission The mission to search + * @param start_index The index to start searching from + * @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; + + /** + * @brief Cache the mission items containing gimbal, camera mode and trigger commands + * + * @param mission_item The mission item to cache if applicable + */ + void cacheItem(const mission_item_s &mission_item); + + /** + * @brief Update the cached items up to the given index + * + * @param end_index The index to update up to + */ + void updateCachedItemsUpToIndex(int end_index); + + /** + * @brief Replay the cached gimbal and camera mode items + */ + void replayCachedGimbalCameraItems(); + + /** + * @brief Replay the cached trigger items + * + */ + void replayCachedTriggerItems(); + + /** + * @brief Reset the item cache + */ + void resetItemCache(); + + /** + * @brief Check if there are cached gimbal or camera mode items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalOrCameraItems(); + + /** + * @brief Check if the camera was triggering + * + * @return true if there was a camera trigger command in the cached items that didn't disable triggering + */ + bool cameraWasTriggering(); + DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamInt) _param_mis_mnt_yaw_ctl @@ -291,4 +365,12 @@ private: 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}; + + int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose 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 {}; + mission_item_s _last_camera_mode_item {}; + mission_item_s _last_camera_trigger_item {}; };