Add trig tests for wind calculations, and fix bugs / edge cases

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

src/modules/navigator/CMakeLists.txt

@@ -57,4 +57,6 @@ px4_add_module(
 		landing_slope
 		geofence_breach_avoidance
 		motion_planning
-	)
+	)
+
+px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman)

src/modules/navigator/RangeRTLTest.cpp

@@ -0,0 +1,120 @@
+#define MODULE_NAME "Navigator"
+
+#include "navigator.h"
+
+#include "rtl.h"
+
+#include <gtest/gtest.h>
+
+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);
+}

src/modules/navigator/rtl.cpp

@@ -43,8 +43,6 @@
 #include "navigator.h"
 #include <dataman/dataman.h>
 
-#include <climits>
-
 
 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;

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);
+PARAM_DEFINE_INT32(RTL_PLD_MD, 0);