diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 6fa0f100b4..117e811a0f 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -315,6 +315,9 @@ protected: float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ 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. + 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*/ + DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/ DatamanClient &_dataman_client = _dataman_cache.client(); /**< Dataman client*/ @@ -334,7 +337,7 @@ private: * @brief Update Dataman cache * */ - void updateDatamanCache(); + virtual void updateDatamanCache(); /** * @brief Update mission subscription * @@ -447,9 +450,6 @@ private: bool canRunMissionFeasibility(); - 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*/ - 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 {}; diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index a8620f2bb7..40b91dee0a 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -52,6 +52,42 @@ RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) : } +void +RtlDirectMissionLand::updateDatamanCache() +{ + int32_t start_index; + + start_index = math::min(_mission.land_start_index, static_cast(_mission.count)); + + if ((start_index >= 0) && (_mission.count > 0) && hasMissionLandStart() && (start_index != _load_mission_index)) { + + int32_t end_index = static_cast(_mission.count); + + // Check that we load all data into the cache + if (end_index - start_index > _dataman_cache_size_signed) { + _dataman_cache.invalidate(); + _dataman_cache_size_signed = end_index - start_index; + _dataman_cache.resize(_dataman_cache_size_signed); + } + + for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) { + + _dataman_cache.load(static_cast(_mission.mission_dataman_id), index); + } + + _load_mission_index = start_index; + } + + _dataman_cache.update(); +} + +void RtlDirectMissionLand::on_inactive() +{ + MissionBase::on_active(); + + updateDatamanCache(); +} + void RtlDirectMissionLand::on_activation() { _land_detected_sub.update(); @@ -62,13 +98,7 @@ void RtlDirectMissionLand::on_activation() 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; - - } + _needs_climbing = checkNeedsToClimb(); } else { _is_current_planned_mission_item_valid = false; @@ -202,9 +232,143 @@ void RtlDirectMissionLand::setActiveMissionItems() 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(); + _rtl_time_estimator.update(); + _rtl_time_estimator.reset(); - return time_estimate; + if (_mission.count > 0 && hasMissionLandStart()) { + int32_t start_item_index{-1}; + bool is_in_climbing_submode{false}; + + if (isActive()) { + start_item_index = math::max(_mission.current_seq, _mission.land_start_index); + is_in_climbing_submode = _needs_climbing; + + } else { + start_item_index = _mission.land_start_index; + is_in_climbing_submode = checkNeedsToClimb(); + } + + if (start_item_index >= 0 && start_item_index < static_cast(_mission.count)) { + float altitude_at_calculation_point; + matrix::Vector2d hor_position_at_calculation_point{_global_pos_sub.get().lat, _global_pos_sub.get().lon}; + + if (is_in_climbing_submode) { + if (_enforce_rtl_alt) { + _rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt); + altitude_at_calculation_point = _rtl_alt; + + } else { + if (_global_pos_sub.get().alt < _rtl_alt) { + _rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt); + } + + altitude_at_calculation_point = math::max(_rtl_alt, _global_pos_sub.get().alt); + } + + } else { + altitude_at_calculation_point = _global_pos_sub.get().alt; + } + + while (start_item_index < _mission.count && start_item_index >= 0) { + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(start_item_index, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items > 0U) { + mission_item_s next_position_mission_item; + const dm_item_t dataman_id = static_cast(_mission.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) { + // Could not load the mission item, mark time estimate as invalid. + _rtl_time_estimator.reset(); + break; + } + + switch (next_position_mission_item.nav_cmd) { + case NAV_CMD_LOITER_UNLIMITED: { + _rtl_time_estimator.reset(); + break; + } + + case NAV_CMD_LOITER_TIME_LIMIT: { + // Go to loiter + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); + + // add time + _rtl_time_estimator.addWait(next_position_mission_item.time_inside); + break; + } + + case NAV_CMD_LAND: // Fallthrough + case NAV_CMD_VTOL_LAND: // Fallthrough + case NAV_CMD_LOITER_TO_ALT: { + // Go to point horizontally + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); + + // Add the vertical loiter + _rtl_time_estimator.addVertDistance(next_position_mission_item.altitude - altitude_at_calculation_point); + + break; + } + + default: { + // Default assume can go to the location directly + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + _rtl_time_estimator.addDistance(hor_dist, direction, + next_position_mission_item.altitude - altitude_at_calculation_point); + break; + } + } + + start_item_index = next_mission_item_index + 1; + hor_position_at_calculation_point(0) = next_position_mission_item.lat; + hor_position_at_calculation_point(1) = next_position_mission_item.lon; + altitude_at_calculation_point = next_position_mission_item.altitude; + + + } else { + start_item_index = -1; + } + } + } + } + + return _rtl_time_estimator.getEstimate(); +} + +bool RtlDirectMissionLand::checkNeedsToClimb() +{ + bool needs_climbing{false}; + + 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; + + } + + return needs_climbing; } diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h index 5edeafc71e..9bfdd620c4 100644 --- a/src/modules/navigator/rtl_direct_mission_land.h +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -43,6 +43,8 @@ #include "rtl_base.h" +#include + #include #include #include @@ -56,6 +58,7 @@ public: ~RtlDirectMissionLand() = default; void on_activation() override; + void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -65,8 +68,12 @@ public: private: bool setNextMissionItem() override; void setActiveMissionItems() override; + void updateDatamanCache() override; + bool checkNeedsToClimb(); bool _needs_climbing{false}; //< Flag if climbing is required at the start bool _enforce_rtl_alt{false}; float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position + + RtlTimeEstimator _rtl_time_estimator; };