From ac2c4097d03f7857cf0295181aa7b7239e9c11a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Wed, 24 May 2023 18:00:08 +0200 Subject: [PATCH] navigator: improve survey resume Co-authored-by: RomanBapst Co-authored-by: Julian Oes --- src/modules/navigator/mission.cpp | 224 +++++++++++++++++++++-- src/modules/navigator/mission.h | 28 +++ src/modules/navigator/navigator.h | 5 +- src/modules/navigator/navigator_main.cpp | 12 ++ 4 files changed, 249 insertions(+), 20 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b7611dcd19..e69f15fe3f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -161,14 +161,7 @@ Mission::on_inactive() void Mission::on_inactivation() { - // Disable camera trigger - vehicle_command_s cmd {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // Pause trigger - cmd.param1 = -1.0f; - cmd.param3 = 1.0f; - _navigator->publish_vehicle_cmd(&cmd); - + _navigator->disable_camera_trigger(); _navigator->stop_capturing_images(); _navigator->release_gimbal_control(); @@ -178,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 @@ -195,15 +190,26 @@ Mission::on_activation() // we already reset the mission items _execution_mode_changed = false; - set_mission_items(); + if (_current_mission_index > 0 && _current_mission_index != _inactivation_index) { + reset_command_cache(); + updateChachedCommandsUpToIndex(_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 (cameraWasTriggering() + && getPreviousPositionItemIndex(_mission, _current_mission_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 + set_current_mission_index(resume_index); + + } else { + set_mission_items(); + } + + _replay_cached_gimbal_commands_at_next_waypoint = haveCachedCommands(); + _inactivation_index = -1; // reset cruise speed _navigator->reset_cruising_speed(); @@ -242,6 +248,43 @@ Mission::on_active() set_mission_items(); } + const bool mission_item_reached = is_mission_item_reached_or_completed(); + + if (mission_item_reached && _replay_cached_gimbal_commands_at_next_waypoint + && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) { + + replay_cached_gimbal_commands(); + _replay_cached_gimbal_commands_at_next_waypoint = false; + + mission_item_s next_mission_item = {}; + + if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_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 if there is one + _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, + next_mission_item.lat, next_mission_item.lon)); + _mission_item.force_heading = true; + + // replay camera commands at the next waypoint to ensure the gimbal and vehicle are aligned for shooting + _replay_cached_camera_commands_at_next_waypoint = cameraWasTriggering(); + + } + + 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(); + + return; + } + + if (_replay_cached_camera_commands_at_next_waypoint && mission_item_reached) { + replay_cached_camera_commands(); + _replay_cached_camera_commands_at_next_waypoint = false; + } + /* 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, @@ -312,6 +355,10 @@ Mission::set_current_mission_index(uint16_t index) _current_mission_index = index; + if (_current_mission_index == 0) { + reset_command_cache(); + } + // 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; @@ -362,6 +409,7 @@ Mission::set_execution_mode(const uint8_t mode) pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; publish_navigator_mission_item(); // for logging _navigator->set_position_setpoint_triplet_updated(); + cache_command(_mission_item); issue_command(_mission_item); } @@ -387,6 +435,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; + reset_command_cache(); } else if (_current_mission_index < _mission.count - 1) { ++_current_mission_index; @@ -585,6 +634,10 @@ Mission::update_mission() _current_mission_index = 0; } + if (_current_mission_index == 0) { + reset_command_cache(); + } + // find and store landing start marker (if available) find_mission_land_start(); @@ -1816,8 +1869,9 @@ Mission::reset_mission(struct mission_s &mission) bool Mission::need_to_reset_mission() { - /* reset mission state when disarmed */ - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) { + /* reset mission state when disarmed and the mission was finished */ + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset + && (_current_mission_index == _mission.count - 1 || _navigator->get_vstatus()->is_vtol)) { _need_mission_reset = false; return true; } @@ -1935,3 +1989,137 @@ void Mission::publish_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } + +bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index) +{ + struct mission_item_s missionitem = {}; + + for (int index = start_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) +{ + 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) +{ + 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::cache_command(const mission_item_s &mission_item) +{ + switch (mission_item.nav_cmd) { + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + _last_gimbal_configure_command = mission_item; + break; + + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + _last_gimbal_control_command = mission_item; + break; + + case NAV_CMD_SET_CAMERA_MODE: + _last_camera_mode_command = 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_command = mission_item; + break; + + + default: + break; + } +} + +void Mission::replay_cached_gimbal_commands() +{ + if (_last_gimbal_configure_command.nav_cmd > 0) { + issue_command(_last_gimbal_configure_command); + } + + if (_last_gimbal_control_command.nav_cmd > 0) { + issue_command(_last_gimbal_control_command); + } +} + +void Mission::replay_cached_camera_commands() +{ + if (_last_camera_mode_command.nav_cmd > 0) { + issue_command(_last_camera_mode_command); + } + + if (_last_gimbal_configure_command.nav_cmd > 0) { + issue_command(_last_camera_trigger_command); + } +} + +void Mission::reset_command_cache() +{ + _last_gimbal_configure_command = {}; + _last_gimbal_control_command = {}; + _last_camera_mode_command = {}; + _last_camera_trigger_command = {}; +} + +bool Mission::haveCachedCommands() +{ + return _last_gimbal_configure_command.nav_cmd > 0 || + _last_gimbal_control_command.nav_cmd > 0 || + _last_camera_mode_command.nav_cmd > 0 || + _last_camera_trigger_command.nav_cmd > 0; + +} + +bool Mission::cameraWasTriggering() +{ + return (_last_camera_trigger_command.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL + && (int)(_last_camera_trigger_command.params[0] + 0.5f) == 1) || + (_last_camera_trigger_command.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || + (_last_camera_trigger_command.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST + && _last_camera_trigger_command.params[0] > 0.f); +} + +void Mission::updateChachedCommandsUpToIndex(const int end_index) +{ + for (int i = 0; i <= end_index; i++) { + mission_item_s mission_item = {}; + + if (readMissionItemAtIndex(_mission, i, mission_item)) { + cache_command(_mission_item); + cache_command(mission_item); + } + } +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 0cdddae4fb..1da1dfe9ed 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -240,6 +240,24 @@ private: void publish_navigator_mission_item(); + bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index); + + bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item); + + bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem); + void cache_command(const mission_item_s &mission_item); + + void updateChachedCommandsUpToIndex(int end_index); + + + void replay_cached_gimbal_commands(); + void replay_cached_camera_commands(); + void reset_command_cache(); + + bool haveCachedCommands(); + + bool cameraWasTriggering(); + DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamInt) _param_mis_mnt_yaw_ctl @@ -291,4 +309,14 @@ 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}; + + + bool _replay_cached_gimbal_commands_at_next_waypoint = false; + bool _replay_cached_camera_commands_at_next_waypoint = false; + int _inactivation_index = -1; + + mission_item_s _last_gimbal_configure_command {}; + mission_item_s _last_gimbal_control_command {}; + mission_item_s _last_camera_mode_command {}; + mission_item_s _last_camera_trigger_command {}; }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b62bffe334..282ffce096 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -302,8 +302,9 @@ public: void acquire_gimbal_control(); void release_gimbal_control(); - void calculate_breaking_stop(double &lat, double &lon, float &yaw); - void stop_capturing_images(); + void calculate_breaking_stop(double &lat, double &lon, float &yaw); + void stop_capturing_images(); + void disable_camera_trigger(); void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cbd0bd8fa2..ad62271bbe 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1488,6 +1488,18 @@ Navigator::stop_capturing_images() } } +void +Navigator::disable_camera_trigger() +{ + // Disable camera trigger + vehicle_command_s cmd {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // Pause trigger + cmd.param1 = -1.0f; + cmd.param3 = 1.0f; + publish_vehicle_cmd(&cmd); +} + bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos) { if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&