diff --git a/msg/rtl_flight_time.msg b/msg/rtl_flight_time.msg index 6ca0912dd7..b21a37623e 100644 --- a/msg/rtl_flight_time.msg +++ b/msg/rtl_flight_time.msg @@ -1,5 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint8 rtl_vehicle_type # from the vehicle_status message +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/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 08accf2e23..4a237ccc5b 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -57,4 +57,6 @@ px4_add_module( landing_slope geofence_breach_avoidance motion_planning - ) \ No newline at end of file + ) + +px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman) \ No newline at end of file diff --git a/src/modules/navigator/RangeRTLTest.cpp b/src/modules/navigator/RangeRTLTest.cpp new file mode 100644 index 0000000000..6de2bd1488 --- /dev/null +++ b/src/modules/navigator/RangeRTLTest.cpp @@ -0,0 +1,120 @@ +#define MODULE_NAME "Navigator" + +#include "navigator.h" + +#include "rtl.h" + +#include + +TEST(Navigator_and_RTL, compiles_woohoooo) +{ + Navigator n; + RTL rtl(&n); + + rtl.find_RTL_destination(); +} + +class RangeRTL_tth : public ::testing::Test +{ +public: + matrix::Vector3f vehicle_local_pos ; + matrix::Vector3f rtl_point_local_pos ; + matrix::Vector2f wind_vel; + float vehicle_speed; + float vehicle_descent_speed; + + void SetUp() override + { + vehicle_local_pos = matrix::Vector3f(0, 0, 0); + rtl_point_local_pos = matrix::Vector3f(0, 0, 0); + wind_vel = matrix::Vector2f(0, 0); + vehicle_speed = 5; + vehicle_descent_speed = 1; + } +}; + +TEST_F(RangeRTL_tth, zero_distance_zero_time) +{ + // GIVEN: zero distances (defaults) + + // WHEN: we get the tth + float tth = time_to_home(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be zero + EXPECT_FLOAT_EQ(tth, 0.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy) +{ + // GIVEN: 10 seconds of distance + vehicle_speed = 6.2f; + vehicle_local_pos(0) = vehicle_local_pos(1) = (vehicle_speed * 10) / sqrtf(2); + + // WHEN: we get the tth + float tth = time_to_home(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be zero + EXPECT_FLOAT_EQ(tth, 10.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_5_seconds_z) +{ + // GIVEN: 10 seconds of xy distance and 5 seconds of Z + vehicle_speed = 4.2f; + vehicle_descent_speed = 1.2f; + vehicle_local_pos(0) = vehicle_local_pos(1) = (vehicle_speed * 10) / sqrtf(2); + vehicle_local_pos(2) = vehicle_descent_speed * 5; + + // WHEN: we get the tth + float tth = time_to_home(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 15 seconds + EXPECT_FLOAT_EQ(tth, 15.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_downwind_to_home) +{ + // GIVEN: 10 seconds of xy distance and 5 seconds of Z, and the wind is towards home + vehicle_speed = 4.2f; + vehicle_local_pos(0) = vehicle_local_pos(1) = (vehicle_speed * 10) / sqrtf(2); + + wind_vel = matrix::Vector2f(-1, -1); + + // WHEN: we get the tth + float tth = time_to_home(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 10, because we don't rely on wind towards home for RTL + EXPECT_FLOAT_EQ(tth, 10.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_upwind_to_home) +{ + // GIVEN: 10 seconds of distance + vehicle_speed = 4.2f; + vehicle_descent_speed = 1.2f; + vehicle_local_pos(0) = vehicle_local_pos(1) = (vehicle_speed * 10) / sqrtf(2); + + wind_vel = matrix::Vector2f(1, 1) / sqrt(2) * vehicle_speed / 10; + + // WHEN: we get the tth + float tth = time_to_home(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 11.111111... because it slows us down by 10% and time = dist/speed + EXPECT_FLOAT_EQ(tth, 10 / 0.9f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_z_wind_across_home) +{ + // GIVEN: a 3 4 5 triangle, with vehicle airspeed being 5, wind 3, ground speed 4 + // and the vehicle is 10 seconds away + + vehicle_speed = 5.f; + wind_vel = matrix::Vector2f(-1, 1) / sqrt(2) * 3.; + vehicle_local_pos(0) = vehicle_local_pos(1) = (4 * 10) / sqrtf(2); + + // WHEN: we get the tth + float tth = time_to_home(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 10 + EXPECT_FLOAT_EQ(tth, 10); +} diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 5a2c8530e6..ae035eca1a 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -43,8 +43,6 @@ #include "navigator.h" #include -#include - static constexpr float DELAY_SIGMA = 0.01f; @@ -684,12 +682,16 @@ float time_to_home(const matrix::Vector3f &vehicle_local_pos, 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(); + const float wind_across_home = matrix::Vector2f(wind_velocity - 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 cruise_speed = sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + fminf(0, wind_towards_home); + if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) { + return INFINITY; // we never reach home if the wind is stronger than vehicle speed + } + // 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_params.c b/src/modules/navigator/rtl_params.c index 0fe23ae9d6..019e67f0db 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -157,4 +157,4 @@ PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15); * @value 2 Required precision landing * @group Return To Land */ -PARAM_DEFINE_INT32(RTL_PLD_MD, 0); \ No newline at end of file +PARAM_DEFINE_INT32(RTL_PLD_MD, 0);