From c522a8b15a89707a4abb07855c0365de3c2f210a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 11 Nov 2021 20:05:00 +0100 Subject: [PATCH] Compute RTL time and react if lower than flight time - Compute RTL time also during RTL - Calculate correct altitude when finding destination --- msg/CMakeLists.txt | 2 +- msg/rtl_flight_time.msg | 4 - msg/rtl_time_estimate.msg | 5 + src/modules/commander/Commander.cpp | 87 ++++---- src/modules/commander/Commander.hpp | 6 +- src/modules/logger/logged_topics.cpp | 2 +- src/modules/navigator/rtl.cpp | 293 ++++++++++++++++++++------- src/modules/navigator/rtl.h | 33 ++- src/modules/navigator/rtl_params.c | 40 +++- 9 files changed, 324 insertions(+), 148 deletions(-) delete mode 100644 msg/rtl_flight_time.msg create mode 100644 msg/rtl_time_estimate.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 324f888ce8..1e400b12ac 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -131,7 +131,7 @@ set(msg_files rc_channels.msg rc_parameter_map.msg rpm.msg - rtl_flight_time.msg + rtl_time_estimate.msg safety.msg satellite_info.msg sensor_accel.msg diff --git a/msg/rtl_flight_time.msg b/msg/rtl_flight_time.msg deleted file mode 100644 index 1f16b67408..0000000000 --- a/msg/rtl_flight_time.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 rtl_time_s # how long in seconds will the RTL take -float32 rtl_limit_fraction # what fraction of the allowable RTL time would be taken diff --git a/msg/rtl_time_estimate.msg b/msg/rtl_time_estimate.msg new file mode 100644 index 0000000000..ee46888dd4 --- /dev/null +++ b/msg/rtl_time_estimate.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +bool valid # Flag indicating whether the time estiamtes are valid +float32 time_estimate # [s] Estimated time for RTL +float32 safe_time_estimate # [s] Same as time_estimate, but with safety factor and safety margin included (factor*t + margin) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0260cc4ebd..21af761f3a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3606,17 +3606,6 @@ void Commander::avoidance_check() void Commander::battery_status_check() { - battery_status_s batteries[_battery_status_subs.size()]; - size_t num_connected_batteries = 0; - - for (auto &battery_sub : _battery_status_subs) { - if (battery_sub.copy(&batteries[num_connected_batteries])) { - if (batteries[num_connected_batteries].connected) { - num_connected_batteries++; - } - } - } - // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // option is to check if ANY of them have a warning, and specifically find which one has the most // urgent warning. @@ -3624,52 +3613,62 @@ void Commander::battery_status_check() // To make sure that all connected batteries are being regularly reported, we check which one has the // oldest timestamp. hrt_abstime oldest_update = hrt_absolute_time(); + size_t num_connected_batteries{0}; + float worst_battery_time_s{NAN}; _battery_current = 0.0f; - float battery_level = 0.0f; + for (auto &battery_sub : _battery_status_subs) { + battery_status_s battery; - // Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing. - for (size_t i = 0; i < num_connected_batteries; i++) { - if (batteries[i].warning > worst_warning) { - worst_warning = batteries[i].warning; + if (!battery_sub.copy(&battery)) { + continue; } - if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) { - oldest_update = batteries[i].timestamp; - } + if (battery.connected) { + num_connected_batteries++; - // Sum up current from all batteries. - _battery_current += batteries[i].current_filtered_a; + if (battery.warning > worst_warning) { + worst_warning = battery.warning; + } - // average levels from all batteries - battery_level += batteries[i].remaining; - } + if (battery.timestamp < oldest_update) { + oldest_update = battery.timestamp; + } - battery_level /= num_connected_batteries; + if (PX4_ISFINITE(battery.time_remaining_s) + && (!PX4_ISFINITE(worst_battery_time_s) + || (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) { + worst_battery_time_s = battery.time_remaining_s; + } - _rtl_flight_time_sub.update(); - float battery_usage_to_home = 0; - - if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) { - battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction; - } - - uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE; - - if (PX4_ISFINITE(battery_usage_to_home)) { - float battery_at_home = battery_level - battery_usage_to_home; - - if (battery_at_home < _param_bat_crit_thr.get()) { - battery_range_warning = battery_status_s::BATTERY_WARNING_CRITICAL; - - } else if (battery_at_home < _param_bat_low_thr.get()) { - battery_range_warning = battery_status_s::BATTERY_WARNING_LOW; + // Sum up current from all batteries. + _battery_current += battery.current_filtered_a; } } - if (battery_range_warning > worst_warning) { - worst_warning = battery_range_warning; + rtl_time_estimate_s rtl_time_estimate{}; + + // Compare estimate of RTL time to estimate of remaining flight time + if (_rtl_time_estimate_sub.copy(&rtl_time_estimate) + && hrt_absolute_time() - rtl_time_estimate.timestamp < 2_s + && rtl_time_estimate.valid + && _armed.armed + && !_rtl_time_actions_done + && PX4_ISFINITE(worst_battery_time_s) + && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { + // Try to trigger RTL + if (main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, + _internal_state) == TRANSITION_CHANGED) { + mavlink_log_emergency(&_mavlink_log_pub, "Flight time low, returning to land"); + + } else { + mavlink_log_emergency(&_mavlink_log_pub, "Flight time low, land now!"); + } + + _rtl_time_actions_done = true; } bool battery_warning_level_increased_while_armed = false; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 5725b5e6a0..97348e0efd 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -77,7 +77,7 @@ #include #include #include -#include +#include #include #include #include @@ -321,6 +321,8 @@ private: bool _geofence_warning_action_on{false}; bool _geofence_violated_prev{false}; + bool _rtl_time_actions_done{false}; + FailureDetector _failure_detector; bool _flight_termination_triggered{false}; bool _lockdown_triggered{false}; @@ -408,6 +410,7 @@ private: uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)}; uORB::Subscription _safety_sub{ORB_ID(safety)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; @@ -431,7 +434,6 @@ private: uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::SubscriptionData _rtl_flight_time_sub{ORB_ID(rtl_flight_time)}; // Publications uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index bc703e4d0a..958658224f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -83,7 +83,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("px4io_status"); add_topic("radio_status"); add_optional_topic("rpm", 500); - add_topic("rtl_flight_time", 1000); + add_topic("rtl_time_estimate", 1000); add_topic("safety"); add_topic("sensor_combined"); add_optional_topic("sensor_correction"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index be24cfdfec..80ecaa646e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,6 +57,14 @@ RTL::RTL(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { + _param_mpc_z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP"); + _param_mpc_z_vel_max_down = param_find("MPC_Z_VEL_MAX_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::on_inactivation() @@ -71,22 +79,20 @@ void RTL::on_inactive() // Reset RTL state. _rtl_state = RTL_STATE_NONE; - find_RTL_destination(); + // Limit inactive calculation to 1Hz + if ((hrt_absolute_time() - _destination_check_time) > 1_s) { + _destination_check_time = hrt_absolute_time(); + + if (_navigator->home_position_valid()) { + find_RTL_destination(); + } + + calc_and_pub_rtl_time_estimate(); + } } void RTL::find_RTL_destination() { - // don't update RTL destination faster than 1 Hz - if (hrt_elapsed_time(&_destination_check_time) < 1_s) { - return; - } - - if (!_navigator->home_position_valid()) { - return; - } - - _destination_check_time = hrt_absolute_time(); - // get home position: home_position_s &home_landing_position = *_navigator->get_home_position(); @@ -225,23 +231,13 @@ void RTL::find_RTL_destination() } } - // figure out how long the RTL will take - float rtl_xy_speed, rtl_z_speed; - get_rtl_xy_z_speed(rtl_xy_speed, rtl_z_speed); + if (_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()); - matrix::Vector3f 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)); - to_destination_vec(2) = _destination.alt - global_position.alt; - - float time_to_home_s = time_to_home(to_destination_vec, get_wind(), rtl_xy_speed, rtl_z_speed); - - float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get()); - rtl_flight_time_s rtl_flight_time{}; - rtl_flight_time.timestamp = hrt_absolute_time(); - rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio; - rtl_flight_time.rtl_time_s = time_to_home_s; - _rtl_flight_time_pub.publish(rtl_flight_time); + } else { + _rtl_alt = max(global_position.alt, max(_destination.alt, + _navigator->get_home_position()->alt + _param_rtl_return_alt.get())); + } } void RTL::on_activation() @@ -268,14 +264,6 @@ void RTL::on_activation() _rtl_loiter_rad = _param_rtl_loiter_rad.get(); - if (_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()); - - } else { - _rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); - } - - if (_navigator->get_land_detected()->landed) { // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; @@ -316,6 +304,12 @@ void RTL::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(); + calc_and_pub_rtl_time_estimate(); + } } void RTL::set_rtl_item() @@ -730,41 +724,148 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) return max(return_altitude_amsl, gpos.alt); } -void RTL::get_rtl_xy_z_speed(float &xy, float &z) +void RTL::calc_and_pub_rtl_time_estimate() { - uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type; - // Caution: here be dragons! - // Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover + rtl_time_estimate_s rtl_time_estimate{}; - if (vehicle_type != _rtl_vehicle_type) { - _rtl_vehicle_type = vehicle_type; + // Calculate RTL time estimate only when there is a valid home position + // TODO: Also check if vehicle position is valid + if (!_navigator->home_position_valid()) { + rtl_time_estimate.valid = false; + + } else { + rtl_time_estimate.valid = true; + + const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + + // 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 (climb_dist > 0) { + 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; + float loiter_altitude = 0; + + 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(); + } + + } - switch (vehicle_type) { - case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: - _param_rtl_xy_speed = param_find("MPC_XY_CRUISE"); - _param_rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN"); break; - case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: - _param_rtl_xy_speed = param_find("FW_AIRSPD_TRIM"); - _param_rtl_descent_speed = param_find("FW_T_SINK_MIN"); + case RTL_STATE_LANDED: + // Remaining time is 0 break; + } - case vehicle_status_s::VEHICLE_TYPE_ROVER: - _param_rtl_xy_speed = param_find("GND_SPEED_THR_SC"); - _param_rtl_descent_speed = PARAM_INVALID; - 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(); + } + + // Publish message + rtl_time_estimate.timestamp = hrt_absolute_time(); + _rtl_time_estimate_pub.publish(rtl_time_estimate); +} + +float RTL::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; } } - if ((_param_rtl_xy_speed == PARAM_INVALID) || param_get(_param_rtl_xy_speed, &xy) != PX4_OK) { - xy = 1e6f; - } - - if ((_param_rtl_descent_speed == PARAM_INVALID) || param_get(_param_rtl_descent_speed, &z) != PX4_OK) { - z = 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() @@ -780,27 +881,67 @@ matrix::Vector2f RTL::get_wind() return wind; } -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) +float RTL::getClimbRate() { - const matrix::Vector2f to_home = to_home_vec.xy(); - const float alt_change = to_home_vec(2); - const matrix::Vector2f to_home_dir = to_home.unit_or_zero(); - const float dist_to_home = to_home.norm(); + float ret = 1e6f; - const float wind_towards_home = wind_velocity.dot(to_home_dir); - const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * wind_towards_home).norm(); + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_vel_max_up == PARAM_INVALID || param_get(_param_mpc_z_vel_max_up, &ret) != PX4_OK) { + ret = 1e6f; + } - // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient - const float cruise_speed = sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + fminf( - 0.f, wind_towards_home); + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) { - return INFINITY; // we never reach home if the wind is stronger than vehicle speed + if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { + ret = 1e6f; + } } - // assume horizontal and vertical motions happen serially, so their time adds - float horiz = dist_to_home / cruise_speed; - float descent = fabsf(alt_change) / vehicle_descent_speed_m_s; - return horiz + descent; + 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_vel_max_down == PARAM_INVALID || param_get(_param_mpc_z_vel_max_down, &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; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 1c0ca650a8..0dfc3afc7e 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include #include @@ -109,6 +109,17 @@ private: void advance_rtl(); float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); + void calc_and_pub_rtl_time_estimate(); + + float getCruiseGroundSpeed(); + + float getClimbRate(); + + float getDescendRate(); + + float getCruiseSpeed(); + + float getHoverLandSpeed(); enum RTLState { RTL_STATE_NONE = 0, @@ -159,21 +170,25 @@ private: (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamFloat) _param_rtl_flt_time, (ParamInt) _param_rtl_pld_md, (ParamFloat) _param_rtl_loiter_rad, - (ParamInt) _param_rtl_hdg_md + (ParamInt) _param_rtl_hdg_md, + (ParamFloat) _param_rtl_time_factor, + (ParamInt) _param_rtl_time_margin ) - // These need to point at different parameters depending on vehicle type. - // Can't hard-code them because we have non-MC/FW/Rover builds - uint8_t _rtl_vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; + param_t _param_mpc_z_vel_max_up{PARAM_INVALID}; + param_t _param_mpc_z_vel_max_down{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_rtl_xy_speed{PARAM_INVALID}; - param_t _param_rtl_descent_speed{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::SubscriptionData _wind_sub{ORB_ID(wind)}; - uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; + uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; }; float time_to_home(const matrix::Vector3f &to_home_vec, diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 4722f76b18..76c50a4b57 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -137,17 +137,6 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0); */ PARAM_DEFINE_INT32(RTL_CONE_ANG, 45); -/** - * Maximum allowed RTL flight in minutes - * - * This is used to determine when the vehicle should be switched to RTL due to low battery. - * Note, particularly for multirotors this should reflect flight time at cruise speed, not while stationary - * - * @unit min - * @group Commander - */ -PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15); - /** * RTL precision land mode * @@ -185,3 +174,32 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); * @group Return Mode */ PARAM_DEFINE_INT32(RTL_HDG_MD, 0); + +/** + * RTL time estimate safety margin factor + * + * Safety factor that is used to scale the actual RTL time estiamte. + * Time with margin = RTL_TIME_FACTOR * time + RTL_TIME_MARGIN + * + * @min 1.0 + * @max 2.0 + * @decimal 1 + * @increment 0.1 + * @group Return To Land + */ +PARAM_DEFINE_FLOAT(RTL_TIME_FACTOR, 1.1f); + +/** + * RTL time estimate safety margin offset + * + * Margin that is added to the time estimate, after it has already been scaled + * Time with margin = RTL_TIME_FACTOR * time + RTL_TIME_MARGIN + * + * @unit s + * @min 0 + * @max 300 + * @decimal 1 + * @increment 1 + * @group Return To Land + */ +PARAM_DEFINE_INT32(RTL_TIME_MARGIN, 110);