Test the navigator side, and fix another trig edge case

src/modules/navigator/RangeRTLTest.cpp

@@ -1,9 +1,11 @@
 #define MODULE_NAME "Navigator"
 
 #include "navigator.h"
-
 #include "rtl.h"
 
+#include <future>
+
+
 #include <gtest/gtest.h>
 
 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_position_s> home_pos_pub{ORB_ID(home_position)};
+	// uORB::Publication<vehicle_global_position_s> global_pos_pub{ORB_ID(vehicle_global_position)};
+	// uORB::Publication<vehicle_local_position_s> local_pos_pub{ORB_ID(vehicle_local_position)};
+	// uORB::Publication<vehicle_status_s> 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_s> _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;
+}

src/modules/navigator/rtl.cpp

@@ -44,6 +44,8 @@
 #include "navigator.h"
 #include <dataman/dataman.h>
 
+#include <lib/ecl/geo/geo.h>
+
 
 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

src/modules/navigator/rtl.h

@@ -51,6 +51,7 @@
 #include <uORB/topics/rtl_flight_time.h>
 #include <uORB/topics/wind_estimate.h>
 #include <matrix/math.hpp>
+#include <lib/ecl/geo/geo.h>
 
 class Navigator;
 
@@ -168,6 +169,8 @@ private:
 
 	uORB::SubscriptionData<wind_estimate_s>		_wind_estimate_sub{ORB_ID(wind_estimate)};
 	uORB::PublicationData<rtl_flight_time_s>	_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,