From 007ed11bbe930d932f0cf1367870098b5bb91b13 Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 27 Jun 2023 15:42:49 +0200 Subject: [PATCH] Mission+RTL: Refactoring mission and RTL to keep them separate. RTL does all its mission related computation in its own class. Dataman: Add write function to dataman cache. RTL and mission have a new common base class mission_base. Both inherit from them and mission, RTL mission, and rtl reverse mission inherit from them and implement their desired functionalities. This simplifies the logic in mission as well as make the logic in rtl mission reverse and mission more readable. Rtl mission reverse now functional again for VTOL flying back the mission and transitioning to MC at the home position. Dataman cache has new write functionality to write to dataman while updating write item in its cache if necessary. Dataman cache is now only updated when the respective module is active. Leads to a higher computation time once on activation, but decreases unnecessary cache updates when inactive. --- msg/Mission.msg | 9 +- src/lib/dataman_client/DatamanClient.cpp | 26 + src/lib/dataman_client/DatamanClient.hpp | 13 + src/modules/navigator/CMakeLists.txt | 5 + src/modules/navigator/mission.cpp | 2316 +++-------------- src/modules/navigator/mission.h | 305 +-- src/modules/navigator/mission_base.cpp | 1235 +++++++++ src/modules/navigator/mission_base.h | 446 ++++ src/modules/navigator/mission_block.cpp | 41 +- src/modules/navigator/mission_block.h | 21 + src/modules/navigator/navigator.h | 25 +- src/modules/navigator/navigator_main.cpp | 141 +- src/modules/navigator/navigator_mode.cpp | 1 - src/modules/navigator/navigator_mode.h | 2 + src/modules/navigator/rtl.cpp | 1060 ++------ src/modules/navigator/rtl.h | 186 +- src/modules/navigator/rtl_direct.cpp | 723 +++++ src/modules/navigator/rtl_direct.h | 239 ++ .../navigator/rtl_direct_mission_land.cpp | 281 ++ .../navigator/rtl_direct_mission_land.h | 72 + src/modules/navigator/rtl_mission_fast.cpp | 241 ++ src/modules/navigator/rtl_mission_fast.h | 71 + .../navigator/rtl_mission_fast_reverse.cpp | 268 ++ .../navigator/rtl_mission_fast_reverse.h | 71 + 24 files changed, 4561 insertions(+), 3237 deletions(-) create mode 100644 src/modules/navigator/mission_base.cpp create mode 100644 src/modules/navigator/mission_base.h create mode 100644 src/modules/navigator/rtl_direct.cpp create mode 100644 src/modules/navigator/rtl_direct.h create mode 100644 src/modules/navigator/rtl_direct_mission_land.cpp create mode 100644 src/modules/navigator/rtl_direct_mission_land.h create mode 100644 src/modules/navigator/rtl_mission_fast.cpp create mode 100644 src/modules/navigator/rtl_mission_fast.h create mode 100644 src/modules/navigator/rtl_mission_fast_reverse.cpp create mode 100644 src/modules/navigator/rtl_mission_fast_reverse.h diff --git a/msg/Mission.msg b/msg/Mission.msg index c7740d4f7a..546959a3b3 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -4,6 +4,9 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest -int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased -int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased -int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased +int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise +int32 land_index # Index of the land item, -1 otherwise + +uint16 mission_update_counter # indicates updates to the mission, reload from dataman if increased +uint16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased +uint16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp index 1fa4820fef..a6b37defa9 100644 --- a/src/lib/dataman_client/DatamanClient.cpp +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -556,6 +556,32 @@ bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uin return success; } +bool DatamanCache::writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = _client.writeSync(item, index, buffer, length, timeout); + + if (success && _items) { + for (uint32_t i = 0; i < _num_items; ++i) { + if ((_items[i].response.item == item) && + (_items[i].response.index == index) && + ((_items[i].cache_state == State::ResponseReceived) || + (_items[i].cache_state == State::RequestPrepared))) { + + memcpy(_items[i].response.data, buffer, length); + _items[i].cache_state = State::ResponseReceived; + break; + } + } + } + + return success; +} + void DatamanCache::update() { if (_item_counter > 0) { diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp index 3fa6d82065..636b41d142 100644 --- a/src/lib/dataman_client/DatamanClient.hpp +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -235,6 +235,19 @@ public: */ bool loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 0); + /** + * @brief Write data back and update it in the cache if stored. + * + * @param[in] item The data item type to write. + * @param[in] index The index of the data item. + * @param[in] buffer The buffer that contains the data to write. + * @param[in] length The length of the data to write. + * @param[in] timeout The maximum time in microseconds to wait for the response. + * + * @return True if the write operation succeeded, false otherwise. + */ + bool writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + /** * @brief Updates the dataman cache by checking for responses from the DatamanClient and processing them. * diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 4532c08d17..72995b367e 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -40,10 +40,15 @@ px4_add_module( SRCS navigator_main.cpp navigator_mode.cpp + mission_base.cpp mission_block.cpp mission.cpp loiter.cpp rtl.cpp + rtl_direct.cpp + rtl_direct_mission_land.cpp + rtl_mission_fast.cpp + rtl_mission_fast_reverse.cpp takeoff.cpp land.cpp precland.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d5f1db688b..3e97896d02 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,7 +50,6 @@ #include #include -#include #include #include #include @@ -63,502 +62,43 @@ using namespace time_literals; +static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; + Mission::Mission(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) { - 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 - - 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 { - // 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)); - } - } - - _current_mission_index = _mission.current_seq; - } void Mission::on_inactive() { - /* Without home a mission can't be valid yet anyway, let's wait. */ - if (!_navigator->home_global_position_valid()) { - return; - } + _vehicle_status_sub.update(); - if (_need_mission_save && _navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (_need_mission_save && _vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { save_mission_state(); } - if (_inited) { - if (_mission_sub.updated()) { - update_mission(); - - if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { - _mission_type = MISSION_TYPE_MISSION; - } - } - - /* reset the current mission if needed */ - if (need_to_reset_mission()) { - reset_mission(_mission); - _navigator->reset_cruising_speed(); - _current_mission_index = 0; - _navigator->reset_vroi(); - set_current_mission_item(); - } - - } else { - - /* load missions from storage */ - mission_s mission_state = {}; - - /* read current state */ - bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), - sizeof(mission_s)); - - if (success) { - _mission.dataman_id = mission_state.dataman_id; - _mission.count = mission_state.count; - _current_mission_index = mission_state.current_seq; - - // find and store landing start marker (if available) - find_mission_land_start(); - } - - /* On init let's check the mission, maybe there is already one available. */ - check_mission_valid(false); - - _inited = true; - } - - /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - - /* reset so MISSION_ITEM_REACHED isn't published */ - _navigator->get_mission_result()->seq_reached = -1; -} - -void -Mission::on_inactivation() -{ - _navigator->disable_camera_trigger(); - _navigator->stop_capturing_images(); - _navigator->release_gimbal_control(); - - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->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; + MissionBase::on_inactive(); } void Mission::on_activation() { - if (_mission_waypoints_changed) { - // do not set the closest mission item in the normal mission mode - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { - _current_mission_index = index_closest_mission_item(); - } + _need_mission_save = true; - _mission_waypoints_changed = false; - } - - // we already reset the mission items - _execution_mode_changed = false; - - // reset the cache and fill it with the items up to the previous item. The cache contains - // commands that are valid for the whole mission, not just a sinlge waypoint. - if (_current_mission_index > 0) { - resetItemCache(); - updateCachedItemsUpToIndex(_current_mission_index - 1); - } - - uint16_t resume_index = _inactivation_index > 0 ? _inactivation_index : 0; - - 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. - - checkClimbRequired(resume_index); - set_current_mission_index(resume_index); - - _align_heading_necessary = true; - - } else { - checkClimbRequired(resume_index); - set_mission_items(); - } - - _inactivation_index = -1; // reset - - // reset cruise speed - _navigator->reset_cruising_speed(); -} - -void -Mission::on_active() -{ - check_mission_valid(false); - - /* Check if stored mission plan has changed */ - const bool mission_sub_updated = _mission_sub.updated(); - - if (mission_sub_updated) { - _navigator->reset_triplets(); - update_mission(); - } - - /* mission is running (and we are armed), need reset after disarm */ - _need_mission_reset = true; - - _mission_changed = false; - - /* reset mission items if needed */ - if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) { - if (_mission_waypoints_changed) { - // do not set the closest mission item in the normal mission mode - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { - _current_mission_index = index_closest_mission_item(); - } - - _mission_waypoints_changed = false; - } - - _execution_mode_changed = false; - 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(); - } - - replayCachedSpeedChangeItems(); - - /* 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, - there is no need to report that we reached it because we didn't. */ - if (_work_item_type != WORK_ITEM_TYPE_CLIMB) { - set_mission_item_reached(); - } - - if (_mission_item.autocontinue) { - /* switch to next waypoint if 'autocontinue' flag set */ - advance_mission(); - set_mission_items(); - } - - } - - /* see if we need to update the current yaw heading */ - if (!_param_mis_mnt_yaw_ctl.get() - && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) - && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) - && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - || _mission_item.nav_cmd == NAV_CMD_LAND - || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND - || _work_item_type == WORK_ITEM_TYPE_ALIGN_HEADING)) { - // Mount control is disabled If the vehicle is in ROI-mode, the vehicle - // needs to rotate such that ROI is in the field of view. - // ROI only makes sense for multicopters. - heading_sp_update(); - } - - // TODO: Add vtol heading update method if required. - // Question: Why does vtol ever have to update heading? - - /* check if landing needs to be aborted */ - if ((_mission_item.nav_cmd == NAV_CMD_LAND) - && (_navigator->abort_landing())) { - - do_abort_landing(); - } - - if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } + MissionBase::on_activation(); } bool -Mission::set_current_mission_index(uint16_t index) -{ - if (index == _current_mission_index) { - return true; // nothing to do, so return true - - } else if (_navigator->get_mission_result()->valid && (index < _mission.count)) { - - _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; - - // update mission items if already in active mission - if (_navigator->is_planned_mission()) { - // prevent following "previous - current" line - _navigator->get_position_setpoint_triplet()->previous.valid = false; - _navigator->get_position_setpoint_triplet()->current.valid = false; - _navigator->get_position_setpoint_triplet()->next.valid = false; - set_mission_items(); - } - - return true; - } - - return false; -} - -void -Mission::set_closest_item_as_current() -{ - _current_mission_index = index_closest_mission_item(); -} - -void -Mission::set_execution_mode(const uint8_t mode) -{ - if (_mission_execution_mode != mode) { - _execution_mode_changed = true; - _navigator->get_mission_result()->execution_mode = mode; - - - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: - if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - // command a transition if in vtol mc mode - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - _navigator->get_vstatus()->is_vtol && - !_navigator->get_land_detected()->landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous = pos_sp_triplet->current; - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); - issue_command(_mission_item); - } - - if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { - _mission_type = MISSION_TYPE_MISSION; - } - - if (_current_mission_index > _mission.count - 1) { - _current_mission_index = _mission.count - 1; - - } else if (_current_mission_index > 0) { - --_current_mission_index; - } - - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - } - - break; - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: - if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) || - (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) { - // 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; - } - - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - } - - break; - - } - - _mission_execution_mode = mode; - } -} - -bool -Mission::find_mission_land_start() -{ - /* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index - * return false if not found - */ - - const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; - struct mission_item_s missionitem = {}; - struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START - - _land_start_available = false; - - bool found_land_start_marker = false; - - for (size_t i = 1; i < _mission.count; i++) { - 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), 500_ms); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - found_land_start_marker = true; - } - - if (found_land_start_marker && !_land_start_available && item_contains_position(missionitem)) { - // use the position of any waypoint after the land start marker which specifies a position. - _landing_start_lat = missionitem.lat; - _landing_start_lon = missionitem.lon; - _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + - _navigator->get_home_position()->alt : missionitem.altitude; - _landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius) - && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) : - _navigator->get_loiter_radius(); - _land_start_available = true; - _land_start_index = i; // set it to the first item containing a position after the land start marker was found - } - - if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || - (missionitem.nav_cmd == NAV_CMD_LAND)) { - - _landing_lat = missionitem.lat; - _landing_lon = missionitem.lon; - _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : - missionitem.altitude; - - // don't have a valid land start yet, use the landing item itself then - if (!_land_start_available) { - _land_start_index = i; - _landing_start_lat = _landing_lat; - _landing_start_lon = _landing_lon; - _landing_start_alt = _landing_alt; - _land_start_available = true; - } - - } - } - - return _land_start_available; -} - -bool -Mission::land_start() -{ - // if not currently landing, jump to do_land_start - if (_land_start_available) { - // check if we're currently already in mission mode and on landing part, then simply return true. - // note: it's not enough to check landing(), as that is not reset until set_current_mission_index(get_land_start_index()) - if (_navigator->on_mission_landing()) { - return true; - - } else { - set_current_mission_index(get_land_start_index()); - return landing(); - } - } - - return false; -} - -bool -Mission::landing() +Mission::isLanding() { // vehicle is currently landing if // mission valid, still flying, and in the landing portion of mission (past land start marker) - - const bool mission_valid = _navigator->get_mission_result()->valid; - bool on_landing_stage = _land_start_available && _current_mission_index > get_land_start_index(); + bool on_landing_stage = get_land_start_available() && _mission.current_seq > get_land_start_index(); // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the // distance to the WP is below the loiter radius + acceptance. - if (_current_mission_index == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + if (_mission.current_seq == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); @@ -570,750 +110,56 @@ Mission::landing() on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); } - return mission_valid && on_landing_stage; + return _navigator->get_mission_result()->valid && on_landing_stage; } -void -Mission::update_mission() +bool +Mission::set_current_mission_index(uint16_t index) { - - bool failed = !_navigator->get_mission_result()->valid; - - _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 */ - _navigator->reset_vroi(); - - const mission_s old_mission = _mission; - - if (_mission_sub.copy(&_mission)) { - - bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), - sizeof(mission_s)); - - if (!success) { - PX4_ERR("Can't update mission state in Dataman"); - } - - /* determine current index */ - if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { - _current_mission_index = _mission.current_seq; - - } else { - /* if less items available, reset to first item */ - if (_current_mission_index >= (int)_mission.count) { - _current_mission_index = 0; - - } else if (_current_mission_index < 0) { - /* if not initialized, set it to 0 */ - _current_mission_index = 0; - } - - /* otherwise, just leave it */ - } - - if (old_mission.mission_update_counter != _mission.mission_update_counter) { - check_mission_valid(true); - - failed = !_navigator->get_mission_result()->valid; - - if (!failed) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; - - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; - - /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_changed = true; - } - - /* check if the mission waypoints changed while the vehicle is in air - * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != old_mission.count) || - (_mission.dataman_id != old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { - _mission_waypoints_changed = true; - } - } - - } else { - PX4_ERR("mission update failed"); - failed = true; + if (index == _mission.current_seq) { + return true; } - if (failed) { - // only warn if the check failed on merit - if ((int)_mission.count > 0) { - PX4_WARN("mission check failed"); + if (_navigator->get_mission_result()->valid && (index < _mission.count)) { + if (goToItem(index, true) != PX4_OK) { + // Keep the old mission index (it was not updated by the interface) and report back. + return false; } - // reset the mission - _mission.count = 0; - _mission.current_seq = 0; - _current_mission_index = 0; + _is_current_planned_mission_item_valid = true; + + // we start from the first item so can reset the cache + if (_mission.current_seq == 0) { + resetItemCache(); + } + + // update mission items if already in active mission + if (isActive()) { + // prevent following "previous - current" line + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + } + + return true; } - // 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(); - - set_current_mission_item(); + return false; } - -void -Mission::advance_mission() +bool Mission::setNextMissionItem() { - /* do not advance mission item if we're processing sub mission work items */ - if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) { - return; - } - - switch (_mission_type) { - case MISSION_TYPE_MISSION: - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - _current_mission_index++; - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - // find next position item in reverse order - dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); - - for (int32_t i = _current_mission_index - 1; i >= 0; i--) { - struct mission_item_s missionitem = {}; - - 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. */ - PX4_ERR("dataman read failure"); - break; - } - - if (item_contains_position(missionitem)) { - _current_mission_index = i; - return; - } - } - - // finished flying back the mission - _current_mission_index = -1; - break; - } - - default: - _current_mission_index++; - } - - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::set_mission_items() -{ - /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = false; - - /* mission item that comes after current if available */ - struct mission_item_s mission_item_next_position; - struct mission_item_s mission_item_after_next_position; - bool has_next_position_item = false; - bool has_after_next_position_item = false; - - work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - - if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, - &mission_item_after_next_position, &has_after_next_position_item)) { - /* if mission type changed, notify */ - if (_mission_type != MISSION_TYPE_MISSION) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission\t" : - "Executing Mission\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_execute_rev"), events::Log::Info, "Executing Reverse Mission"); - - } else { - events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); - } - - user_feedback_done = true; - } - - _mission_type = MISSION_TYPE_MISSION; - - } else { - if (_mission_type != MISSION_TYPE_NONE) { - - if (_navigator->get_land_detected()->landed) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed\t" : - "Mission finished, landed\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_finished_rev"), events::Log::Info, "Reverse Mission finished, landed"); - - } else { - events::send(events::ID("mission_finished"), events::Log::Info, "Mission finished, landed"); - } - - } else { - /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering\t" : - "Mission finished, loitering\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_finished_rev_loiter"), events::Log::Info, "Reverse Mission finished, loitering"); - - } else { - events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); - } - } - - user_feedback_done = true; - } - - _mission_type = MISSION_TYPE_NONE; - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - if (_navigator->get_land_detected()->landed) { - _mission_item.nav_cmd = NAV_CMD_IDLE; - - } else { - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - setLoiterItemFromCurrentPositionSetpoint(&_mission_item); - - } else { - setLoiterItemFromCurrentPosition(&_mission_item); - } - - } - - /* update position setpoint triplet */ - pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - - // set mission finished - _navigator->get_mission_result()->finished = true; - _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); - - if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - * https://en.wikipedia.org/wiki/Loiter_(aeronautics) - */ - - if (_navigator->get_land_detected()->landed) { - /* landed, refusing to take off without a mission */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); - events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, refusing takeoff"); - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); - events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, loitering"); - } - - user_feedback_done = true; - } - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); - - return; - } - - /*********************************** handle mission item *********************************************/ - - /* handle mission items depending on the mode */ - - const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current; - - if (item_contains_position(_mission_item)) { - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - /* force vtol land */ - if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { - _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; - } - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* do climb before going to setpoint if needed and not already executing climb */ - /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ - - if (PX4_ISFINITE(_mission_init_climb_altitude_amsl) && - _work_item_type == WORK_ITEM_TYPE_DEFAULT && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_CLIMB; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; - has_next_position_item = true; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Climb to %.1f meters above home\t", - (double)(_mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt)); - events::send(events::ID("mission_climb_before_start"), events::Log::Info, - "Climb to {1:.1m_v} above home", _mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt); - - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - /* hold heading for takeoff items */ - _mission_item.yaw = _navigator->get_local_position()->heading; - _mission_item.altitude = _mission_init_climb_altitude_amsl; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - _mission_init_climb_altitude_amsl = NAN; - - } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - - } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - // if the vehicle is already in fixed wing mode then the current mission item - // will be accepted immediately and the work items will be skipped - _work_item_type = WORK_ITEM_TYPE_CLIMB; - - - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - } - - /* if we just did a normal takeoff navigate to the actual waypoint now */ - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_CLIMB && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - } - - /* if we just did a VTOL takeoff, prepare transition */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_CLIMB && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { - - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - - /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - - _mission_item.force_heading = true; - - new_work_item_type = WORK_ITEM_TYPE_ALIGN_HEADING; - - /* set position setpoint to current while aligning */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - } - - /* heading is aligned now, prepare transition */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_ALIGN_HEADING && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { - - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - - /* check if the vtol_takeoff waypoint is on top of us */ - if (do_need_move_to_takeoff()) { - new_work_item_type = WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; - } - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - _mission_item.yaw = _navigator->get_local_position()->heading; - - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - /* move to land wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && !_navigator->get_land_detected()->landed) { - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - has_next_position_item = true; - - float altitude = _navigator->get_global_position()->alt; - - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - _mission_item.vtol_back_transition = true; - } - - /* transition to MC */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_navigator->get_land_detected()->landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _navigator->get_global_position()->alt; - _mission_item.altitude_is_relative = false; - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; - - // make previous setpoint invalid, such that there will be no prev-current line following - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - pos_sp_triplet->previous.valid = false; - } - - /* move to landing waypoint before descent if necessary */ - if (do_need_move_to_land() && - (_work_item_type == WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - has_next_position_item = true; - - /* - * Ignoring waypoint altitude: - * Set altitude to the same as we have now to prevent descending too fast into - * the ground. Actual landing will descend anyway until it touches down. - * XXX: We might want to change that at some point if it is clear to the user - * what the altitude means on this waypoint type. - */ - float altitude = _navigator->get_global_position()->alt; - - if (pos_sp_triplet->current.valid - && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - // make previous setpoint invalid, such that there will be no prev-current line following. - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - pos_sp_triplet->previous.valid = false; - - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - } - - /* we just moved to the landing waypoint, now descend */ - if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - - } - - /* ignore yaw for landing items */ - /* XXX: if specified heading for landing is desired we could add another step before the descent - * that aligns the vehicle first */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { - _mission_item.yaw = NAN; - } - - - // for fast forward convert certain types to simple waypoint - // XXX: add other types which should be ignored in fast forward - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD && - ((_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) || - (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT))) { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - if (item_contains_position(_mission_item)) { - // convert mission item to a simple waypoint - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "MissionReverse: Got a non-position mission item, ignoring it\t"); - events::send(events::ID("mission_ignore_non_position_item"), events::Log::Info, - "MissionReverse: Got a non-position mission item, ignoring it"); - } - - break; - } - } - - } else { - /* handle non-position mission items such as commands */ - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* turn towards next waypoint before MC to FW transition */ - if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_navigator->get_land_detected()->landed - && has_next_position_item) { - - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - - new_work_item_type = WORK_ITEM_TYPE_ALIGN_HEADING; - - set_align_mission_item(&_mission_item, &mission_item_next_position); - - /* set position setpoint to target during the transition */ - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); - } - - /* yaw is aligned now */ - if (_work_item_type == WORK_ITEM_TYPE_ALIGN_HEADING && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - - pos_sp_triplet->previous = pos_sp_triplet->current; - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - // ignore certain commands in mission fast forward - if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) && - (_mission_item.nav_cmd == NAV_CMD_DELAY)) { - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - // nothing to do, all commands are ignored - break; - } - } - - if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; - } - } - - /*********************************** set setpoints and check next *********************************************/ - // The logic in this section establishes the tracking between the current waypoint - // which we are approaching and the next waypoint, which will tell us in which direction - // we will change our trajectory right after reaching it. - - // Because actions, gates and jump labels can be interleaved with waypoints, - // we are searching around the current mission item in the list to find the closest - // gate and the closest waypoint. We then store them separately. - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - // Check if the mission item is a gate along the current trajectory - if (item_contains_gate(_mission_item)) { - - // The mission item is a gate, let's check if the next item in the list provides - // a position to go towards. - - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (has_next_position_item) { - // We have a position, convert it to the setpoint and update setpoint triplet - mission_apply_limitation(mission_item_next_position); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); - } - - // ELSE: The current position setpoint stays unchanged. - - } else { - // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - } - - // ELSE: The current position setpoint stays unchanged. - } - - // Only set the previous position item if the current one really changed - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) && - !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { - pos_sp_triplet->previous = current_setpoint_copy; - } - - /* issue command if ready (will do nothing for position mission items) */ - issue_command(_mission_item); - - /* set current work item type */ - _work_item_type = new_work_item_type; - - reset_mission_item_reached(); - - if (_mission_type == MISSION_TYPE_MISSION) { - set_current_mission_item(); - } - - // If the mission item under evaluation contains a gate, we need to check if we have a next position item so - // the controller can fly the correct line between the current and next setpoint - if (item_contains_gate(_mission_item)) { - if (has_after_next_position_item) { - /* got next mission item, update setpoint triplet */ - mission_apply_limitation(mission_item_next_position); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); - - } else { - pos_sp_triplet->next.valid = false; - } - - } else { - // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout - // This is done by setting the position triplet's next position's valid flag to false, - // which makes the FlightTask disregard the next position - // TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior - // seems hacky, handle this more properly. - const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item)); - - if (_mission_item.autocontinue && !brake_for_hold) { - /* try to process next mission item */ - if (has_next_position_item) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); - - } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; - } - - } else { - /* vehicle will be paused on current waypoint, don't set next item */ - pos_sp_triplet->next.valid = false; - } - } - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); + return (goToNextItem(true) == PX4_OK); } bool Mission::do_need_move_to_land() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); } @@ -1324,11 +170,11 @@ Mission::do_need_move_to_land() bool Mission::do_need_move_to_takeoff() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); } @@ -1336,334 +182,416 @@ Mission::do_need_move_to_takeoff() return false; } -void -Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) +void Mission::setActiveMissionItems() { - if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - mission_item->lat = setpoint->lat; - mission_item->lon = setpoint->lon; - mission_item->altitude = setpoint->alt; + /* Get mission item that comes after current if available */ + constexpr static size_t max_num_next_items{2u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items; - } else { - mission_item->lat = _navigator->get_global_position()->lat; - mission_item->lon = _navigator->get_global_position()->lon; - mission_item->altitude = _navigator->get_global_position()->alt; - } + getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); - mission_item->altitude_is_relative = false; -} + mission_item_s next_mission_items[max_num_next_items]; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); -void -Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next) -{ - mission_item->nav_cmd = NAV_CMD_WAYPOINT; - copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); - mission_item->altitude_is_relative = false; - mission_item->autocontinue = true; - mission_item->time_inside = 0.0f; - mission_item->yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - mission_item_next->lat, mission_item_next->lon); - mission_item->force_heading = true; -} + for (size_t i = 0U; i < num_found_items; i++) { + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i], + reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); -void -Mission::heading_sp_update() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = - _navigator->get_position_setpoint_triplet(); - - // Only update if current triplet is valid - if (pos_sp_triplet->current.valid) { - - double point_from_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon - }; - double point_to_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon - }; - float yaw_offset = 0.0f; - - // Depending on ROI-mode, update heading - switch (_navigator->get_vroi().mode) { - case vehicle_roi_s::ROI_LOCATION: { - // ROI is a fixed location. Vehicle needs to point towards that location - point_to_latlon[0] = _navigator->get_vroi().lat; - point_to_latlon[1] = _navigator->get_vroi().lon; - // No yaw offset required - yaw_offset = 0.0f; - break; - } - - case vehicle_roi_s::ROI_WPNEXT: { - // ROI is current waypoint. Vehcile needs to point towards current waypoint - point_to_latlon[0] = pos_sp_triplet->current.lat; - point_to_latlon[1] = pos_sp_triplet->current.lon; - // Add the gimbal's yaw offset - yaw_offset = _navigator->get_vroi().yaw_offset; - break; - } - - case vehicle_roi_s::ROI_NONE: - case vehicle_roi_s::ROI_WPINDEX: - case vehicle_roi_s::ROI_TARGET: - case vehicle_roi_s::ROI_ENUM_END: - default: { - return; - } - } - - // Get desired heading and update it. - // However, only update if distance to desired heading is - // larger than acceptance radius to prevent excessive yawing - float d_current = get_distance_to_next_waypoint(point_from_latlon[0], - point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); - - if (d_current > _navigator->get_acceptance_radius()) { - float yaw = matrix::wrap_pi( - get_bearing_to_next_waypoint(point_from_latlon[0], - point_from_latlon[1], point_to_latlon[0], - point_to_latlon[1]) + yaw_offset); - - _mission_item.yaw = yaw; - pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; + if (success) { + next_mission_items[i] = next_mission_item; } else { - if (!pos_sp_triplet->current.yaw_valid) { - _mission_item.yaw = _navigator->get_local_position()->heading; - pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; - } + num_found_items = i; + break; + } + } + + /*********************************** handle mission item *********************************************/ + WorkItemType new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; + + if (item_contains_position(_mission_item)) { + /* force vtol land */ + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } - // we set yaw directly so we can run this in parallel to the FOH update - publish_navigator_mission_item(); - _navigator->set_position_setpoint_triplet_updated(); - } -} + handleTakeoff(new_work_item_type, next_mission_items, num_found_items); -void -Mission::do_abort_landing() -{ - // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT + handleLanding(new_work_item_type, next_mission_items, num_found_items); - if (_mission_item.nav_cmd != NAV_CMD_LAND) { - return; + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 + if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout + // This is done by setting the position triplet's next position's valid flag to false, + // which makes the FlightTask disregard the next position + // TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior + // seems hacky, handle this more properly. + const bool brake_for_hold = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item)); + + if (_mission_item.autocontinue && !brake_for_hold) { + /* try to process next mission item */ + if (num_found_items >= 1u) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + + } else { + /* vehicle will be paused on current waypoint, don't set next item */ + pos_sp_triplet->next.valid = false; + } + + } else if (item_contains_gate(_mission_item)) { + // The mission item is a gate, let's check if the next item in the list provides + // a position to go towards. + + if (num_found_items > 0u) { + // We have a position, convert it to the setpoint and update setpoint triplet + mission_apply_limitation(next_mission_items[0u]); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); + } + + if (num_found_items >= 2u) { + /* got next mission item, update setpoint triplet */ + mission_apply_limitation(next_mission_items[1u]); + mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next); + + } else { + pos_sp_triplet->next.valid = false; + } + + } else { + handleVtolTransition(new_work_item_type, next_mission_items, num_found_items); } - const float alt_landing = get_absolute_altitude_for_item(_mission_item); - const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), - _navigator->get_global_position()->alt); + // Only set the previous position item if the current one really changed + if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) && + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + pos_sp_triplet->previous = current_setpoint_copy; + } - // turn current landing waypoint into an indefinite loiter - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = alt_sp; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; + issue_command(_mission_item); - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + /* set current work item type */ + _work_item_type = new_work_item_type; - // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during - // the landing abort hold. otherwise, the "next" setpoint would still register as a "LAND" point, and trigger - // the early landing configuration (flaps and landing airspeed) during the hold. - _navigator->get_position_setpoint_triplet()->next.lat = (double)NAN; - _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; - _navigator->get_position_setpoint_triplet()->next.alt = NAN; + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } publish_navigator_mission_item(); // for logging _navigator->set_position_setpoint_triplet_updated(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", - (int)(alt_sp - alt_landing)); - events::send(events::ID("mission_holding_above_landing"), events::Log::Info, - "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); - - // reset mission index to start of landing - if (_land_start_available) { - _current_mission_index = get_land_start_index(); - - } else { - // move mission index back (landing approach point) - _current_mission_index -= 1; - } - - // send reposition cmd to get out of mission - vehicle_command_s vcmd = {}; - - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vcmd.param1 = -1; - vcmd.param2 = 1; - vcmd.param5 = _mission_item.lat; - vcmd.param6 = _mission_item.lon; - vcmd.param7 = alt_sp; - - _navigator->publish_vehicle_cmd(&vcmd); } -bool -Mission::prepare_mission_items(struct mission_item_s *mission_item, - struct mission_item_s *next_position_mission_item, bool *has_next_position_item, - struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item) +void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) { - *has_next_position_item = false; - bool first_res = false; - int offset = 1; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - offset = -1; - } + /* do climb before going to setpoint if needed and not already executing climb */ + /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ + if (PX4_ISFINITE(_mission_init_climb_altitude_amsl) && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { - if (read_mission_item(0, mission_item)) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; - first_res = true; + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + next_mission_items[0u].nav_cmd = NAV_CMD_WAYPOINT; - /* trying to find next position mission item */ - while (read_mission_item(offset, next_position_mission_item)) { - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - offset--; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Climb to %.1f meters above home\t", + (double)(_mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt)); + events::send(events::ID("mission_climb_before_start"), events::Log::Info, + "Climb to {1:.1m_v} above home", _mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt); - } else { - offset++; - } - - if (item_contains_position(*next_position_mission_item)) { - *has_next_position_item = true; - break; - } - } - - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && - after_next_position_mission_item && has_after_next_position_item) { - /* trying to find next next position mission item */ - while (read_mission_item(offset, after_next_position_mission_item)) { - offset++; - - if (item_contains_position(*after_next_position_mission_item)) { - *has_after_next_position_item = true; - break; - } - } - } - } - - return first_res; -} - -bool -Mission::read_mission_item(int offset, struct mission_item_s *mission_item) -{ - /* select mission */ - const int current_index = _current_mission_index; - int index_to_read = current_index + offset; - - int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read; - const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; - - /* do not work on empty missions */ - if (_mission.count == 0) { - return false; - } - - /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after - * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ - for (int i = 0; i < 10; i++) { - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_mission.count) { - /* mission item index out of bounds - if they are equal, we just reached the end */ - if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission item index out of bound, index: %d, max: %" PRIu16 ".\t", - *mission_index_ptr, _mission.count); - events::send(events::ID("mission_index_out_of_bound"), events::Log::Error, - "Mission item index out of bound, index: {1}, max: {2}", *mission_index_ptr, _mission.count); - } - - return false; - } - - /* read mission item to temp storage first to not overwrite current mission item if data damaged */ - struct mission_item_s mission_item_tmp; - - /* read mission item from datamanager */ - 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. */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); - events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, - "Waypoint {1} could not be read from storage", *mission_index_ptr); - return false; - } - - /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { - const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL; - - /* do DO_JUMP as many times as requested if not in reverse mode */ - if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) { - - /* only raise the repeat count if this is for the current mission item - * but not for the read ahead mission item */ - if (offset == 0) { - (mission_item_tmp.do_jump_current_count)++; - - /* save repeat count */ - success = _dataman_client.writeSync(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), - sizeof(struct mission_item_s)); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the dataman */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); - events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, - "DO JUMP waypoint could not be written"); - return false; - } - - report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count); - } - - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index_ptr = mission_item_tmp.do_jump_mission_index; - - } else { - if (offset == 0 && execute_jumps) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed.\t"); - events::send(events::ID("mission_do_jump_rep_completed"), events::Log::Info, - "DO JUMP repetitions completed"); - } - - /* no more DO_JUMPS, therefore just try to continue with next mission item */ - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - (*mission_index_ptr)--; - - } else { - (*mission_index_ptr)++; - } - } + if (_land_detected_sub.get().landed) { + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; } else { - /* if it's not a DO_JUMP, then we were successful */ - memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); - return true; + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + /* hold heading for takeoff items */ + _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.altitude = _mission_init_climb_altitude_amsl; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + _mission_init_climb_altitude_amsl = NAN; + + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + + } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // if the vehicle is already in fixed wing mode then the current mission item + // will be accepted immediately and the work items will be skipped + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a normal takeoff navigate to the actual waypoint now */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) { + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a VTOL takeoff, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + _mission_item.force_heading = true; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; + + /* set position setpoint to current while aligning */ + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + } + + /* heading is aligned now, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + /* check if the vtol_takeoff waypoint is on top of us */ + if (do_need_move_to_takeoff()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + } + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + + /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + } +} + +void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* move to land wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) + && !_land_detected_sub.get().landed) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + } + + /* transition to MC */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_land_detected_sub.get().landed) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + } + + /* move to landing waypoint before descent if necessary */ + if (do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + /* + * Ignoring waypoint altitude: + * Set altitude to the same as we have now to prevent descending too fast into + * the ground. Actual landing will descend anyway until it touches down. + * XXX: We might want to change that at some point if it is clear to the user + * what the altitude means on this waypoint type. + */ + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid + && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); } } - /* we have given up, we don't want to cycle forever */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.\t"); - events::send(events::ID("mission_do_jump_cycle"), events::Log::Error, "DO JUMP is cycling, giving up"); - return false; + /* we just moved to the landing waypoint, now descend */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + } + } + + /* ignore yaw for landing items */ + /* XXX: if specified heading for landing is desired we could add another step before the descent + * that aligns the vehicle first */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + _mission_item.yaw = NAN; + } +} + +void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* turn towards next waypoint before MC to FW transition */ + if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_land_detected_sub.get().landed + && (num_found_items > 0u)) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; + + set_align_mission_item(&_mission_item, &next_mission_items[0u]); + + /* set position setpoint to target during the transition */ + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); + } + + /* yaw is aligned now */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + pos_sp_triplet->previous = pos_sp_triplet->current; + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } } void Mission::save_mission_state() { - if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Save only while disarmed, as this is a blocking operation _need_mission_save = true; return; @@ -1678,11 +606,11 @@ Mission::save_mission_state() if (success) { /* data read successfully, check dataman ID and items count */ - if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) { + if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count + && mission_state.mission_update_counter && _mission.mission_update_counter) { /* navigator may modify only sequence, write modified state only if it changed */ - if (mission_state.current_seq != _current_mission_index) { - mission_state.current_seq = _current_mission_index; - mission_state.timestamp = hrt_absolute_time(); + if (mission_state.current_seq != _mission.current_seq) { + mission_state = _mission; success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), sizeof(mission_s)); @@ -1696,10 +624,7 @@ Mission::save_mission_state() } else { /* invalid data, this must not happen and indicates error in mission publisher */ - mission_state.timestamp = hrt_absolute_time(); - mission_state.dataman_id = _mission.dataman_id; - mission_state.count = _mission.count; - mission_state.current_seq = _current_mission_index; + mission_state = _mission; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Invalid mission state.\t"); /* EVENT @@ -1717,402 +642,3 @@ Mission::save_mission_state() } } } - -void -Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) -{ - /* inform about the change */ - _navigator->get_mission_result()->item_do_jump_changed = true; - _navigator->get_mission_result()->item_changed_index = index; - _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; - - _navigator->set_mission_result_updated(); -} - -void -Mission::set_mission_item_reached() -{ - _navigator->get_mission_result()->seq_reached = _current_mission_index; - _navigator->set_mission_result_updated(); - reset_mission_item_reached(); -} - -void -Mission::set_current_mission_item() -{ - _navigator->get_mission_result()->finished = false; - _navigator->get_mission_result()->seq_current = _current_mission_index; - - _navigator->set_mission_result_updated(); - - save_mission_state(); -} - -void -Mission::check_mission_valid(bool force) -{ - if ((!_home_inited && _navigator->home_global_position_valid()) || force) { - - MissionFeasibilityChecker _missionFeasibilityChecker(_navigator, _dataman_client); - - _navigator->get_mission_result()->valid = - _missionFeasibilityChecker.checkMissionFeasible(_mission); - - _navigator->get_mission_result()->seq_total = _mission.count; - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); - _home_inited = _navigator->home_global_position_valid(); - - // find and store landing start marker (if available) - find_mission_land_start(); - } -} - -void -Mission::reset_mission(struct mission_s &mission) -{ - 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 */ - mission.current_seq = 0; - - /* reset jump counters */ - if (mission.count > 0) { - 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(dataman_id, index, reinterpret_cast(&item), sizeof(mission_item_s), - 500_ms); - - if (!success) { - PX4_WARN("could not read mission item during reset"); - break; - } - - if (item.nav_cmd == NAV_CMD_DO_JUMP) { - item.do_jump_current_count = 0; - - success = _dataman_client.writeSync(dataman_id, index, reinterpret_cast(&item), len); - - if (!success) { - PX4_WARN("could not save mission item during reset"); - break; - } - } - } - } - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t"); - events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission"); - - /* initialize mission state in dataman */ - mission.timestamp = hrt_absolute_time(); - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.count = 0; - mission.current_seq = 0; - } - - _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); - } -} - -bool -Mission::need_to_reset_mission() -{ - // reset mission when disarmed, mission was actually started and we reached the last mission item - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset - && (_current_mission_index == _mission.count - 1)) { - _need_mission_reset = false; - return true; - } - - return false; -} - -int32_t -Mission::index_closest_mission_item() -{ - int32_t min_dist_index(0); - float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); - - dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); - - for (size_t i = 0; i < _mission.count; i++) { - struct mission_item_s missionitem = {}; - - bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s), 500_ms); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (item_contains_position(missionitem)) { - // do not consider land waypoints for a fw - if (!((missionitem.nav_cmd == NAV_CMD_LAND) && - (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && - (!_navigator->get_vstatus()->is_vtol))) { - float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon, - get_absolute_altitude_for_item(missionitem), - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (dist < min_dist) { - min_dist = dist; - min_dist_index = i; - } - } - } - } - - // for mission reverse also consider the home position - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - float dist = get_distance_to_point_global_wgs84( - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon, - _navigator->get_home_position()->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (dist < min_dist) { - min_dist = dist; - min_dist_index = -1; - } - } - - return min_dist_index; -} - -bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const -{ - return ((p1->valid == p2->valid) && - (p1->type == p2->type) && - (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && - (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && - (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && - (fabs(p1->lat - p2->lat) < DBL_EPSILON) && - (fabs(p1->lon - p2->lon) < DBL_EPSILON) && - (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && - ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && - (p1->yaw_valid == p2->yaw_valid) && - (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && - (p1->yawspeed_valid == p2->yawspeed_valid) && - (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && - (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && - (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && - (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && - ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) - && !PX4_ISFINITE(p2->cruising_throttle)))); - -} - -void Mission::publish_navigator_mission_item() -{ - navigator_mission_item_s navigator_mission_item{}; - - navigator_mission_item.instance_count = _navigator->mission_instance_count(); - navigator_mission_item.sequence_current = _current_mission_index; - navigator_mission_item.nav_cmd = _mission_item.nav_cmd; - navigator_mission_item.latitude = _mission_item.lat; - navigator_mission_item.longitude = _mission_item.lon; - navigator_mission_item.altitude = _mission_item.altitude; - - navigator_mission_item.time_inside = get_time_inside(_mission_item); - navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; - navigator_mission_item.loiter_radius = _mission_item.loiter_radius; - navigator_mission_item.yaw = _mission_item.yaw; - - navigator_mission_item.frame = _mission_item.frame; - navigator_mission_item.frame = _mission_item.origin; - - navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; - navigator_mission_item.force_heading = _mission_item.force_heading; - navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; - navigator_mission_item.autocontinue = _mission_item.autocontinue; - navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; - - navigator_mission_item.timestamp = hrt_absolute_time(); - - _navigator_mission_item_pub.publish(navigator_mission_item); -} - -bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactivation_index, - uint16_t &prev_pos_index) const -{ - - for (int index = inactivation_index; index >= 0; index--) { - 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(mission_item)) { - prev_pos_index = index; - return true; - } - } - - return false; -} - -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) { - // 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; - } - - start_index++; - } - - return false; -} - -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; - - case NAV_CMD_DO_CHANGE_SPEED: - _last_speed_change_item = mission_item; - break; - - case NAV_CMD_DO_VTOL_TRANSITION: - // delete speed changes after a VTOL transition - _last_speed_change_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::replayCachedSpeedChangeItems() -{ - if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { - issue_command(_last_speed_change_item); - _last_speed_change_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; - 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 (success) { - cacheItem(mission_item); - } - } -} - -void Mission::checkClimbRequired(uint16_t mission_item_index) -{ - mission_item_s next_position_mission_item = {}; - - if (getNextPositionMissionItem(_mission, mission_item_index, next_position_mission_item)) { - const float altitude_amsl_next_position_item = get_absolute_altitude_for_item(next_position_mission_item); - - const float error_below_setpoint = altitude_amsl_next_position_item - - _navigator->get_global_position()->alt; - - if (error_below_setpoint > _navigator->get_altitude_acceptance_radius()) { - - _mission_init_climb_altitude_amsl = altitude_amsl_next_position_item; - - } else { - - _mission_init_climb_altitude_amsl = NAN; - } - } -} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index c6a4dd9668..e8c33e1f0e 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -46,92 +46,32 @@ #pragma once -#include "mission_block.h" -#include "mission_feasibility_checker.h" -#include "navigator_mode.h" +#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "mission_base.h" +#include "navigation.h" class Navigator; -class Mission : public MissionBlock, public ModuleParams +class Mission : public MissionBase { public: Mission(Navigator *navigator); - ~Mission() override = default; + ~Mission() = default; - /** - * @brief function to call regularly to do background work - */ - void run(); - - void on_inactive() override; - void on_inactivation() override; - void on_activation() override; - void on_active() override; + virtual void on_inactive() override; + virtual void on_activation() override; bool set_current_mission_index(uint16_t index); - bool land_start(); - bool landing(); + uint16_t get_land_start_index() const { return _mission.land_start_index; } + bool get_land_start_available() const { return hasMissionLandStart(); } - uint16_t get_land_start_index() const { return _land_start_index; } - bool get_land_start_available() const { return _land_start_available; } - bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; } - bool get_mission_changed() const { return _mission_changed ; } - bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; } - double get_landing_start_lat() { return _landing_start_lat; } - double get_landing_start_lon() { return _landing_start_lon; } - float get_landing_start_alt() { return _landing_start_alt; } + bool isLanding(); - double get_landing_lat() { return _landing_lat; } - double get_landing_lon() { return _landing_lon; } - float get_landing_alt() { return _landing_alt; } - float get_landing_loiter_rad() { return _landing_loiter_radius; } - - void set_closest_item_as_current(); - - /** - * Set a new mission mode and handle the switching between the different modes - * - * For a list of the different modes refer to mission_result.msg - */ - void set_execution_mode(const uint8_t mode); private: - void mission_init(); - - /** - * Update mission topic - */ - void update_mission(); - - /** - * Move on to next mission item or switch to loiter - */ - void advance_mission(); - - /** - * @brief Configures mission items in current setting - * - * Configure the mission items depending on current mission item index and settings such - * as terrain following, etc. - */ - void set_mission_items(); + bool setNextMissionItem() override; /** * Returns true if we need to move to waypoint location before starting descent @@ -144,232 +84,23 @@ private: bool do_need_move_to_takeoff(); /** - * Copies position from setpoint if valid, otherwise copies current position + * Calculate takeoff height for mission item considering ground clearance */ - void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint); - - /** - * Create mission item to align towards next waypoint - */ - void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next); - - /** - * Updates the heading of the vehicle. Rotary wings only. - */ - void heading_sp_update(); - - /** - * Abort landing - */ - void do_abort_landing(); - - /** - * Read the current and the next mission item. The next mission item read is the - * next mission item that contains a position. - * - * @return true if current mission item available - */ - bool prepare_mission_items(mission_item_s *mission_item, - mission_item_s *next_position_mission_item, bool *has_next_position_item, - mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr); - - /** - * Read current (offset == 0) or a specific (offset > 0) mission item - * from the dataman and watch out for DO_JUMPS - * - * @return true if successful - */ - bool read_mission_item(int offset, struct mission_item_s *mission_item); + float calculate_takeoff_altitude(struct mission_item_s *mission_item); /** * Save current mission state to dataman */ void save_mission_state(); - /** - * Inform about a changed mission item after a DO_JUMP - */ - void report_do_jump_mission_changed(int index, int do_jumps_remaining); + void setActiveMissionItems() override; - /** - * Set a mission item as reached - */ - void set_mission_item_reached(); + void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - /** - * Set the current mission item - */ - void set_current_mission_item(); + void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - /** - * Check whether a mission is ready to go - */ - void check_mission_valid(bool force); + void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items); - /** - * Reset mission - */ - void reset_mission(struct mission_s &mission); - - /** - * Returns true if we need to reset the mission (call this only when inactive) - */ - bool need_to_reset_mission(); - - /** - * Find and store the index of the landing sequence (DO_LAND_START) - */ - bool find_mission_land_start(); - - /** - * Return the index of the closest mission item to the current global position. - */ - int32_t index_closest_mission_item(); - - bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; - - 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, uint16_t &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); - - /** - * @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 Replay the cached speed change items and delete them afterwards - * - */ - void replayCachedSpeedChangeItems(); - - /** - * @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(); - - /** - * @brief Check if a climb is necessary to align with mission altitude prior to starting the mission - * - * @param mission_item_index The index of the mission item to check if a climb is necessary - */ - void checkClimbRequired(uint16_t mission_item_index); - - DEFINE_PARAMETERS( - (ParamFloat) _param_mis_dist_1wp, - (ParamInt) _param_mis_mnt_yaw_ctl - ) - - uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; - - uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ - mission_s _mission {}; - - 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 - bool _land_start_available{false}; - uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */ - double _landing_start_lat{0.0}; - double _landing_start_lon{0.0}; - float _landing_start_alt{0.0f}; - - double _landing_lat{0.0}; - double _landing_lon{0.0}; - float _landing_alt{0.0f}; - - float _landing_loiter_radius{0.f}; - - float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_MISSION - } _mission_type{MISSION_TYPE_NONE}; - - bool _inited{false}; - bool _home_inited{false}; - bool _need_mission_reset{false}; bool _need_mission_save{false}; - bool _mission_waypoints_changed{false}; - bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ - - // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message - enum work_item_type { - WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ - WORK_ITEM_TYPE_CLIMB, /**< climb at current position before moving to waypoint */ - WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ - WORK_ITEM_TYPE_ALIGN_HEADING, /**< align heading for next waypoint */ - WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF, - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, - WORK_ITEM_TYPE_PRECISION_LAND - } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ - - 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 {}; - mission_item_s _last_speed_change_item {}; }; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp new file mode 100644 index 0000000000..ebdd026e74 --- /dev/null +++ b/src/modules/navigator/mission_base.cpp @@ -0,0 +1,1235 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mission_base.cpp + * + * Mission base mode class that can be used for modes interacting with a mission. + * + */ + +#include + +#include "mission_base.h" + +#include "px4_platform_common/defines.h" + +#include "mission_feasibility_checker.h" +#include "navigator.h" + +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : + MissionBlock(navigator), + ModuleParams(navigator), + _dataman_cache_size_signed(dataman_cache_size_signed) +{ + _dataman_cache.resize(abs(dataman_cache_size_signed)); + _is_current_planned_mission_item_valid = (initMission() == PX4_OK); + + updateDatamanCache(); + + _mission_pub.advertise(); +} + +int MissionBase::initMission() +{ + mission_s mission; + int ret_val{PX4_ERROR}; + + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), + sizeof(mission_s)); + + if (success) { + if (isMissionValid(mission)) { + _mission = mission; + ret_val = PX4_OK; + + } else { + resetMission(); + } + + } else { + PX4_ERR("Could not initialize Mission: Dataman read failed"); + resetMission(); + } + + return ret_val; +} + +void +MissionBase::updateDatamanCache() +{ + if ((_mission.count > 0) && (_mission.current_seq != _load_mission_index)) { + + int32_t start_index = _mission.current_seq; + int32_t end_index = start_index + _dataman_cache_size_signed; + + end_index = math::max(math::min(end_index, static_cast(_mission.count)), INT32_C(0)); + + for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) { + + _dataman_cache.load(static_cast(_mission.dataman_id), index); + } + + _load_mission_index = _mission.current_seq; + } + + _dataman_cache.update(); +} + +void MissionBase::updateMavlinkMission() +{ + if (_mission_sub.updated()) { + mission_s new_mission; + _mission_sub.update(&new_mission); + + if (isMissionValid(new_mission)) { + /* Relevant mission items updated externally*/ + if (checkMissionDataChanged(new_mission)) { + bool mission_items_changed = (new_mission.mission_update_counter != _mission.mission_update_counter); + + if (new_mission.current_seq < 0) { + new_mission.current_seq = math::max(math::min(_mission.current_seq, static_cast(new_mission.count) - 1), + INT32_C(0)); + } + + _mission = new_mission; + + onMissionUpdate(mission_items_changed); + } + } + } +} + +void MissionBase::onMissionUpdate(bool has_mission_items_changed) +{ + _is_current_planned_mission_item_valid = _mission.count > 0; + + if (has_mission_items_changed) { + _dataman_cache.invalidate(); + _load_mission_index = -1; + + check_mission_valid(); + + // only warn if the check failed on merit + if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) { + PX4_WARN("mission check failed"); + resetMission(); + } + } + + if (isActive()) { + _mission_has_been_activated = true; + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + + } else { + if (has_mission_items_changed) { + _mission_has_been_activated = false; + } + } + + // reset as when we update mission we don't want to proceed at previous index + _inactivation_index = -1; +} + +void +MissionBase::on_inactive() +{ + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _global_pos_sub.update(); + + parameters_update(); + + updateMavlinkMission(); + + /* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */ + if (_navigator->home_global_position_valid() && !_initialized_mission_checked) { + check_mission_valid(); + _initialized_mission_checked = true; + } + + if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + _system_disarmed_while_inactive = true; + } +} + +void +MissionBase::on_inactivation() +{ + _navigator->disable_camera_trigger(); + + _navigator->stop_capturing_images(); + _navigator->release_gimbal_control(); + + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } + + /* reset so current mission item gets restarted if mission was paused */ + _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* reset so MISSION_ITEM_REACHED isn't published */ + _navigator->get_mission_result()->seq_reached = -1; + + _mission_type = MissionType::MISSION_TYPE_NONE; + + _inactivation_index = _mission.current_seq; +} + +void +MissionBase::on_activation() +{ + /* reset the current mission to the start sequence if needed.*/ + checkMissionRestart(); + + _mission_has_been_activated = true; + _system_disarmed_while_inactive = false; + + check_mission_valid(); + + update_mission(); + + // reset the cache and fill it with the items up to the previous item. The cache contains + // commands that are valid for the whole mission, not just a single waypoint. + if (_mission.current_seq > 0) { + resetItemCache(); + updateCachedItemsUpToIndex(_mission.current_seq - 1); + } + + int32_t resume_index = _inactivation_index > 0 ? _inactivation_index : 0; + + if (_inactivation_index > 0 && cameraWasTriggering()) { + size_t num_found_items{0U}; + getPreviousPositionItems(_inactivation_index - 1, &resume_index, num_found_items, 1U); + + if (num_found_items == 1U) { + // 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. + setMissionIndex(resume_index); + + _align_heading_necessary = true; + } + } + + checkClimbRequired(resume_index); + set_mission_items(); + + _inactivation_index = -1; // reset + + // reset cruise speed + _navigator->reset_cruising_speed(); +} + +void +MissionBase::on_active() +{ + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _global_pos_sub.update(); + + parameters_update(); + + updateMavlinkMission(); + updateDatamanCache(); + + // 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()) { + + // add yaw alignment requirement on the current mission item + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items == 1U && !PX4_ISFINITE(_mission_item.yaw)) { + mission_item_s next_position_mission_item; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + _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(); + } + + replayCachedSpeedChangeItems(); + + /* lets check if we reached the current mission item */ + if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { + /* If we just completed a takeoff which was inserted before the right waypoint, + there is no need to report that we reached it because we didn't. */ + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_CLIMB) { + set_mission_item_reached(); + } + + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + } + } + + /* see if we need to update the current yaw heading */ + if (!_param_mis_mnt_yaw_ctl.get() + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) + && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + || _mission_item.nav_cmd == NAV_CMD_LAND + || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING)) { + // Mount control is disabled If the vehicle is in ROI-mode, the vehicle + // needs to rotate such that ROI is in the field of view. + // ROI only makes sense for multicopters. + heading_sp_update(); + } + + // TODO: Add vtol heading update method if required. + // Question: Why does vtol ever have to update heading? + + /* check if landing needs to be aborted */ + if ((_mission_item.nav_cmd == NAV_CMD_LAND) + && (_navigator->abort_landing())) { + + do_abort_landing(); + } + + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void MissionBase::update_mission() +{ + if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_navigator->get_mission_result()->valid) { + if (_land_detected_sub.get().landed) { + /* landed, refusing to take off without a mission */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); + events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, + "No valid mission available, refusing takeoff"); + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); + events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, + "No valid mission available, loitering"); + } + + _mission_type = MissionType::MISSION_TYPE_NONE; + + } else { + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing Mission\t"); + events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); + } + + _mission_type = MissionType::MISSION_TYPE_MISSION; + } + + /* Reset vehicle_roi + * Missions that do not explicitly configure ROI would not override + * an existing ROI setting from previous missions */ + _navigator->reset_vroi(); + + if (_navigator->get_mission_result()->valid) { + /* reset work item if new mission has been accepted */ + _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; + + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; + } + + // we start from the first item so can reset the cache + if (_mission.current_seq == 0) { + resetItemCache(); + } + + set_mission_result(); +} + +void +MissionBase::advance_mission() +{ + /* do not advance mission item if we're processing sub mission work items */ + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + return; + } + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + _is_current_planned_mission_item_valid = setNextMissionItem(); + + if (!_is_current_planned_mission_item_valid) { + // Mission ended + if (_land_detected_sub.get().landed) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, landed\t"); + + events::send(events::ID("mission_finished"), events::Log::Info, "Mission finished, landed"); + + } else { + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, loitering\t"); + + events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); + } + + // Reset jump counter if the mission was completed + if ((_mission.current_seq + 1) == _mission.count) { + resetMissionJumpCounter(); + } + } + } +} + +void +MissionBase::set_mission_items() +{ + if (_is_current_planned_mission_item_valid) { + /* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */ + loadCurrentMissionItem(); + + setActiveMissionItems(); + + } else { + setEndOfMissionItems(); + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + } +} + +void MissionBase::loadCurrentMissionItem() +{ + const dm_item_t dm_item = static_cast(_mission.dataman_id); + bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast(&_mission_item), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item could not be set.\t"); + events::send(events::ID("mission_item_set_failed"), events::Log::Error, + "Mission item could not be set"); + } +} + +void MissionBase::setEndOfMissionItems() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + if (_land_detected_sub.get().landed) { + _mission_item.nav_cmd = NAV_CMD_IDLE; + + } else { + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + + } else { + setLoiterItemFromCurrentPosition(&_mission_item); + } + } + + /* update position setpoint triplet */ + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + // set mission finished + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + _mission_type = MissionType::MISSION_TYPE_NONE; +} + +void +MissionBase::set_mission_item_reached() +{ + _navigator->get_mission_result()->seq_reached = _mission.current_seq; + _navigator->set_mission_result_updated(); + + reset_mission_item_reached(); +} + +void +MissionBase::set_mission_result() +{ + _navigator->get_mission_result()->finished = false; + _navigator->get_mission_result()->seq_current = _mission.current_seq > 0 ? _mission.current_seq : 0; + + _navigator->set_mission_result_updated(); +} + +bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const +{ + return ((p1->valid == p2->valid) && + (p1->type == p2->type) && + (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && + (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && + (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && + (fabs(p1->lat - p2->lat) < DBL_EPSILON) && + (fabs(p1->lon - p2->lon) < DBL_EPSILON) && + (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && + ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && + (p1->yaw_valid == p2->yaw_valid) && + (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && + (p1->yawspeed_valid == p2->yawspeed_valid) && + (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && + (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && + (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && + (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && + ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) + && !PX4_ISFINITE(p2->cruising_throttle)))); + +} + +void +MissionBase::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + + _navigator->set_mission_result_updated(); +} + +void +MissionBase::checkMissionRestart() +{ + if (_system_disarmed_while_inactive && _mission_has_been_activated && (_mission.count > 0U) + && ((_mission.current_seq + 1) == _mission.count)) { + setMissionIndex(0); + _is_current_planned_mission_item_valid = isMissionValid(_mission); + resetMissionJumpCounter(); + _navigator->reset_cruising_speed(); + _navigator->reset_vroi(); + set_mission_result(); + } +} + +void +MissionBase::check_mission_valid() +{ + if (_navigator->get_mission_result()->instance_count != _mission.mission_update_counter) { + MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client); + + bool is_mission_valid = + missionFeasibilityChecker.checkMissionFeasible(_mission); + + _navigator->get_mission_result()->valid = is_mission_valid; + _navigator->get_mission_result()->instance_count = _mission.mission_update_counter; + _navigator->get_mission_result()->seq_total = _mission.count; + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->failure = false; + set_mission_result(); + } +} + +void +MissionBase::heading_sp_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = + _navigator->get_position_setpoint_triplet(); + + // Only update if current triplet is valid + if (pos_sp_triplet->current.valid) { + + double point_from_latlon[2] = { _global_pos_sub.get().lat, + _global_pos_sub.get().lon + }; + double point_to_latlon[2] = { _global_pos_sub.get().lat, + _global_pos_sub.get().lon + }; + float yaw_offset = 0.0f; + + // Depending on ROI-mode, update heading + switch (_navigator->get_vroi().mode) { + case vehicle_roi_s::ROI_LOCATION: { + // ROI is a fixed location. Vehicle needs to point towards that location + point_to_latlon[0] = _navigator->get_vroi().lat; + point_to_latlon[1] = _navigator->get_vroi().lon; + // No yaw offset required + yaw_offset = 0.0f; + break; + } + + case vehicle_roi_s::ROI_WPNEXT: { + // ROI is current waypoint. Vehcile needs to point towards current waypoint + point_to_latlon[0] = pos_sp_triplet->current.lat; + point_to_latlon[1] = pos_sp_triplet->current.lon; + // Add the gimbal's yaw offset + yaw_offset = _navigator->get_vroi().yaw_offset; + break; + } + + case vehicle_roi_s::ROI_NONE: + case vehicle_roi_s::ROI_WPINDEX: + case vehicle_roi_s::ROI_TARGET: + case vehicle_roi_s::ROI_ENUM_END: + default: { + return; + } + } + + // Get desired heading and update it. + // However, only update if distance to desired heading is + // larger than acceptance radius to prevent excessive yawing + float d_current = get_distance_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); + + if (d_current > _navigator->get_acceptance_radius()) { + float yaw = matrix::wrap_pi( + get_bearing_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], + point_to_latlon[1]) + yaw_offset); + + _mission_item.yaw = yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + + } else { + if (!pos_sp_triplet->current.yaw_valid) { + _mission_item.yaw = _navigator->get_local_position()->heading; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + } + } + + // we set yaw directly so we can run this in parallel to the FOH update + publish_navigator_mission_item(); + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void +MissionBase::do_abort_landing() +{ + // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + return; + } + + if (_mission_item.nav_cmd != NAV_CMD_LAND) { + return; + } + + const float alt_landing = get_absolute_altitude_for_item(_mission_item); + const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), + _global_pos_sub.get().alt); + + // turn current landing waypoint into an indefinite loiter + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = alt_sp; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during + // the landing abort hold. otherwise, the "next" setpoint would still register as a "LAND" point, and trigger + // the early landing configuration (flaps and landing airspeed) during the hold. + _navigator->get_position_setpoint_triplet()->next.lat = (double)NAN; + _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; + _navigator->get_position_setpoint_triplet()->next.alt = NAN; + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", + (int)(alt_sp - alt_landing)); + events::send(events::ID("mission_holding_above_landing"), events::Log::Info, + "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); + + // reset mission index to start of landing + if (hasMissionLandStart()) { + _is_current_planned_mission_item_valid = true; + setMissionIndex(_mission.land_start_index); + + } else { + // move mission index back (landing approach point) + _is_current_planned_mission_item_valid = goToPreviousItem(false); + } + + // send reposition cmd to get out of mission + vehicle_command_s vcmd = {}; + + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.param1 = -1; + vcmd.param2 = 1; + vcmd.param5 = _mission_item.lat; + vcmd.param6 = _mission_item.lon; + vcmd.param7 = alt_sp; + + _navigator->publish_vehicle_cmd(&vcmd); +} + +void MissionBase::publish_navigator_mission_item() +{ + navigator_mission_item_s navigator_mission_item{}; + + navigator_mission_item.instance_count = _navigator->mission_instance_count(); + navigator_mission_item.sequence_current = _mission.current_seq; + navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.latitude = _mission_item.lat; + navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.altitude = _mission_item.altitude; + + navigator_mission_item.time_inside = get_time_inside(_mission_item); + navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; + navigator_mission_item.loiter_radius = _mission_item.loiter_radius; + navigator_mission_item.yaw = _mission_item.yaw; + + navigator_mission_item.frame = _mission_item.frame; + navigator_mission_item.frame = _mission_item.origin; + + navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; + navigator_mission_item.force_heading = _mission_item.force_heading; + navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; + navigator_mission_item.autocontinue = _mission_item.autocontinue; + navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; + + navigator_mission_item.timestamp = hrt_absolute_time(); + + _navigator_mission_item_pub.publish(navigator_mission_item); +} + +bool MissionBase::isMissionValid(mission_s &mission) const +{ + bool ret_val{false}; + + if (((mission.current_seq < mission.count) || (mission.count == 0U && mission.current_seq <= 0)) && + (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && + (mission.timestamp != 0u)) { + ret_val = true; + + } + + return ret_val; +} + +int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, + bool write_jumps, bool mission_direction_backward) +{ + if (mission_index >= _mission.count || mission_index < 0) { + return PX4_ERROR; + } + + const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; + int32_t new_mission_index{mission_index}; + mission_item_s new_mission; + + for (uint16_t jump_count = 0u; jump_count < MAX_JUMP_ITERATION; jump_count++) { + /* read mission item from datamanager */ + bool success = _dataman_cache.loadWait(dataman_id, new_mission_index, reinterpret_cast(&new_mission), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); + events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, + "Waypoint {1} could not be read from storage", new_mission_index); + return PX4_ERROR; + } + + if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) { + if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) { + PX4_ERR("Do Jump mission index is out of bounds."); + return PX4_ERROR; + } + + if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) { + if (write_jumps) { + new_mission.do_jump_current_count++; + success = _dataman_cache.writeWait(dataman_id, new_mission_index, reinterpret_cast(&new_mission), + sizeof(struct mission_item_s)); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); + events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, + "DO JUMP waypoint could not be written"); + // Still continue searching for next non jump item. + } + + report_do_jump_mission_changed(new_mission_index, new_mission.do_jump_repeat_count - new_mission.do_jump_current_count); + } + + new_mission_index = new_mission.do_jump_mission_index; + + } else { + if (mission_direction_backward) { + new_mission_index--; + + } else { + new_mission_index++; + } + } + + } else { + break; + } + } + + mission_index = new_mission_index; + mission = new_mission; + + return PX4_OK; +} + +int MissionBase::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward) +{ + mission_item_s mission_item; + + if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == PX4_OK) { + setMissionIndex(index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +void MissionBase::setMissionIndex(int32_t index) +{ + if (index != _mission.current_seq) { + _mission.current_seq = index; + _mission.timestamp = hrt_absolute_time(); + _mission_pub.publish(_mission); + } +} + +void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items) +{ + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index < 0) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + next_mission_index--; + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == PX4_OK; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items_index[item_idx] = next_mission_index; + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items) +{ + // Make sure vector does not contain any preexisting elements. + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index >= _mission.count) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == PX4_OK; + next_mission_index++; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items_index[item_idx] = math::max(next_mission_index - 1, + static_cast(0)); // subtract 1 to get the index of the first position item + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +int MissionBase::goToNextItem(bool execute_jump) +{ + if (_mission.current_seq + 1 >= (_mission.count)) { + return PX4_ERROR; + } + + return goToItem(_mission.current_seq + 1, execute_jump); +} + +int MissionBase::goToPreviousItem(bool execute_jump) +{ + if (_mission.current_seq <= 0) { + return PX4_ERROR; + } + + return goToItem(_mission.current_seq - 1, execute_jump, true); +} + +int MissionBase::goToPreviousPositionItem(bool execute_jump) +{ + size_t num_found_items{0U}; + int32_t previous_position_item_index; + getPreviousPositionItems(_mission.current_seq, &previous_position_item_index, num_found_items, 1); + + if (num_found_items == 1U) { + setMissionIndex(previous_position_item_index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int MissionBase::goToNextPositionItem(bool execute_jump) +{ + size_t num_found_items{0U}; + int32_t next_position_item_index; + getNextPositionItems(_mission.current_seq + 1, &next_position_item_index, num_found_items, 1); + + if (num_found_items == 1U) { + setMissionIndex(next_position_item_index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, float home_alt, + const vehicle_status_s &vehicle_status) +{ + int32_t min_dist_index(-1); + float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) { + mission_item_s mission; + + bool success = _dataman_cache.loadWait(dataman_id, mission_item_index, reinterpret_cast(&mission), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not set mission closest to position.\t"); + events::send(events::ID("mission_failed_set_closest"), events::Log::Error, + "Could not set mission closest to position"); + return PX4_ERROR; + } + + if (MissionBlock::item_contains_position(mission)) { + // do not consider land waypoints for a fw + if (!((mission.nav_cmd == NAV_CMD_LAND) && + (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (!vehicle_status.is_vtol))) { + float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon, + MissionBlock::get_absolute_altitude_for_item(mission, home_alt), + lat, + lon, + alt, + &dist_xy, &dist_z); + + if (dist < min_dist) { + min_dist = dist; + min_dist_index = mission_item_index; + } + } + } + } + + setMissionIndex(min_dist_index); + + return PX4_OK; +} + +void MissionBase::resetMission() +{ + /* we do not need to reset mission if is already.*/ + if (_mission.count == 0u && isMissionValid(_mission)) { + return; + } + + /* Set a new mission*/ + mission_s new_mission{_mission}; + new_mission.timestamp = hrt_absolute_time(); + new_mission.current_seq = 0; + new_mission.land_start_index = -1; + new_mission.land_index = -1; + new_mission.count = 0u; + new_mission.mission_update_counter = _mission.mission_update_counter + 1; + new_mission.dataman_id = _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0; + + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&new_mission), + sizeof(mission_s)); + + if (success) { + _mission = new_mission; + _mission_pub.publish(_mission); + + } else { + PX4_ERR("Mission Initialization failed."); + } +} + +void MissionBase::resetMissionJumpCounter() +{ + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) { + mission_item_s mission_item; + + bool success = _dataman_client.readSync(dataman_id, mission_index, reinterpret_cast(&mission_item), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission could not reset jump count.\t"); + events::send(events::ID("mission_failed_set_jump_count"), events::Log::Error, + "Mission could not reset jump count"); + break; + } + + if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) { + mission_item.do_jump_current_count = 0u; + + bool write_success = _dataman_cache.writeWait(dataman_id, mission_index, reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); + + if (!write_success) { + PX4_ERR("Could not write mission item for jump count reset."); + break; + } + } + } +} + +void MissionBase::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; + + case NAV_CMD_DO_CHANGE_SPEED: + _last_speed_change_item = mission_item; + break; + + case NAV_CMD_DO_VTOL_TRANSITION: + // delete speed changes after a VTOL transition + _last_speed_change_item = {}; + break; + + default: + break; + } +} + +void MissionBase::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 MissionBase::replayCachedTriggerItems() +{ + if (_last_camera_trigger_item.nav_cmd > 0) { + issue_command(_last_camera_trigger_item); + _last_camera_trigger_item = {}; // delete cached item + } +} + +void MissionBase::replayCachedSpeedChangeItems() +{ + if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { + issue_command(_last_speed_change_item); + _last_speed_change_item = {}; // delete cached item + } +} + +void MissionBase::resetItemCache() +{ + _last_gimbal_configure_item = {}; + _last_gimbal_control_item = {}; + _last_camera_mode_item = {}; + _last_camera_trigger_item = {}; +} + +bool MissionBase::haveCachedGimbalOrCameraItems() +{ + return _last_gimbal_configure_item.nav_cmd > 0 || + _last_gimbal_control_item.nav_cmd > 0 || + _last_camera_mode_item.nav_cmd > 0; +} + +bool MissionBase::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 MissionBase::updateCachedItemsUpToIndex(const int end_index) +{ + for (int i = 0; i <= end_index; i++) { + 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 (success) { + cacheItem(mission_item); + } + } +} + +void MissionBase::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} + +void MissionBase::checkClimbRequired(int32_t mission_item_index) +{ + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(mission_item_index, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items > 0U) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s mission; + _mission_init_climb_altitude_amsl = NAN; // default to NAN, overwrite below if applicable + + const bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, reinterpret_cast(&mission), + sizeof(mission), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + const float altitude_amsl_next_position_item = MissionBlock::get_absolute_altitude_for_item(mission); + const float error_below_setpoint = altitude_amsl_next_position_item - + _navigator->get_global_position()->alt; + + if (error_below_setpoint > _navigator->get_altitude_acceptance_radius()) { + + _mission_init_climb_altitude_amsl = altitude_amsl_next_position_item; + } + } + } +} + +bool MissionBase::checkMissionDataChanged(mission_s new_mission) +{ + /* count and land_index are the same if the mission_counter did not change. We do not care about changes in geofence or rally counters.*/ + return ((new_mission.dataman_id != _mission.dataman_id) || + (new_mission.mission_update_counter != _mission.mission_update_counter) || + (new_mission.current_seq != _mission.current_seq)); +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h new file mode 100644 index 0000000000..54a24fefa1 --- /dev/null +++ b/src/modules/navigator/mission_base.h @@ -0,0 +1,446 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mission_base.h + * + * Mission base mode class that can be used for modes interacting with a mission. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mission_block.h" +#include "navigation.h" + +using namespace time_literals; + +class Navigator; + +class MissionBase : public MissionBlock, public ModuleParams +{ +public: + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + ~MissionBase() override = default; + + virtual void on_inactive() override; + virtual void on_inactivation() override; + virtual void on_activation() override; + virtual void on_active() override; + +protected: + + /** + * @brief Maximum time to wait for dataman loading + * + */ + static constexpr hrt_abstime MAX_DATAMAN_LOAD_WAIT{500_ms}; + + // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message + enum class WorkItemType { + WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ + WORK_ITEM_TYPE_CLIMB, /**< takeoff before moving to waypoint */ + WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ + WORK_ITEM_TYPE_ALIGN_HEADING, /**< align for next waypoint */ + WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF, + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, + WORK_ITEM_TYPE_PRECISION_LAND + } _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ + + enum class MissionType { + MISSION_TYPE_NONE, + MISSION_TYPE_MISSION + } _mission_type{MissionType::MISSION_TYPE_NONE}; + + /** + * @brief Get the Previous Mission Position Items + * + * @param[in] start_index is the index from where to start searching the previous mission position items + * @param[out] items_index is an array of indexes indicating the previous mission position items found + * @param[out] num_found_items are the amount of previous position items found + * @param[in] max_num_items are the maximum amount of previous position items to be searched + */ + void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items); + /** + * @brief Get the next mission item containing a position setpoint + * + * @param[in] start_index is the index from where to start searching (first possible return index) + * @param[out] items_index is an array of indexes indicating the next mission position items found + * @param[out] num_found_items are the amount of next position items found + * @param[in] max_num_items are the maximum amount of next position items to be searched + */ + void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items); + /** + * @brief Has Mission a Land Start or Land Item + * + * @return true If mission has a land start of land item + * @return false otherwise + */ + bool hasMissionLandStart() const { return _mission.land_start_index > 0;}; + /** + * @brief Go to next Mission Item + * Go to next non jump mission item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextItem(bool execute_jump); + /** + * @brief Go to previous Mission Item + * Go to previous non jump mission item + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousItem(bool execute_jump); + /** + * @brief Go to Mission Item + * + * @param[in] index Index of the mission item to go to + * @param[in] execute_jump Flag indicating if a jump should be executed of ignored + * @param[in] mission_direction_backward Flag indicating if a mission is flown backward + * @return PX4_OK if the mission item exists, PX4_ERR otherwise + */ + int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false); + /** + * @brief Go To Previous Mission Position Item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousPositionItem(bool execute_jump); + /** + * @brief Go To Next Mission Position Item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextPositionItem(bool execute_jump); + /** + * @brief Go to Mission Land Start Item + * + * @return PX4_OK if land start item exists and is loaded, PX4_ERR otherwise + */ + int goToMissionLandStart(); + /** + * @brief Set the Mission to closest mission position item from current position + * + * @param[in] lat latitude of the current position + * @param[in] lon longitude of the current position + * @param[in] alt altitude of the current position + * @param[in] home_alt altitude of the home position + * @param[in] vehicle_status vehicle status struct + * @return PX4_OK if closest item is found and loaded, PX4_ERR otherwise + */ + int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status); + /** + * @brief Initialize Mission + * + * @return PX4_OK if mission could be loaded, PX4_ERR otherwise + */ + int initMission(); + /** + * @brief Reset Mission + * + */ + void resetMission(); + /** + * @brief Reset Mission Jump Counter of Mission Jump Items + * + */ + void resetMissionJumpCounter(); + /** + * @brief Get the Non Jump Mission Item + * + * @param[out] mission_index Index of the mission item + * @param[out] mission The return mission item + * @param execute_jump Flag indicating if a jump item should be executed or ignored + * @param write_jumps Flag indicating if the jump counter should be updated + * @param mission_direction_backward Flag indicating if the mission is flown backwards + * @return PX4_OK if mission item could be loaded, PX4_ERR otherwise + */ + int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps, + bool mission_direction_backward = false); + /** + * @brief Is Mission Parameters Valid + * + * @param mission Mission struct + * @return true is mission parameters are valid + * @return false otherwise + */ + bool isMissionValid(mission_s &mission) const; + + /** + * On mission update + * Change behaviour after external mission update. + * @param[in] has_mission_items_changed flag if the mission items have been changed. + */ + void onMissionUpdate(bool has_mission_items_changed); + + /** + * Update mission topic + */ + void update_mission(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * @brief Configures mission items in current setting + * + * Configure the mission items depending on current mission item index and settings such + * as terrain following, etc. + */ + void set_mission_items(); + + /** + * @brief Load current mission item + * + * Load current mission item from dataman cache. + */ + void loadCurrentMissionItem(); + + /** + * Set the mission result + */ + void set_mission_result(); + + /** + * @brief Reset the item cache + */ + void resetItemCache(); + + /** + * @brief Set the actions to be performed on Active Mission Item + * + */ + virtual void setActiveMissionItems() = 0; + /** + * @brief Set the Next Mission Item after old mission item has been completed + * + * @return true if the next mission item could be set + * @return false otherwise + */ + virtual bool setNextMissionItem() = 0; + /** + * @brief Set action at the end of the mission + * + */ + void setEndOfMissionItems(); + /** + * @brief Publish navigator mission item + * + */ + void publish_navigator_mission_item(); + /** + * @brief I position setpoint equal + * + * @param p1 First position setpoint to compare + * @param p2 Second position setpoint to compare + * @return true if both setpoints are equal + * @return false otherwise + */ + bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + + bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ + bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ + bool _initialized_mission_checked{false}; /**< Flag indicating if the initialized mission has been checked by the mission validator*/ + bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/ + mission_s _mission; /**< Currently active mission*/ + float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ + + DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/ + DatamanClient &_dataman_client = _dataman_cache.client(); /**< Dataman client*/ + + uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription*/ + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/ + uORB::Publication _mission_pub{ORB_ID(mission)}; /**< Mission publication*/ +private: + /** + * @brief Maximum number of jump mission items iterations + * + */ + static constexpr uint16_t MAX_JUMP_ITERATION{10u}; + /** + * @brief Update Dataman cache + * + */ + void updateDatamanCache(); + /** + * @brief Update mission subscription + * + */ + void updateMavlinkMission(); + + /** + * Check whether a mission is ready to go + */ + void check_mission_valid(); + + /** + * Reset mission + */ + void checkMissionRestart(); + + /** + * Set a mission item as reached + */ + void set_mission_item_reached(); + + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + + /** + * Abort landing + */ + void do_abort_landing(); + + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + + /** + * @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 Replay the cached speed change items and delete them afterwards + * + */ + void replayCachedSpeedChangeItems(); + + /** + * @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(); + + /** + * @brief Set the Mission Index + * + * @param[in] index Index of the mission item + */ + void setMissionIndex(int32_t index); + + /** + * @brief Parameters update + * + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + /** + * @brief Check if a climb is necessary to align with mission altitude prior to starting the mission + * + * @param mission_item_index The index of the mission item to check if a climb is necessary + */ + void checkClimbRequired(int32_t mission_item_index); + + /** + * @brief check if relevant data in the new mission have changed. + * @param[in] new_mission new mission received over uorb + * @return true if the relevant mission data has changed, false otherwise + */ + bool checkMissionDataChanged(mission_s new_mission); + + int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ + int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ + + 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 {}; + mission_item_s _last_speed_change_item {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mis_dist_1wp, + (ParamInt) _param_mis_mnt_yaw_ctl + ) + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 565b2f2150..71fcb756a5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -901,15 +901,54 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const +{ + return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt); +} + +float +MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt) { if (mission_item.altitude_is_relative) { - return mission_item.altitude + _navigator->get_home_position()->alt; + return mission_item.altitude + home_alt; } else { return mission_item.altitude; } } +void +MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const +{ + if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + mission_item->lat = setpoint->lat; + mission_item->lon = setpoint->lon; + mission_item->altitude = setpoint->alt; + + } else { + mission_item->lat = _navigator->get_global_position()->lat; + mission_item->lon = _navigator->get_global_position()->lon; + mission_item->altitude = _navigator->get_global_position()->alt; + } + + mission_item->altitude_is_relative = false; +} + +void +MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const +{ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); + mission_item->altitude_is_relative = false; + mission_item->autocontinue = true; + mission_item->time_inside = 0.0f; + mission_item->yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + mission_item_next->lat, mission_item_next->lon); + mission_item->force_heading = true; +} + void MissionBlock::initialize() { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 450b57ba7b..1a669512af 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,6 +91,15 @@ public: */ static bool item_contains_gate(const mission_item_s &item); + /** + * Get the absolute altitude for mission item + * + * @param mission_item the mission item of interest + * @param home_alt the home altitude in [m AMSL]. + * @return Mission item altitude in [m AMSL] + */ + static float get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt); + /** * Check if the mission item contains a marker * @@ -124,6 +133,18 @@ public: _payload_deploy_timeout_s = timeout_s; } + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const; + + /** + * Create mission item to align towards next waypoint + */ + void set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const; + protected: /** * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index cb053b49e2..3cc471ab3d 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -246,32 +247,12 @@ public: orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - void increment_mission_instance_count() { _mission_result.instance_count++; } - int mission_instance_count() const { return _mission_result.instance_count; } void set_mission_failure_heading_timeout(); - bool is_planned_mission() const { return _navigation_mode == &_mission; } - - bool on_mission_landing() { return (_mission.landing() && _navigation_mode == &_mission); } - - bool start_mission_landing() { return _mission.land_start(); } - bool get_mission_start_land_available() { return _mission.get_land_start_available(); } - int get_mission_landing_index() { return _mission.get_land_start_index(); } - - double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); } - double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); } - float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); } - - double get_mission_landing_lat() { return _mission.get_landing_lat(); } - double get_mission_landing_lon() { return _mission.get_landing_lon(); } - float get_mission_landing_alt() { return _mission.get_landing_alt(); } - - float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); } - // RTL bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } @@ -339,8 +320,6 @@ private: vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ - bool _rtl_activated{false}; - // Publications geofence_result_s _geofence_result{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ @@ -359,8 +338,6 @@ private: bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ - bool _shouldEngageMissionForLanding{false}; - Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 32b99ea7b3..44b3a82e6d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -168,8 +168,8 @@ void Navigator::run() fds[2].fd = _mission_sub; fds[2].events = POLLIN; - int16_t geofence_update_counter{0}; - int16_t safe_points_update_counter{0}; + uint16_t geofence_update_counter{0}; + uint16_t safe_points_update_counter{0}; /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); @@ -589,8 +589,9 @@ void Navigator::run() // find NAV_CMD_DO_LAND_START in the mission and // use MAV_CMD_MISSION_START to start the mission from the next item containing a position setpoint + uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; - if (_mission.land_start()) { + if (_mission.get_land_start_available()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = _mission.get_land_start_index(); @@ -598,9 +599,10 @@ void Navigator::run() } else { PX4_WARN("planned mission landing not available"); + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED; } - publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + publish_vehicle_command_ack(cmd, result); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { @@ -694,7 +696,6 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); navigation_mode_new = &_mission; break; @@ -704,122 +705,19 @@ void Navigator::run() navigation_mode_new = &_loiter; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + + // If we are already in mission landing, do not switch. + if (_navigation_mode == &_mission && _mission.isLanding()) { + navigation_mode_new = &_mission; + + } else { _pos_sp_triplet_published_invalid_once = false; - const bool rtl_activated_now = !_rtl_activated; - - switch (_rtl.get_rtl_type()) { - case RTL::RTL_TYPE_MISSION_LANDING: - case RTL::RTL_TYPE_CLOSEST: { - // If a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode. - // In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission. - if (rtl_activated_now) { - _shouldEngageMissionForLanding = _rtl.getRTLDestinationTypeMission() - && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - } - - if (_shouldEngageMissionForLanding && (on_mission_landing() || _rtl.getRTLState() > RTL::RTL_STATE_CLIMB)) { - - // already in a mission landing, we just need to inform the user and stay in mission - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t"); - events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info, - "RTL to Mission landing, continue landing"); - } - - if (_navigation_mode != &_mission) { - // the first time we're here start the mission landig - start_mission_landing(); - } - - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - navigation_mode_new = &_mission; - - } else { - navigation_mode_new = &_rtl; - } - - break; - } - - case RTL::RTL_TYPE_MISSION_LANDING_REVERSED: - if (_mission.get_land_start_available() && !get_land_detected()->landed) { - // the mission contains a landing spot - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - - if (_navigation_mode != &_mission) { - if (_navigation_mode == nullptr) { - // switching from an manual mode, go to landing if not already landing - if (!on_mission_landing()) { - start_mission_landing(); - } - - } else { - // switching from an auto mode, continue the mission from the closest item - _mission.set_closest_item_as_current(); - } - } - - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t"); - events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info, - "RTL Mission activated, continue mission"); - } - - navigation_mode_new = &_mission; - - } else { - // fly the mission in reverse if switching from a non-manual mode - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE); - - if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) && - (! _mission.get_mission_finished()) && - (!get_land_detected()->landed)) { - // determine the closest mission item if switching from a non-mission mode, and we are either not already - // mission mode or the mission waypoints changed. - // The seconds condition is required so that when no mission was uploaded and one is available the closest - // mission item is determined and also that if the user changes the active mission index while rtl is active - // always that waypoint is tracked first. - if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) { - _mission.set_closest_item_as_current(); - } - - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t"); - events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info, - "RTL Mission activated, fly mission in reverse"); - } - - navigation_mode_new = &_mission; - - } else { - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t"); - events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info, - "RTL Mission activated, fly to home"); - } - - navigation_mode_new = &_rtl; - } - } - - break; - - default: - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t"); - events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated"); - } - - navigation_mode_new = &_rtl; - break; - - } - - _rtl_activated = true; - break; + navigation_mode_new = &_rtl; } + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_takeoff; @@ -854,11 +752,6 @@ void Navigator::run() break; } - if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _rtl_activated = false; - _rtl.resetRtlState(); - } - // Do not execute any state machine while we are disarmed if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { navigation_mode_new = nullptr; @@ -928,9 +821,7 @@ void Navigator::run() publish_mission_result(); } - _mission.run(); _geofence.run(); - _rtl.run(); perf_end(_loop_perf); } diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 5adb0b1bdb..b0ced9006c 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -55,7 +55,6 @@ NavigatorMode::run(bool active) { if (active) { if (!_active) { - _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 3aa6f255f6..60abdcd165 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -54,6 +54,8 @@ public: void run(bool active); + bool isActive() {return _active;}; + /** * This function is called while the mode is inactive */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 4dbb30e77d..224d3ca04f 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,32 +42,24 @@ #include "rtl.h" #include "navigator.h" -#include + +#include #include -#include - - -static constexpr float DELAY_SIGMA = 0.01f; - using namespace time_literals; using namespace math; RTL::RTL(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + NavigatorMode(navigator), + ModuleParams(navigator), + _rtl_direct(navigator), + _rtl_direct_mission_land(navigator), + _rtl_mission(navigator), + _rtl_mission_reverse(navigator) { - _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); - _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); - _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); - _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); - _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); - _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); - _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); - _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); } -void RTL::run() +void RTL::updateDatamanCache() { bool success; @@ -85,8 +77,8 @@ void RTL::run() case DatamanState::Read: _dataman_state = DatamanState::ReadWait; - success = _dataman_client.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), - sizeof(mission_stats_entry_s)); + success = _dataman_client_geofence.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); if (!success) { _error_state = DatamanState::Read; @@ -97,9 +89,9 @@ void RTL::run() case DatamanState::ReadWait: - _dataman_client.update(); + _dataman_client_geofence.update(); - if (_dataman_client.lastOperationCompleted(success)) { + if (_dataman_client_geofence.lastOperationCompleted(success)) { if (!success) { _error_state = DatamanState::ReadWait; @@ -110,14 +102,14 @@ void RTL::run() _update_counter = _stats.update_counter; _safe_points_updated = false; - _dataman_cache.invalidate(); + _dataman_cache_geofence.invalidate(); - if (_dataman_cache.size() != _stats.num_items) { - _dataman_cache.resize(_stats.num_items); + if (_dataman_cache_geofence.size() != _stats.num_items) { + _dataman_cache_geofence.resize(_stats.num_items); } - for (int index = 1; index <= _dataman_cache.size(); ++index) { - _dataman_cache.load(DM_KEY_SAFE_POINTS, index); + for (int index = 1; index <= _dataman_cache_geofence.size(); ++index) { + _dataman_cache_geofence.load(DM_KEY_SAFE_POINTS, index); } _dataman_state = DatamanState::Load; @@ -131,9 +123,9 @@ void RTL::run() case DatamanState::Load: - _dataman_cache.update(); + _dataman_cache_geofence.update(); - if (!_dataman_cache.isLoading()) { + if (!_dataman_cache_geofence.isLoading()) { _dataman_state = DatamanState::UpdateRequestWait; _safe_points_updated = true; } @@ -149,643 +141,328 @@ void RTL::run() break; } + + if (_mission_counter != _mission_sub.get().mission_update_counter) { + _mission_counter = _mission_sub.get().mission_update_counter; + const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + _dataman_cache_landItem.invalidate(); + + if (_mission_sub.get().land_start_index > 0) { + _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_start_index); + } + + if (_mission_sub.get().land_index > 0) { + _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_index); + } + } + + _dataman_cache_landItem.update(); } void RTL::on_inactivation() { - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + _rtl_mission.on_inactivation(); + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_reverse.on_inactivation(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactivation(); + break; + + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_direct_mission_land.on_inactivation(); + break; + + default: + break; } } void RTL::on_inactive() { + _global_pos_sub.update(); + _vehicle_status_sub.update(); + _mission_sub.update(); + _home_pos_sub.update(); + + updateDatamanCache(); + + parameters_update(); + + _rtl_mission.on_inactive(); + _rtl_mission_reverse.on_inactive(); + _rtl_direct.on_inactive(); + _rtl_direct_mission_land.on_inactive(); + // Limit inactive calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); + hrt_abstime now{hrt_absolute_time()}; - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + if ((now - _destination_check_time) > 1_s) { + _destination_check_time = now; + setRtlTypeAndDestination(); - const bool global_position_recently_updated = global_position.timestamp > 0 - && hrt_elapsed_time(&global_position.timestamp) < 10_s; + const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0 + && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; - rtl_time_estimate_s rtl_time_estimate{}; - rtl_time_estimate.valid = false; + rtl_time_estimate_s estimated_time{}; + estimated_time.valid = false; - // Calculate RTL destination and time estimate only when there is a valid home and global position if (_navigator->home_global_position_valid() && global_position_recently_updated) { - find_RTL_destination(); - calcRtlTimeEstimate(RTLState::RTL_STATE_NONE, rtl_time_estimate); - rtl_time_estimate.valid = true; + switch (_rtl_type) { + case RtlType::RTL_DIRECT: estimated_time = _rtl_direct.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_DIRECT_MISSION_LAND: estimated_time = _rtl_direct_mission_land.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_MISSION_FAST: estimated_time = _rtl_mission.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: estimated_time = _rtl_mission_reverse.calc_rtl_time_estimate(); + break; + + default: break; + } } - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); + _rtl_time_estimate_pub.publish(estimated_time); } } -void RTL::find_RTL_destination() +void RTL::on_activation() { - // get home position: - home_position_s &home_landing_position = *_navigator->get_home_position(); + setRtlTypeAndDestination(); - // get global position - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + _rtl_mission.on_activation(); + break; + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_reverse.on_activation(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_activation(_enforce_rtl_alt); + break; + + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_direct_mission_land.on_activation(_enforce_rtl_alt); + break; + + default: + break; + } +} + +void RTL::on_active() +{ + _global_pos_sub.update(); + _vehicle_status_sub.update(); + _mission_sub.update(); + _home_pos_sub.update(); + + updateDatamanCache(); + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + _rtl_mission.on_active(); + _rtl_mission_reverse.on_inactive(); + _rtl_direct.on_inactive(); + _rtl_direct_mission_land.on_inactive(); + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_reverse.on_active(); + _rtl_mission.on_inactive(); + _rtl_direct.on_inactive(); + _rtl_direct_mission_land.on_inactive(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_active(); + _rtl_direct_mission_land.on_inactive(); + _rtl_mission_reverse.on_inactive(); + _rtl_mission.on_inactive(); + break; + + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_direct_mission_land.on_active(); + _rtl_mission_reverse.on_inactive(); + _rtl_mission.on_inactive(); + _rtl_direct.on_inactive(); + break; + + default: + break; + } +} + +void RTL::setRtlTypeAndDestination() +{ + if (_param_rtl_type.get() == 2) { + if (hasMissionLandStart()) { + _rtl_type = RtlType::RTL_MISSION_FAST; + + } else { + _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; + } + + } else { + // check the closest allowed destination. + bool isMissionLanding{false}; + RtlDirect::RtlPosition rtl_position; + float rtl_alt; + findRtlDestination(isMissionLanding, rtl_position, rtl_alt); + + if (isMissionLanding) { + _rtl_direct_mission_land.setRtlAlt(rtl_alt); + _rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; + + } else { + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position); + _rtl_type = RtlType::RTL_DIRECT; + } + } +} + +void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +{ // set destination to home per default, then check if other valid landing spot is closer - _destination.set(home_landing_position); + rtl_position.alt = _home_pos_sub.get().alt; + rtl_position.lat = _home_pos_sub.get().lat; + rtl_position.lon = _home_pos_sub.get().lon; + rtl_position.yaw = _home_pos_sub.get().yaw; + isMissionLanding = false; // get distance to home position - double dlat = home_landing_position.lat - global_position.lat; - double dlon = home_landing_position.lon - global_position.lon; + float min_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; - double lon_scale = cos(radians(global_position.lat)); + const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); - auto coord_dist_sq = [lon_scale](double lat_diff, double lon_diff) -> double { - double lon_diff_scaled = lon_scale * matrix::wrap(lon_diff, -180., 180.); - return lat_diff * lat_diff + lon_diff_scaled * lon_diff_scaled; - }; + // consider the mission landing if available and allowed + if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) { + mission_item_s land_mission_item; + const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + bool success = _dataman_cache_landItem.loadWait(dm_item, _mission_sub.get().land_index, + reinterpret_cast(&land_mission_item), sizeof(mission_item_s), 500_ms); - double min_dist_squared = coord_dist_sq(dlat, dlon); - - _destination.type = RTL_DESTINATION_HOME; - - const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - - // consider the mission landing if not RTL_TYPE_HOME_OR_RALLY type set - if (_param_rtl_type.get() != RTL_TYPE_HOME_OR_RALLY && _navigator->get_mission_start_land_available()) { - double mission_landing_lat; - double mission_landing_lon; - float mission_landing_alt; - - if (vtol_in_rw_mode) { - mission_landing_lat = _navigator->get_mission_landing_lat(); - mission_landing_lon = _navigator->get_mission_landing_lon(); - mission_landing_alt = _navigator->get_mission_landing_alt(); - - } else { - mission_landing_lat = _navigator->get_mission_landing_start_lat(); - mission_landing_lon = _navigator->get_mission_landing_start_lon(); - mission_landing_alt = _navigator->get_mission_landing_start_alt(); + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t"); + events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error, + "Mission land item could not be read"); } - dlat = mission_landing_lat - global_position.lat; - dlon = mission_landing_lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; // always find closest destination if in hover and VTOL - if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) { + if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) { + // Use the mission landing destination. + min_dist = dist; + setLandPosAsDestination(rtl_position, land_mission_item); + isMissionLanding = true; - // compare home position to landing position to decide which is closer - if (dist_squared < min_dist_squared) { - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; - } - - } else { - // it has to be the mission landing - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; + } else if (dist < min_dist) { + min_dist = dist; + setLandPosAsDestination(rtl_position, land_mission_item); + isMissionLanding = true; } } - // do not consider rally point if RTL type is set to RTL_TYPE_MISSION_LANDING_REVERSED, so exit function and use either home or mission landing - if (_param_rtl_type.get() == RTL_TYPE_MISSION_LANDING_REVERSED) { - return; - } - - mission_safe_point_s closest_safe_point {}; - // check if a safe point is closer than home or landing - int closest_index = 0; - if (_safe_points_updated) { - for (int current_seq = 1; current_seq <= _dataman_cache.size(); ++current_seq) { + for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) { mission_safe_point_s mission_safe_point; - bool success = _dataman_cache.loadWait(DM_KEY_SAFE_POINTS, current_seq, - reinterpret_cast(&mission_safe_point), - sizeof(mission_safe_point_s)); + bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq, + reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s), 500_ms); if (!success) { PX4_ERR("dm_read failed"); continue; } - // 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); + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; + if (dist < min_dist) { + min_dist = dist; + setSafepointAsDestination(rtl_position, mission_safe_point); + isMissionLanding = false; } } } - if (closest_index > 0) { - _destination.type = RTL_DESTINATION_SAFE_POINT; - - // There is a safe point closer than home/mission landing - // TODO: handle all possible mission_safe_point.frame cases - switch (closest_safe_point.frame) { - case 0: // MAV_FRAME_GLOBAL - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt; - _destination.yaw = home_landing_position.yaw; - break; - - case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home - _destination.yaw = home_landing_position.yaw; - break; - - default: - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); - events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", - closest_safe_point.frame); - break; - } - } - if (_param_rtl_cone_half_angle_deg.get() > 0 - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get()); } else { - _rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); + rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get()); } } -void RTL::on_activation() +void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item) { - _rtl_state = RTL_STATE_NONE; - - // output the correct message, depending on where the RTL destination is - switch (_destination.type) { - case RTL_DESTINATION_HOME: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.\t"); - events::send(events::ID("rtl_land_at_home"), events::Log::Info, "RTL: landing at home position"); - break; - - case RTL_DESTINATION_MISSION_LANDING: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.\t"); - events::send(events::ID("rtl_land_at_mission"), events::Log::Info, "RTL: landing at mission landing"); - break; - - case RTL_DESTINATION_SAFE_POINT: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.\t"); - events::send(events::ID("rtl_land_at_safe_point"), events::Log::Info, "RTL: landing at safe landing point"); - break; - } - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - if (_navigator->get_land_detected()->landed) { - // For safety reasons don't go into RTL if landed. - _rtl_state = RTL_STATE_LANDED; - - } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { - - // If lower than return altitude, climb up first. - // If rtl_alt_min is true then forcing altitude change even if above. - _rtl_state = RTL_STATE_CLIMB; - - } else { - // Otherwise go straight to return - _rtl_state = RTL_STATE_RETURN; - } - - // reset cruising speed and throttle to default for RTL - _navigator->reset_cruising_speed(); - _navigator->set_cruising_throttle(); - - set_rtl_item(); - + rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + + _home_pos_sub.get().alt : land_mission_item.altitude; + rtl_position.lat = land_mission_item.lat; + rtl_position.lon = land_mission_item.lon; + rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::on_active() +void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, + const mission_safe_point_s &mission_safe_point) { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { - advance_rtl(); - set_rtl_item(); - } - - if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - - // Limit rtl time calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - const bool global_position_recently_updated = global_position.timestamp > 0 - && hrt_elapsed_time(&global_position.timestamp) < 10_s; - - rtl_time_estimate_s rtl_time_estimate{}; - rtl_time_estimate.valid = false; - - // Calculate time estimate only when there is a valid home and global position - if (_navigator->home_global_position_valid() && global_position_recently_updated) { - calcRtlTimeEstimate(_rtl_state, rtl_time_estimate); - rtl_time_estimate.valid = true; - } - - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); - } -} - -void RTL::set_rtl_item() -{ - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - - // if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission - const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ? - _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get(); - - const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - - _mission_item.lat = gpos.lat; - _mission_item.lon = gpos.lon; - _mission_item.altitude = _rtl_alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", - (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); - events::send(events::ID("rtl_climb_to"), events::Log::Info, - "RTL: climb to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); - break; - } - - case RTL_STATE_RETURN: { - - // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status - // can be displayed on groundstation and the WP is accepted once within loiter radius - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - - - } else { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _rtl_alt; // Don't change altitude - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && - destination_dist > _param_rtl_min_dist.get()) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || - destination_dist < _param_rtl_min_dist.get()) { - // Use destination yaw if close to _destination. - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_return_at"), events::Log::Info, - "RTL: return at {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - // Except for vtol which might be still off here and should point towards this location. - const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_descend_to"), events::Log::Info, - "RTL: descend to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - break; - } - - case RTL_STATE_LOITER: { - const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); - - if (autocontinue) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", - (double)_param_rtl_land_delay.get()); - events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); - - } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); - events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; // Don't change altitude. - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); - _mission_item.autocontinue = autocontinue; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - break; - } - - case RTL_STATE_HEAD_TO_CENTER: { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.vtol_back_transition = true; - // acceptance_radius will be overwritten since vtol_back_transition is set, - // set as a default value only - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - break; - } - - case RTL_STATE_TRANSITION_TO_MC: { - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - break; - } - - case RTL_MOVE_TO_LAND_HOVER_VTOL: { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.origin = ORIGIN_ONBOARD; - break; - } - - case RTL_STATE_LAND: { - // Land at destination. - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _destination.alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.land_precision = _param_rtl_pld_md.get(); - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - _navigator->get_precland()->on_activation(); - - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); - } - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); - events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); - break; - } - - case RTL_STATE_LANDED: { - set_idle_item(&_mission_item); - set_return_alt_min(false); - break; - } - - default: - break; - } - - reset_mission_item_reached(); - - // Execute command if set. This is required for commands like VTOL transition. - if (!item_contains_position(_mission_item)) { - issue_command(_mission_item); - } - - // Convert mission item to current position setpoint and make it valid. - mission_apply_limitation(_mission_item); - - if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { - _navigator->set_position_setpoint_triplet_updated(); - } -} - -void RTL::advance_rtl() -{ - // determines if the vehicle should loiter above land - const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; - - // vehicle is a vtol and currently in fixed wing mode - const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + // There is a safe point closer than home/mission landing + // TODO: handle all possible mission_safe_point.frame cases + switch (mission_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt; + rtl_position.yaw = _home_pos_sub.get().yaw;; break; - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; - break; - - case RTL_STATE_DESCEND: - - if (descend_and_loiter) { - _rtl_state = RTL_STATE_LOITER; - - } else if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_LOITER: - - if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_HEAD_TO_CENTER: - - _rtl_state = RTL_STATE_TRANSITION_TO_MC; - - break; - - case RTL_STATE_TRANSITION_TO_MC: - - _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; - - break; - - case RTL_MOVE_TO_LAND_HOVER_VTOL: - - _rtl_state = RTL_STATE_LAND; - - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt + _home_pos_sub.get().alt; // alt of safe point is rel to home + rtl_position.yaw = _home_pos_sub.get().yaw;; break; default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); + events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", + mission_safe_point.frame); break; } } -float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) +float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, + float cone_half_angle_deg) { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - // horizontal distance to destination - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); + const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + rtl_position.lat, rtl_position.lon); // minium rtl altitude to use when outside of horizontal acceptance radius of target position. // We choose the minimum height to be two times the distance from the land position in order to // avoid the vehicle touching the ground while still moving horizontally. - const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f * - _navigator->get_acceptance_radius(); + const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get(); + float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); - if (destination_dist <= _navigator->get_acceptance_radius()) { - return_altitude_amsl = _destination.alt + 2.0f * destination_dist; + if (destination_dist <= _param_nav_acc_rad.get()) { + return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { @@ -795,7 +472,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); // minimum altitude we need in order to be within the user defined cone - const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + _destination.alt; + const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt; return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); } @@ -803,211 +480,24 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, gpos.alt); + return max(return_altitude_amsl, _global_pos_sub.get().alt); } -void RTL::calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate) +void RTL::parameters_update() { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); - // Sum up time estimate for various segments of the landing procedure - switch (rtl_state) { - case RTL_STATE_NONE: - case RTL_STATE_CLIMB: { - // Climb segment is only relevant if the drone is below return altitude - const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0; + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); - if (climb_dist > FLT_EPSILON) { - rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); - } - } - - // FALLTHROUGH - case RTL_STATE_RETURN: - - // Add cruise segment to home - rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed(); - - // FALLTHROUGH - case RTL_STATE_HEAD_TO_CENTER: - case RTL_STATE_TRANSITION_TO_MC: - case RTL_STATE_DESCEND: { - // when descending, the target altitude is stored in the current mission item - float initial_altitude = 0.f; - float loiter_altitude = 0.f; - - if (rtl_state == RTL_STATE_DESCEND) { - // Take current vehicle altitude as the starting point for calculation - initial_altitude = gpos.alt; // TODO: Check if this is in the right frame - loiter_altitude = _mission_item.altitude; // Next waypoint = loiter - - - } else { - // Take the return altitude as the starting point for the calculation - initial_altitude = _rtl_alt; // CLIMB and RETURN - loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - } - - // Add descend segment (first landing phase: return alt to loiter alt) - rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); - } - - // FALLTHROUGH - case RTL_STATE_LOITER: - // Add land delay (the short pause for deploying landing gear) - // TODO: Check if landing gear is deployed or not - rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); - - // FALLTHROUGH - case RTL_MOVE_TO_LAND_HOVER_VTOL: - case RTL_STATE_LAND: { - float initial_altitude; - - // Add land segment (second landing phase) which comes after LOITER - if (rtl_state == RTL_STATE_LAND) { - // If we are in this phase, use the current vehicle altitude instead - // of the altitude paramteter to get a continous time estimate - initial_altitude = gpos.alt; - - - } else { - // If this phase is not active yet, simply use the loiter altitude, - // which is where the LAND phase will start - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - initial_altitude = loiter_altitude; - } - - // Prevent negative times when close to the ground - if (initial_altitude > _destination.alt) { - rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); - } - } - - break; - - case RTL_STATE_LANDED: - // Remaining time is 0 - break; + setRtlTypeAndDestination(); } - - // Prevent negative durations as phyiscally they make no sense. These can - // occur during the last phase of landing when close to the ground. - rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); - - // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin - rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate - + _param_rtl_time_margin.get(); } -float RTL::getCruiseSpeed() +bool RTL::hasMissionLandStart() { - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { - if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getHoverLandSpeed() -{ - float ret = 1e6f; - - if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - - return ret; -} - -matrix::Vector2f RTL::get_wind() -{ - _wind_sub.update(); - matrix::Vector2f wind; - - if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { - wind(0) = _wind_sub.get().windspeed_north; - wind(1) = _wind_sub.get().windspeed_east; - } - - return wind; -} - -float RTL::getClimbRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - - if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getDescendRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getCruiseGroundSpeed() -{ - float cruise_speed = getCruiseSpeed(); - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - matrix::Vector2f wind = get_wind(); - - matrix::Vector2f to_destination_vec; - get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, - &to_destination_vec(0), &to_destination_vec(1)); - - const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); - - const float wind_towards_home = wind.dot(to_home_dir); - const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); - - - // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient - const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( - 0.f, wind_towards_home); - - cruise_speed = ground_speed; - } - - return cruise_speed; + return _mission_sub.get().land_start_index > 0; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9c7fe55d5b..76cef15e9f 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,106 +44,85 @@ #include #include "navigator_mode.h" -#include "mission_block.h" - -#include -#include -#include -#include -#include -#include -#include #include +#include "rtl_direct.h" +#include "rtl_direct_mission_land.h" +#include "rtl_mission_fast.h" +#include "rtl_mission_fast_reverse.h" + +#include +#include +#include +#include +#include +#include +#include class Navigator; -class RTL : public MissionBlock, public ModuleParams +class RTL : public NavigatorMode, public ModuleParams { public: RTL(Navigator *navigator); ~RTL() = default; - enum RTLType { - RTL_TYPE_HOME_OR_RALLY = 0, - RTL_TYPE_MISSION_LANDING, - RTL_TYPE_MISSION_LANDING_REVERSED, - RTL_TYPE_CLOSEST, + enum class RtlType { + RTL_DIRECT, + RTL_DIRECT_MISSION_LAND, + RTL_MISSION_FAST, + RTL_MISSION_FAST_REVERSE, }; - enum RTLDestinationType { - RTL_DESTINATION_HOME = 0, - RTL_DESTINATION_MISSION_LANDING, - RTL_DESTINATION_SAFE_POINT, - }; - - enum RTLHeadingMode { - RTL_NAVIGATION_HEADING = 0, - RTL_DESTINATION_HEADING, - RTL_CURRENT_HEADING, - }; - - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_TRANSITION_TO_MC, - RTL_MOVE_TO_LAND_HOVER_VTOL, - RTL_STATE_LAND, - RTL_STATE_LANDED, - 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; void on_active() override; - void find_RTL_destination(); + void initialize() override {}; - void set_return_alt_min(bool min) { _rtl_alt_min = min; } - - int get_rtl_type() const { return _param_rtl_type.get(); } - - void get_rtl_xy_z_speed(float &xy, float &z); - - matrix::Vector2f get_wind(); - - RTLState getRTLState() { return _rtl_state; } - - bool getRTLDestinationTypeMission() { return _destination.type == RTLDestinationType::RTL_DESTINATION_MISSION_LANDING; } - - void resetRtlState() { _rtl_state = RTL_STATE_NONE; } + void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } void updateSafePoints() { _initiate_safe_points_updated = true; } private: + bool hasMissionLandStart(); - void set_rtl_item(); + /** + * @brief function to call regularly to do background work + */ + void updateDatamanCache(); - void advance_rtl(); + void setRtlTypeAndDestination(); - float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); - void calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate); + /** + * @brief Find RTL destination. + * + */ + void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); - float getCruiseGroundSpeed(); + /** + * @brief Set the position of the land start marker in the planned mission as destination. + * + */ + void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item); - float getClimbRate(); + /** + * @brief Set the safepoint as destination. + * + * @param mission_safe_point is the mission safe point/rally point to set as destination. + */ + void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point); - float getDescendRate(); + /** + * @brief + * + * @param cone_half_angle_deg + * @return float + */ + float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); - float getCruiseSpeed(); - - float getHoverLandSpeed(); - - RTLState _rtl_state{RTL_STATE_NONE}; + void parameters_update(); enum class DatamanState { UpdateRequestWait, @@ -153,71 +132,46 @@ private: Error }; - struct RTLPosition { - double lat; - double lon; - float alt; - float yaw; - uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points) - RTLDestinationType type{RTL_DESTINATION_HOME}; + hrt_abstime _destination_check_time{0}; - void set(const home_position_s &home_position) - { - lat = home_position.lat; - lon = home_position.lon; - alt = home_position.alt; - yaw = home_position.yaw; - safe_point_index = 0; - type = RTL_DESTINATION_HOME; - } - }; + RtlType _rtl_type{RtlType::RTL_DIRECT}; 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(); + DatamanCache _dataman_cache_geofence{"rtl_dm_cache_miss_geo", 4}; + DatamanClient &_dataman_client_geofence = _dataman_cache_geofence.client(); bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed + DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; + int16_t _mission_counter = -1; mission_stats_entry_s _stats; - RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) + RtlDirect _rtl_direct; - hrt_abstime _destination_check_time{0}; + RtlDirectMissionLand _rtl_direct_mission_land; - float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position + RtlMissionFast _rtl_mission; - bool _rtl_alt_min{false}; + RtlMissionFastReverse _rtl_mission_reverse; + + bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rtl_return_alt, - (ParamFloat) _param_rtl_descend_alt, - (ParamFloat) _param_rtl_land_delay, - (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamInt) _param_rtl_pld_md, - (ParamFloat) _param_rtl_loiter_rad, - (ParamInt) _param_rtl_hdg_md, - (ParamFloat) _param_rtl_time_factor, - (ParamInt) _param_rtl_time_margin + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_min_dist, + (ParamFloat) _param_nav_acc_rad ) - param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; - param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; - param_t _param_mpc_land_speed{PARAM_INVALID}; - param_t _param_fw_climb_rate{PARAM_INVALID}; - param_t _param_fw_sink_rate{PARAM_INVALID}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - param_t _param_fw_airspeed_trim{PARAM_INVALID}; - param_t _param_mpc_xy_cruise{PARAM_INVALID}; - param_t _param_rover_cruise_speed{PARAM_INVALID}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; - uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; }; - -float time_to_home(const matrix::Vector3f &to_home_vec, - const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, - float vehicle_descent_speed_m_s); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp new file mode 100644 index 0000000000..0b1d8442f5 --- /dev/null +++ b/src/modules/navigator/rtl_direct.cpp @@ -0,0 +1,723 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl.cpp + * + * Helper class to access RTL + * + * @author Julian Oes + * @author Anton Babushkin + * @author Julian Kent + */ + +#include "rtl_direct.h" +#include "navigator.h" +#include +#include + +#include + + +static constexpr float DELAY_SIGMA = 0.01f; + +using namespace time_literals; +using namespace math; + +RtlDirect::RtlDirect(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ + _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); + _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); + _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); + _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); + _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); + _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); +} + +void RtlDirect::on_inactivation() +{ + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RtlDirect::on_activation(bool enforce_rtl_alt) +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + + parameters_update(); + + if (_land_detected_sub.get().landed) { + // For safety reasons don't go into RTL if landed. + _rtl_state = RTL_STATE_LANDED; + + } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If rtl_alt_min is true then forcing altitude change even if above. + _rtl_state = RTL_STATE_CLIMB; + + } else { + // Otherwise go start with climb + _rtl_state = RTL_STATE_RETURN; + } + + // reset cruising speed and throttle to default for RTL + _navigator->reset_cruising_speed(); + _navigator->set_cruising_throttle(); + + set_rtl_item(); +} + +void RtlDirect::on_active() +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + + parameters_update(); + + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { + advance_rtl(); + set_rtl_item(); + } + + if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + // Need to update the position and type on the current setpoint triplet. + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RtlDirect::set_rtl_item() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + + const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + events::send(events::ID("rtl_climb_to"), events::Log::Info, + "RTL: climb to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); + break; + } + + case RTL_STATE_RETURN: { + + // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status + // can be displayed on groundstation and the WP is accepted once within loiter radius + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + + + } else { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _rtl_alt; // Don't change altitude + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && + destination_dist > _param_rtl_min_dist.get()) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || + destination_dist < _param_rtl_min_dist.get()) { + // Use destination yaw if close to _destination. + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_return_at"), events::Log::Info, + "RTL: return at {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // Except for vtol which might be still off here and should point towards this location. + const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_descend_to"), events::Log::Info, + "RTL: descend to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + break; + } + + case RTL_STATE_LOITER: { + const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); + + if (autocontinue) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", + (double)_param_rtl_land_delay.get()); + events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); + events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; // Don't change altitude. + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); + _mission_item.autocontinue = autocontinue; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + break; + } + + case RTL_STATE_HEAD_TO_CENTER: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.vtol_back_transition = true; + // acceptance_radius will be overwritten since vtol_back_transition is set, + // set as a default value only + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + break; + } + + case RTL_STATE_TRANSITION_TO_MC: { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + break; + } + + case RTL_MOVE_TO_LAND_HOVER_VTOL: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.origin = ORIGIN_ONBOARD; + break; + } + + case RTL_STATE_LAND: { + // Land at destination. + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _destination.alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _navigator->get_precland()->on_activation(); + + } else if (_mission_item.land_precision == 2) { + _navigator->get_precland()->set_mode(PrecLandMode::Required); + _navigator->get_precland()->on_activation(); + } + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); + events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); + break; + } + + case RTL_STATE_LANDED: { + set_idle_item(&_mission_item); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + // Execute command if set. This is required for commands like VTOL transition. + if (!MissionBlock::item_contains_position(_mission_item)) { + issue_command(_mission_item); + } + + // Convert mission item to current position setpoint and make it valid. + mission_apply_limitation(_mission_item); + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void RtlDirect::advance_rtl() +{ + // determines if the vehicle should loiter above land + const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; + + // vehicle is a vtol and currently in fixed wing mode + const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + + if (descend_and_loiter) { + _rtl_state = RTL_STATE_LOITER; + + } else if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_LOITER: + + if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_HEAD_TO_CENTER: + + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + break; + + case RTL_STATE_TRANSITION_TO_MC: + + _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; + + break; + + case RTL_MOVE_TO_LAND_HOVER_VTOL: + + _rtl_state = RTL_STATE_LAND; + + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + break; + + default: + break; + } +} + +rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() +{ + _global_pos_sub.update(); + + rtl_time_estimate_s rtl_time_estimate{}; + + RTLState start_state_for_estimate{RTL_STATE_NONE}; + + if (isActive()) { + start_state_for_estimate = _rtl_state; + } + + // Calculate RTL time estimate only when there is a valid home position + // TODO: Also check if vehicle position is valid + if (!_navigator->home_global_position_valid()) { + rtl_time_estimate.valid = false; + + } else { + rtl_time_estimate.valid = true; + + // Sum up time estimate for various segments of the landing procedure + switch (start_state_for_estimate) { + case RTL_STATE_NONE: + case RTL_STATE_CLIMB: { + // Climb segment is only relevant if the drone is below return altitude + const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0; + + if (climb_dist > FLT_EPSILON) { + rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); + } + } + + // FALLTHROUGH + case RTL_STATE_RETURN: + + // Add cruise segment to home + rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( + _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + + // FALLTHROUGH + case RTL_STATE_HEAD_TO_CENTER: + case RTL_STATE_TRANSITION_TO_MC: + case RTL_STATE_DESCEND: { + // when descending, the target altitude is stored in the current mission item + float initial_altitude = 0.f; + float loiter_altitude = 0.f; + + if (start_state_for_estimate == RTL_STATE_DESCEND) { + // Take current vehicle altitude as the starting point for calculation + initial_altitude = _global_pos_sub.get().alt; // TODO: Check if this is in the right frame + loiter_altitude = _mission_item.altitude; // Next waypoint = loiter + + + } else { + // Take the return altitude as the starting point for the calculation + initial_altitude = _rtl_alt; // CLIMB and RETURN + loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + } + + // Add descend segment (first landing phase: return alt to loiter alt) + rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); + } + + // FALLTHROUGH + case RTL_STATE_LOITER: + // Add land delay (the short pause for deploying landing gear) + // TODO: Check if landing gear is deployed or not + rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); + + // FALLTHROUGH + case RTL_MOVE_TO_LAND_HOVER_VTOL: + case RTL_STATE_LAND: { + float initial_altitude; + + // Add land segment (second landing phase) which comes after LOITER + if (start_state_for_estimate == RTL_STATE_LAND) { + // If we are in this phase, use the current vehicle altitude instead + // of the altitude paramteter to get a continous time estimate + initial_altitude = _global_pos_sub.get().alt; + + + } else { + // If this phase is not active yet, simply use the loiter altitude, + // which is where the LAND phase will start + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + initial_altitude = loiter_altitude; + } + + // Prevent negative times when close to the ground + if (initial_altitude > _destination.alt) { + rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); + } + } + + break; + + case RTL_STATE_LANDED: + // Remaining time is 0 + break; + } + + // Prevent negative durations as phyiscally they make no sense. These can + // occur during the last phase of landing when close to the ground. + rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); + + // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin + rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate + + _param_rtl_time_margin.get(); + } + + // return message + rtl_time_estimate.timestamp = hrt_absolute_time(); + + return rtl_time_estimate; +} + +float RtlDirect::getCruiseSpeed() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getHoverLandSpeed() +{ + float ret = 1e6f; + + if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + + return ret; +} + +matrix::Vector2f RtlDirect::get_wind() +{ + _wind_sub.update(); + matrix::Vector2f wind; + + if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { + wind(0) = _wind_sub.get().windspeed_north; + wind(1) = _wind_sub.get().windspeed_east; + } + + return wind; +} + +float RtlDirect::getClimbRate() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getDescendRate() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getCruiseGroundSpeed() +{ + float cruise_speed = getCruiseSpeed(); + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + matrix::Vector2f wind = get_wind(); + + matrix::Vector2f to_destination_vec; + get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, + &to_destination_vec(0), &to_destination_vec(1)); + + const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); + + const float wind_towards_home = wind.dot(to_home_dir); + const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); + + + // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient + const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( + 0.f, wind_towards_home); + + cruise_speed = ground_speed; + } + + return cruise_speed; +} + +void RtlDirect::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h new file mode 100644 index 0000000000..892421cb22 --- /dev/null +++ b/src/modules/navigator/rtl_direct.h @@ -0,0 +1,239 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_direct.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include + +#include "mission_block.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class Navigator; + +class RtlDirect : public MissionBlock, public ModuleParams +{ +public: + /** + * @brief Return to launch position. + * Defines the position and landing yaw for the return to launch destination. + * + */ + struct RtlPosition { + double lat; /**< latitude in WGS84 [rad].*/ + double lon; /**< longitude in WGS84 [rad].*/ + float alt; /**< altitude in MSL [m].*/ + float yaw; /**< final yaw when landed [rad].*/ + }; + + RtlDirect(Navigator *navigator); + + ~RtlDirect() = default; + + /** + * @brief on inactivation + * + */ + void on_inactivation() override; + + /** + * @brief On activation. + * Initialize the return to launch calculations. + * + * @param[in] enforce_rtl_alt boolean if the minimal return to launch altitude should be enforced at the beginning of the return, even when the current vehicle altitude is above. + */ + void on_activation(bool enforce_rtl_alt); + + /** + * @brief on active + * Update the return to launch calculation and set new setpoints for controller if necessary. + * + */ + void on_active() override; + + /** + * @brief Calculate the estimated time needed to return to launch. + * + * @return estimated time to return to launch. + */ + rtl_time_estimate_s calc_rtl_time_estimate(); + + void setRtlAlt(float alt) {_rtl_alt = alt;}; + + void setRtlPosition(RtlPosition position) {_destination = position;}; + +private: + /** + * @brief Return to launch heading mode. + * + */ + enum RTLHeadingMode { + RTL_NAVIGATION_HEADING = 0, + RTL_DESTINATION_HEADING, + RTL_CURRENT_HEADING, + }; + + /** + * @brief Return to launch state machine. + * + */ + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_TRANSITION_TO_MC, + RTL_MOVE_TO_LAND_HOVER_VTOL, + RTL_STATE_LAND, + RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, + }; + +private: + /** + * @brief Get the horizontal wind velocity + * + * @return horizontal wind velocity. + */ + matrix::Vector2f get_wind(); + + /** + * @brief Set the return to launch control setpoint. + * + */ + void set_rtl_item(); + + /** + * @brief Advance the return to launch state machine. + * + */ + void advance_rtl(); + + /** + * @brief Get the Cruise Ground Speed + * + * @return Ground speed in cruise mode [m/s]. + */ + float getCruiseGroundSpeed(); + + /** + * @brief Get the climb rate + * + * @return Climb rate [m/s] + */ + float getClimbRate(); + + /** + * @brief Get the descend rate + * + * @return descend rate [m/s] + */ + float getDescendRate(); + + /** + * @brief Get the cruise speed + * + * @return cruise speed [m/s] + */ + float getCruiseSpeed(); + + /** + * @brief Get the Hover Land Speed + * + * @return Hover land speed [m/s] + */ + float getHoverLandSpeed(); + + /** + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + /** Current state in the state machine.*/ + RTLState _rtl_state{RTL_STATE_NONE}; + + RtlPosition _destination{}; ///< the RTL position to fly to + + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position + + DEFINE_PARAMETERS( + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_descend_alt, + (ParamFloat) _param_rtl_land_delay, + (ParamFloat) _param_rtl_min_dist, + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad, + (ParamInt) _param_rtl_hdg_md, + (ParamFloat) _param_rtl_time_factor, + (ParamInt) _param_rtl_time_margin + ) + + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; + param_t _param_mpc_land_speed{PARAM_INVALID}; + param_t _param_fw_climb_rate{PARAM_INVALID}; + param_t _param_fw_sink_rate{PARAM_INVALID}; + + param_t _param_fw_airspeed_trim{PARAM_INVALID}; + param_t _param_mpc_xy_cruise{PARAM_INVALID}; + param_t _param_rover_cruise_speed{PARAM_INVALID}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */ + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; +}; diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp new file mode 100644 index 0000000000..7eb5c54814 --- /dev/null +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -0,0 +1,281 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_direct_mission_land.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_direct_mission_land.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5; + +RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) : + MissionBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE) +{ + +} + +void RtlDirectMissionLand::on_activation(bool enforce_rtl_alt) +{ + _land_detected_sub.update(); + _global_pos_sub.update(); + + _needs_climbing = false; + + if (hasMissionLandStart()) { + _is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK); + + if ((_global_pos_sub.get().alt < _rtl_alt) || enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If enforce_rtl_alt is true then forcing altitude change even if above. + _needs_climbing = true; + + } + + } else { + _is_current_planned_mission_item_valid = false; + } + + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +bool RtlDirectMissionLand::setNextMissionItem() +{ + return (goToNextPositionItem(true) == PX4_OK); +} + +void RtlDirectMissionLand::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Climb to altitude + if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, + // even if current climb altitude is below (e.g. RTL immediately after take off) + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL Mission land: climb to %d m\t", + (int)ceilf(_rtl_alt)); + events::send(events::ID("rtl_mission_land_climb"), events::Log::Info, + "RTL Mission Land: climb to {1m_v}", + (int32_t)ceilf(_rtl_alt)); + + _needs_climbing = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Transition to fixed wing if necessary. + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + int32_t next_mission_item_index; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlDirectMissionLand::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + } +} + +bool RtlDirectMissionLand::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h new file mode 100644 index 0000000000..1642a61b5a --- /dev/null +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -0,0 +1,72 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_direct_mission_land.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mission.h" + +#include +#include +#include + +class Navigator; + +class RtlDirectMissionLand : public MissionBase +{ +public: + RtlDirectMissionLand(Navigator *navigator); + ~RtlDirectMissionLand() = default; + + void on_activation(bool enforce_rtl_alt); + + rtl_time_estimate_s calc_rtl_time_estimate(); + + void setRtlAlt(float alt) {_rtl_alt = alt;}; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + bool _needs_climbing{false}; //< Flag if climbing is required at the start + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position +}; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp new file mode 100644 index 0000000000..f667e3e79f --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -0,0 +1,241 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_mission_fast.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5; + +RtlMissionFast::RtlMissionFast(Navigator *navigator) : + MissionBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) +{ + +} + +void RtlMissionFast::on_activation() +{ + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFast::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFast::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFast::setNextMissionItem() +{ + return (goToNextPositionItem(true) == PX4_OK); +} + +void RtlMissionFast::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Transition to fixed wing if necessary. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + int32_t next_mission_item_index; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + } +} + +bool RtlMissionFast::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h new file mode 100644 index 0000000000..0b6f689041 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_mission_fast.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mission_base.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFast : public MissionBase +{ +public: + RtlMissionFast(Navigator *navigator); + ~RtlMissionFast() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate(); + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ +}; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp new file mode 100644 index 0000000000..42c014ac74 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -0,0 +1,268 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_mission_fast_reverse.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast_reverse.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5; + +RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : + MissionBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) +{ + +} + +void RtlMissionFastReverse::on_activation() +{ + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidate the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFastReverse::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFastReverse::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFastReverse::setNextMissionItem() +{ + return (goToPreviousPositionItem(true) == PX4_OK); +} + +void RtlMissionFastReverse::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Transition to fixed wing if necessary. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + int32_t next_mission_item_index; + size_t num_found_items = 0; + getPreviousPositionItems(_mission.current_seq, &next_mission_item_index, num_found_items, 1u); + + // If the current item is a takeoff item or there is no further position item start landing. + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || + num_found_items == 0) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed; + bool vtol_in_fw = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + if (needs_to_land) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Go to Take off location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + if (!PX4_ISFINITE(_mission_item.altitude)) { + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + } + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (vtol_in_fw) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) { + // Go to home location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.yaw = NAN; + + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } else { + _mission_item.altitude = _home_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + } + } +} + +bool RtlMissionFastReverse::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h new file mode 100644 index 0000000000..e4dba5312b --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_mission_fast_reverse.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mission_base.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFastReverse : public MissionBase +{ +public: + RtlMissionFastReverse(Navigator *navigator); + ~RtlMissionFastReverse() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate(); + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ +};