diff --git a/src/modules/navigator/RangeRTLTest.cpp b/src/modules/navigator/RangeRTLTest.cpp index 6de2bd1488d..fd3a993077f 100644 --- a/src/modules/navigator/RangeRTLTest.cpp +++ b/src/modules/navigator/RangeRTLTest.cpp @@ -1,9 +1,11 @@ #define MODULE_NAME "Navigator" #include "navigator.h" - #include "rtl.h" +#include + + #include TEST(Navigator_and_RTL, compiles_woohoooo) @@ -11,7 +13,43 @@ TEST(Navigator_and_RTL, compiles_woohoooo) Navigator n; RTL rtl(&n); + + home_position_s home_pos{}; + home_pos.valid_hpos = true; + home_pos.valid_alt = true; + + vehicle_global_position_s glob_pos{}; + + vehicle_local_position_s local_pos{}; + local_pos.xy_valid = true; + local_pos.z_valid = true; + + 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 the 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_local_position() = local_pos; + *n.get_vstatus() = v_status; + + uORB::SubscriptionData _rtl_flight_time_sub{ORB_ID(rtl_flight_time)}; 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); } class RangeRTL_tth : public ::testing::Test @@ -118,3 +156,35 @@ TEST_F(RangeRTL_tth, ten_seconds_xy_z_wind_across_home) // 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; + vehicle_local_pos(0) = vehicle_local_pos(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(vehicle_local_pos, rtl_point_local_pos, 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; + vehicle_local_pos(0) = vehicle_local_pos(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(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should never get home + EXPECT_TRUE(std::isinf(tth)) << tth; +} diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index eae7253df13..e28cb6e7ff1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -44,6 +44,8 @@ #include "navigator.h" #include +#include + static constexpr float DELAY_SIGMA = 0.01f; @@ -216,20 +218,30 @@ 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 + if (local_pos_s.xy_valid && local_pos_s.z_valid) { + matrix::Vector3f local_pos(local_pos_s.x, local_pos_s.y, local_pos_s.z); - 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); + matrix::Vector3f local_destination(0, 0, global_position.alt); // TODO - 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); + if (!map_projection_initialized(&_projection_reference)) { + map_projection_init(&_projection_reference, global_position.lat, global_position.lon); + } + + map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &local_destination(0), + &local_destination(1)); + + 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() @@ -686,8 +698,8 @@ float time_to_home(const matrix::Vector3f &vehicle_local_pos, 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 = fminf(vehicle_speed_m_s, - sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + wind_towards_home); + 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); if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) { return INFINITY; // we never reach home if the wind is stronger than vehicle speed diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index eabd0a9ba2f..9f445180e14 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -51,6 +51,7 @@ #include #include #include +#include class Navigator; @@ -168,6 +169,8 @@ private: uORB::SubscriptionData _wind_estimate_sub{ORB_ID(wind_estimate)}; uORB::PublicationData _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; + + map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] }; float time_to_home(const matrix::Vector3f &vehicle_local_pos,