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 */ +};