From 748241300553202429ef1ef400f425c03b021f66 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Thu, 23 Jul 2020 18:35:56 +0200 Subject: [PATCH] Add Range-based RTL --- msg/CMakeLists.txt | 1 + msg/rtl_flight_time.msg | 5 ++ src/modules/commander/Commander.cpp | 33 ++++++++ src/modules/commander/Commander.hpp | 9 +- .../commander/state_machine_helper.cpp | 3 - src/modules/navigator/rtl.cpp | 83 +++++++++++++++++++ src/modules/navigator/rtl.h | 21 +++++ src/modules/navigator/rtl_params.c | 12 ++- 8 files changed, 162 insertions(+), 5 deletions(-) create mode 100644 msg/rtl_flight_time.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 307ae06196..8b08cfcf6e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -108,6 +108,7 @@ set(msg_files rc_channels.msg rc_parameter_map.msg rpm.msg + rtl_flight_time.msg safety.msg satellite_info.msg sensor_accel.msg diff --git a/msg/rtl_flight_time.msg b/msg/rtl_flight_time.msg new file mode 100644 index 0000000000..6ca0912dd7 --- /dev/null +++ b/msg/rtl_flight_time.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 rtl_vehicle_type # from the vehicle_status message +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/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c7716717b6..15c8ee97a6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3858,6 +3858,8 @@ void Commander::battery_status_check() hrt_abstime oldest_update = hrt_absolute_time(); _battery_current = 0.0f; + float battery_level = 0.0f; + // 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++) { @@ -3871,6 +3873,36 @@ void Commander::battery_status_check() // Sum up current from all batteries. _battery_current += batteries[i].current_filtered_a; + + // average levels from all batteries + battery_level += batteries[i].remaining; + } + + battery_level /= num_connected_batteries; + + _rtl_flight_time_sub.update(); + float battery_usage_to_home = _rtl_flight_time_sub.valid() ? + _rtl_flight_time_sub.get().rtl_limit_fraction : 0; + + + auto warning_level = [this](float battery_level_fraction, float battery_to_home) { + float battery_at_home = battery_level_fraction - battery_to_home; + + if (battery_at_home < _param_bat_crit_thr.get()) { + return battery_status_s::BATTERY_WARNING_CRITICAL; + } + + if (battery_at_home < _param_bat_low_thr.get()) { + return battery_status_s::BATTERY_WARNING_LOW; + } + + return battery_status_s::BATTERY_WARNING_NONE; + }; + + uint8_t battery_range_warning = warning_level(battery_level, battery_usage_to_home); + + if (battery_range_warning > worst_warning) { + worst_warning = battery_range_warning; } bool battery_warning_level_increased_while_armed = false; @@ -3892,6 +3924,7 @@ void Commander::battery_status_check() _battery_warning = worst_warning; } + _status_flags.condition_battery_healthy = // All connected batteries are regularly being published (hrt_elapsed_time(&oldest_update) < 5_s) diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 608bbfa48e..d47d18454e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -33,6 +33,8 @@ #pragma once + +/* Helper classes */ #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" #include "state_machine_helper.h" @@ -75,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -269,7 +272,10 @@ private: (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_type, - (ParamFloat) _param_cp_dist + (ParamFloat) _param_cp_dist, + + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_bat_crit_thr ) enum class PrearmedMode { @@ -426,6 +432,7 @@ 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/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 75d766a23c..ee99c0a8e5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -985,9 +985,6 @@ void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsaf } } - - - void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning, const low_battery_action_t low_battery_action) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2fc3ba2d4a..5a2c8530e6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -43,6 +43,8 @@ #include "navigator.h" #include +#include + static constexpr float DELAY_SIGMA = 0.01f; @@ -212,6 +214,23 @@ void RTL::find_RTL_destination() } } + // figure out how long the RTL will take + const vehicle_local_position_s &local_pos_s = *_navigator->get_local_position(); + + matrix::Vector3f local_pos(local_pos_s.x, local_pos_s.y, local_pos_s.z); + matrix::Vector3f local_destination; // TODO + + uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type; + float xy_speed, z_speed; + get_rtl_xy_z_speed(vehicle_type, xy_speed, z_speed); + float time_to_home_s = time_to_home(local_pos, local_destination, get_wind(), xy_speed, 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.rtl_limit_fraction = rtl_flight_time_ratio; + rtl_flight_time.rtl_time_s = time_to_home_s; + rtl_flight_time.rtl_vehicle_type = vehicle_type; + _rtl_flight_time_pub.publish(rtl_flight_time); } void RTL::on_activation() @@ -611,3 +630,67 @@ 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(uint8_t vehicle_type, float &xy, float &z) +{ + // Caution: here be dragons! + // Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover + + if (vehicle_type != _rtl_vehicle_type) { + _rtl_vehicle_type = vehicle_type; + + switch (vehicle_type) { + case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: + _rtl_xy_speed = param_find("MPC_XY_CRUISE"); + _rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN"); + break; + + case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: + _rtl_xy_speed = param_find("FW_AIRSPD_TRIM"); + _rtl_descent_speed = param_find("FW_T_SINK_MAX"); + break; + + case vehicle_status_s::VEHICLE_TYPE_ROVER: + _rtl_xy_speed = param_find("GND_SPEED_THR_SC"); + _rtl_descent_speed = 65535; + break; + } + } + + if (param_get(_rtl_xy_speed, &xy) != 0) { + xy = 1e6f; + } + + if (param_get(_rtl_descent_speed, &z) != 0) { + z = 1e6f; + } + +} + +matrix::Vector2f RTL::get_wind() +{ + _wind_estimate_sub.update(); + matrix::Vector2f wind(_wind_estimate_sub.get().windspeed_north, _wind_estimate_sub.get().windspeed_east); + return wind; +} + +float time_to_home(const matrix::Vector3f &vehicle_local_pos, + const matrix::Vector3f &rtl_point_local_pos, + const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s) +{ + const matrix::Vector2f to_home = (rtl_point_local_pos - vehicle_local_pos).xy(); + const float alt_change = rtl_point_local_pos(2) - vehicle_local_pos(2); + const matrix::Vector2f to_home_dir = to_home.unit_or_zero(); + const float dist_to_home = to_home.norm(); + + const float wind_towards_home = wind_velocity.dot(to_home_dir); + const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * fabsf(wind_towards_home)).norm(); + + // 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, + wind_towards_home); + + // assume horizontal and vertical motions happen serially, so their time adds + const float time_to_home = dist_to_home / cruise_speed + fabsf(alt_change) / vehicle_descent_speed_m_s; + return time_to_home; +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 7439438f1b..eabd0a9ba2 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -46,7 +46,11 @@ #include "navigator_mode.h" #include "mission_block.h" +#include #include +#include +#include +#include class Navigator; @@ -100,6 +104,8 @@ private: */ void advance_rtl(); + void get_rtl_xy_z_speed(uint8_t vehicle_type, float &xy, float &z); + matrix::Vector2f get_wind(); float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); @@ -150,6 +156,21 @@ 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 ) + + // 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{255}; + param_t _rtl_xy_speed; + param_t _rtl_descent_speed; + + uORB::SubscriptionData _wind_estimate_sub{ORB_ID(wind_estimate)}; + uORB::PublicationData _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; }; + +float time_to_home(const matrix::Vector3f &vehicle_local_pos, + const matrix::Vector3f &rtl_point_local_pos, + const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, + float vehicle_descent_speed_m_s); diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 3c56c41f94..0fe23ae9d6 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -137,6 +137,16 @@ 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 + * + * @group Commander + */ +PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15); + /** * RTL precision land mode * @@ -147,4 +157,4 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 45); * @value 2 Required precision landing * @group Return To Land */ -PARAM_DEFINE_INT32(RTL_PLD_MD, 0); +PARAM_DEFINE_INT32(RTL_PLD_MD, 0); \ No newline at end of file