From 14e416947329d9059799d9febd94f334f611017e Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 26 Jan 2024 09:46:00 +0100 Subject: [PATCH] rtl_direct: Move the time estimation calculation into a separate helper class --- src/lib/CMakeLists.txt | 1 + src/lib/rtl/CMakeLists.txt | 34 ++++ src/lib/rtl/rtl_time_estimator.cpp | 267 +++++++++++++++++++++++++++ src/lib/rtl/rtl_time_estimator.h | 146 +++++++++++++++ src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/rtl_direct.cpp | 193 +++---------------- src/modules/navigator/rtl_direct.h | 56 +----- 7 files changed, 481 insertions(+), 217 deletions(-) create mode 100644 src/lib/rtl/CMakeLists.txt create mode 100644 src/lib/rtl/rtl_time_estimator.cpp create mode 100644 src/lib/rtl/rtl_time_estimator.h diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1d88b3b022..4e20ce106b 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -67,6 +67,7 @@ add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) +add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) diff --git a/src/lib/rtl/CMakeLists.txt b/src/lib/rtl/CMakeLists.txt new file mode 100644 index 0000000000..4f3e7a3c53 --- /dev/null +++ b/src/lib/rtl/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +px4_add_library(rtl_time_estimator rtl_time_estimator.cpp) diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp new file mode 100644 index 0000000000..055b62ccab --- /dev/null +++ b/src/lib/rtl/rtl_time_estimator.cpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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_time_estimator.cpp + * + * Helper class to calculate the remaining time estimate to go to RTL landing point. + * + */ + +#include "rtl_time_estimator.h" + +#include +#include + +#include +#include +#include + +RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr) +{ + _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"); +}; + +rtl_time_estimate_s RtlTimeEstimator::getEstimate() const +{ + rtl_time_estimate_s time_estimate{}; + + if (_is_valid && PX4_ISFINITE(_time_estimate)) { + time_estimate.valid = true; + time_estimate.time_estimate = _time_estimate; + // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin + time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * _time_estimate + _param_rtl_time_margin.get(); + + } else { + time_estimate.valid = false; + } + + time_estimate.timestamp = hrt_absolute_time(); + return time_estimate; +} + +void RtlTimeEstimator::update() +{ + _vehicle_status_sub.update(); + _wind_sub.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 RtlTimeEstimator::addVertDistance(float alt) +{ + if (PX4_ISFINITE(alt)) { + _is_valid = true; + + _time_estimate += calcVertTimeEstimate(alt); + } +} + +void RtlTimeEstimator::addDistance(float hor_dist, const matrix::Vector2f &direction, float vert_dist) +{ + if (PX4_ISFINITE(hor_dist) && PX4_ISFINITE(vert_dist)) { + _is_valid = true; + + float hor_time_estimate{0.f}; + + if (hor_dist > FLT_EPSILON && PX4_ISFINITE(hor_dist)) { + hor_time_estimate = hor_dist / getCruiseGroundSpeed(direction.normalized()); + } + + float ver_time_estimate{calcVertTimeEstimate(vert_dist)}; + + _time_estimate += math::max(hor_time_estimate, ver_time_estimate); + + } +} + +void RtlTimeEstimator::addWait(float time_s) +{ + if (PX4_ISFINITE(time_s)) { + _is_valid = true; + + if (time_s > FLT_EPSILON) { + _time_estimate += time_s; + } + } +} + +void RtlTimeEstimator::addDescendMCLand(float alt) +{ + if (PX4_ISFINITE(alt)) { + _is_valid = true; + + if (alt < -FLT_EPSILON && PX4_ISFINITE(alt)) { + _time_estimate += -alt / getHoverLandSpeed(); + } + } +} + +float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_norm) +{ + float cruise_speed = getCruiseSpeed(); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + matrix::Vector2f wind = get_wind(); + + const float wind_along_dir = wind.dot(direction_norm); + const float wind_across_dir = matrix::Vector2f(wind - direction_norm * wind_along_dir).norm(); + + // Note: use fminf so that we don't _rely_ on tailwind towards direction to make RTL more efficient + const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_dir * wind_across_dir) + fminf( + 0.f, wind_along_dir); + + cruise_speed = ground_speed; + } + + return cruise_speed; +} + +float RtlTimeEstimator::calcVertTimeEstimate(float alt) +{ + float vertical_rate{0.1f}; + float time_estimate{0.f}; + + if (alt > FLT_EPSILON) { + vertical_rate = getClimbRate(); + + } else { + vertical_rate = getDescendRate(); + } + + float abs_alt = fabsf(alt); + + if (abs_alt > FLT_EPSILON) { + time_estimate = abs_alt / vertical_rate; + } + + return time_estimate; +} + +float RtlTimeEstimator::getCruiseSpeed() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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 RtlTimeEstimator::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 RtlTimeEstimator::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 RtlTimeEstimator::getClimbRate() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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 RtlTimeEstimator::getDescendRate() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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; +} diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h new file mode 100644 index 0000000000..db0f8c234e --- /dev/null +++ b/src/lib/rtl/rtl_time_estimator.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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_time_estimator.h + * + * Helper class to calculate the remaining time estimate to go to RTL landing point. + * + */ + +#ifndef RTL_TIME_ESTIMATOR_H_ +#define RTL_TIME_ESTIMATOR_H_ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +class Navigator; + +class RtlTimeEstimator : public ModuleParams +{ +public: + RtlTimeEstimator(); + ~RtlTimeEstimator() = default; + + void update(); + void reset() { _time_estimate = 0.f; _is_valid = false;}; + rtl_time_estimate_s getEstimate() const; + void addDistance(float hor_dist, const matrix::Vector2f &hor_direction, float vert_dist); + void addVertDistance(float alt); + void addWait(float time_s); + void addDescendMCLand(float alt); + +private: + /** + * @brief Get the Cruise Ground Speed + * + * @param direction_norm normalized direction in which to fly + * @return Ground speed in cruise mode [m/s]. + */ + float getCruiseGroundSpeed(const matrix::Vector2f &direction_norm); + + /** + * @brief Get time estimate of vertical distance + * + */ + float calcVertTimeEstimate(float alt); + + /** + * @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(); + + /** + * @brief Get the horizontal wind velocity + * + * @return horizontal wind velocity. + */ + matrix::Vector2f get_wind(); + + float _time_estimate; /**< Accumulated time estimate [s] */ + bool _is_valid{false}; /**< Checks if time estimate is valid */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_rtl_time_factor, /**< Safety factory for safe time estimate */ + (ParamInt) _param_rtl_time_margin /**< Safety margin for safe time estimate */ + ) + + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; /**< MC climb velocity parameter */ + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; /**< MC descend velocity parameter */ + param_t _param_mpc_land_speed{PARAM_INVALID}; /**< MC land descend speed parameter */ + param_t _param_fw_climb_rate{PARAM_INVALID}; /**< FW climb speed parameter */ + param_t _param_fw_sink_rate{PARAM_INVALID}; /**< FW descend speed parameter */ + + param_t _param_fw_airspeed_trim{PARAM_INVALID}; /**< FW cruise airspeed parameter */ + param_t _param_mpc_xy_cruise{PARAM_INVALID}; /**< MC horizontal cruise speed parameter */ + param_t _param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< Parameter update topic */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; /**< wind topic */ +}; + +#endif /* RTL_TIME_ESTIMATOR_H_ */ diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index b996566def..c55cae3f07 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -69,4 +69,5 @@ px4_add_module( geofence_breach_avoidance motion_planning mission_feasibility_checker + rtl_time_estimator ) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 275cf011e2..8db9447bfe 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2024 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 @@ -59,15 +59,6 @@ RtlDirect::RtlDirect(Navigator *navigator) : _land_approach.lat = static_cast(NAN); _land_approach.lon = static_cast(NAN); _land_approach.height_m = NAN; - - _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() @@ -379,8 +370,9 @@ RtlDirect::RTLState RtlDirect::getActivationLandState() rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() { _global_pos_sub.update(); + _rtl_time_estimator.update(); - rtl_time_estimate_s rtl_time_estimate{}; + _rtl_time_estimator.reset(); RTLState start_state_for_estimate; @@ -393,12 +385,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() // 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; - rtl_time_estimate.time_estimate = 0.f; + if (_navigator->home_global_position_valid()) { const float loiter_altitude = min(_land_approach.height_m, _rtl_alt); @@ -406,19 +393,19 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() switch (start_state_for_estimate) { case RTLState::CLIMBING: { // 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(); + if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) { + _rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt); } } // FALLTHROUGH - case RTLState::MOVE_TO_LOITER: - - // Add cruise segment to home - rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _land_approach.lat, _land_approach.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + case RTLState::MOVE_TO_LOITER: { + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _land_approach.lat, + _land_approach.lon, &direction(0), &direction(1)); + _rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _land_approach.lat, _land_approach.lon), direction, 0.f); + } // FALLTHROUGH case RTLState::LOITER_DOWN: { @@ -434,15 +421,18 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() initial_altitude = _rtl_alt; // CLIMB and RETURN } - // Add descend segment (first landing phase: return alt to loiter alt) - rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); + _rtl_time_estimator.addVertDistance(loiter_altitude - initial_altitude); } // FALLTHROUGH case RTLState::LOITER_HOLD: // 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(); + _rtl_time_estimator.addWait(_param_rtl_land_delay.get()); + + if (_param_rtl_land_delay.get() < -FLT_EPSILON) { // Set to loiter infinitely and not land. Stop calculation here + break; + } + // FALLTHROUGH case RTLState::MOVE_TO_LAND: @@ -450,19 +440,22 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() case RTLState::MOVE_TO_LAND_HOVER: { // Add cruise segment to home float move_to_land_dist{0.f}; + matrix::Vector2f direction{}; if (start_state_for_estimate >= RTLState::MOVE_TO_LAND) { move_to_land_dist = get_distance_to_next_waypoint( - _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon); + get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon, + &direction(0), &direction(1)); } else { move_to_land_dist = get_distance_to_next_waypoint( - _destination.lat, _destination.lon, _land_approach.lat, _land_approach.lon); + _land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon); + get_vector_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon, &direction(0), + &direction(1)); } - if (move_to_land_dist > FLT_EPSILON) { - rtl_time_estimate.time_estimate += move_to_land_dist / getCruiseGroundSpeed(); - } + _rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f); } // FALLTHROUGH @@ -482,10 +475,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() 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(); - } + _rtl_time_estimator.addDescendMCLand(_destination.alt - initial_altitude); } break; @@ -494,132 +484,9 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() // 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; + return _rtl_time_estimator.getEstimate(); } void RtlDirect::parameters_update() diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 9dc89e42fc..c71a5f5736 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -54,6 +54,7 @@ #include #include +#include #include "mission_block.h" #include "navigation.h" #include "safe_point_land.hpp" @@ -119,54 +120,12 @@ private: } _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/ 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 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. */ @@ -178,6 +137,7 @@ private: bool _enforce_rtl_alt{false}; bool _force_heading{false}; + RtlTimeEstimator _rtl_time_estimator; PositionYawSetpoint _destination; ///< the RTL position to fly to loiter_point_s _land_approach; @@ -190,23 +150,11 @@ private: (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_pld_md, (ParamFloat) _param_rtl_loiter_rad, - (ParamFloat) _param_rtl_time_factor, - (ParamInt) _param_rtl_time_margin, // external params (ParamBool) _param_wv_en ) - 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 _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */