From ea3b99e9648fda6091b4f8f025d000fc894b6eff Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 4 Aug 2021 11:40:14 +0200 Subject: [PATCH] navigator: Remove deprecated RangeRTL unit tests --- src/modules/navigator/CMakeLists.txt | 2 - src/modules/navigator/RangeRTLTest.cpp | 219 ------------------------- 2 files changed, 221 deletions(-) delete mode 100644 src/modules/navigator/RangeRTLTest.cpp diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 5668be5846..7803826920 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -56,5 +56,3 @@ px4_add_module( geofence_breach_avoidance motion_planning ) - -px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman) diff --git a/src/modules/navigator/RangeRTLTest.cpp b/src/modules/navigator/RangeRTLTest.cpp deleted file mode 100644 index 13efe03de4..0000000000 --- a/src/modules/navigator/RangeRTLTest.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#define MODULE_NAME "Navigator" - -#include "navigator.h" -#include "rtl.h" - -#include - - -#include - -TEST(Navigator_and_RTL, interact_correctly) -{ - Navigator n; - RTL rtl(&n); - - - home_position_s home_pos{}; - home_pos.valid_hpos = true; - home_pos.valid_lpos = true; - home_pos.valid_alt = true; - home_pos.timestamp = 1000; - - vehicle_global_position_s glob_pos{}; - - vehicle_status_s v_status{}; - v_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - // TODO: can't do this, it hangs forever in Navigator's while loop - // uORB::Publication home_pos_pub{ORB_ID(home_position)}; - // uORB::Publication global_pos_pub{ORB_ID(vehicle_global_position)}; - // uORB::Publication local_pos_pub{ORB_ID(vehicle_local_position)}; - // uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - // home_pos_pub.publish(home_pos); - // global_pos_pub.publish(glob_pos); - // local_pos_pub.publish(local_pos); - // vehicle_status_pub.publish(v_status); - // n.run(); - - // Hacky-hack, don't use pub-sub, just set them directly in navigator. NB! This isn't the "real" API, they should - // be set via pub-sub otherwise this will be a constant drag on development - *n.get_home_position() = home_pos; - *n.get_global_position() = glob_pos; - *n.get_vstatus() = v_status; - - uORB::SubscriptionData _rtl_flight_time_sub{ORB_ID(rtl_flight_time)}; - ASSERT_FALSE(_rtl_flight_time_sub.update()); - rtl.find_RTL_destination(); - ASSERT_TRUE(_rtl_flight_time_sub.update()); - auto msg = _rtl_flight_time_sub.get(); - EXPECT_EQ(msg.rtl_time_s, 0); - - // WHEN: we set the vehicle type to multirotor - v_status.vehicle_type = n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - float xy, z; - rtl.get_rtl_xy_z_speed(xy, z); - - // THEN: the RTL speed should correspond to multirotor parameters - float xy_desired, z_desired; - param_get(param_handle(px4::params::MPC_XY_CRUISE), &xy_desired); - param_get(param_handle(px4::params::MPC_Z_VEL_MAX_DN), &z_desired); - EXPECT_FLOAT_EQ(xy, xy_desired); - EXPECT_FLOAT_EQ(z, z_desired); - - // WHEN: it is a fixed wing - n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - rtl.get_rtl_xy_z_speed(xy, z); - - // THEN: it should be fixed wing parameters - param_get(param_handle(px4::params::FW_AIRSPD_TRIM), &xy_desired); - param_get(param_handle(px4::params::FW_T_SINK_MIN), &z_desired); - EXPECT_FLOAT_EQ(xy, xy_desired); - EXPECT_FLOAT_EQ(z, z_desired); - - // WHEN: it is rover - n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; - rtl.get_rtl_xy_z_speed(xy, z); - - // THEN: it should be rover parameters, and z should just be large (no RTL time in Z -> high speed) - param_get(param_handle(px4::params::GND_SPEED_THR_SC), &xy_desired); - EXPECT_FLOAT_EQ(xy, xy_desired); - EXPECT_GT(z, 1000); -} - -class RangeRTL_tth : public ::testing::Test -{ -public: - matrix::Vector3f rtl_vector; - matrix::Vector3f rtl_point_local_pos; - matrix::Vector2f wind_vel; - float vehicle_speed; - float vehicle_descent_speed; - - void SetUp() override - { - rtl_vector = 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(rtl_vector, 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; - rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); - - // WHEN: we get the tth - float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); - - // THEN: it should be ten seconds - 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; - rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); - rtl_vector(2) = vehicle_descent_speed * 5; - - // WHEN: we get the tth - float tth = time_to_home(rtl_vector, 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; - rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); - - wind_vel = matrix::Vector2f(-1, -1); - - // WHEN: we get the tth - float tth = time_to_home(rtl_vector, 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; - rtl_vector(0) = rtl_vector(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(rtl_vector, 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.; - rtl_vector(0) = rtl_vector(1) = -(4 * 10) / sqrtf(2); - - // WHEN: we get the tth - float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); - - // THEN: it should be 10 - EXPECT_FLOAT_EQ(tth, 10); -} - -TEST_F(RangeRTL_tth, too_strong_upwind_to_home) -{ - // GIVEN: 10 seconds of distance - vehicle_speed = 4.2f; - vehicle_descent_speed = 1.2f; - rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); - - wind_vel = matrix::Vector2f(1, 1) / sqrt(2) * vehicle_speed * 1.001f; - - // WHEN: we get the tth - float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); - - // THEN: it should never get home - EXPECT_TRUE(std::isinf(tth)) << tth; -} - -TEST_F(RangeRTL_tth, too_strong_crosswind_to_home) -{ - // GIVEN: 10 seconds of distance - vehicle_speed = 4.2f; - vehicle_descent_speed = 1.2f; - rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); - - wind_vel = matrix::Vector2f(1, -1) / sqrt(2) * vehicle_speed * 1.001f; - - // WHEN: we get the tth - float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); - - // THEN: it should never get home - EXPECT_TRUE(std::isinf(tth)) << tth; -}