Fix spurious RTL triggers

Two sources:
1. global to local conversion was sometimes giving issues, so do everything in global
2. on startup the RTL didn't check if the home position was valid before processing it

src/modules/commander/Commander.cpp

@@ -3881,25 +3881,24 @@ void Commander::battery_status_check()
 	battery_level /= num_connected_batteries;
 
 	_rtl_flight_time_sub.update();
-	float battery_usage_to_home = _rtl_flight_time_sub.valid() ?
+	float battery_usage_to_home = 0;
-				      _rtl_flight_time_sub.get().rtl_limit_fraction : 0;
 
+	if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) {
+		battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction;
+	}
 
-	auto warning_level = [this](float battery_level_fraction, float battery_to_home) {
+	uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE;
-		float battery_at_home = battery_level_fraction - battery_to_home;
+
+	if (PX4_ISFINITE(battery_usage_to_home)) {
+		float battery_at_home = battery_level - battery_usage_to_home;
 
 		if (battery_at_home < _param_bat_crit_thr.get()) {
-			return battery_status_s::BATTERY_WARNING_CRITICAL;
+			battery_range_warning =  battery_status_s::BATTERY_WARNING_CRITICAL;
+
+		} else if (battery_at_home < _param_bat_low_thr.get()) {
+			battery_range_warning = battery_status_s::BATTERY_WARNING_LOW;
 		}
-
+	}
-		if (battery_at_home < _param_bat_low_thr.get()) {
-			return battery_status_s::BATTERY_WARNING_LOW;
-		}
-
-		return battery_status_s::BATTERY_WARNING_NONE;
-	};
-
-	uint8_t battery_range_warning = warning_level(battery_level, battery_usage_to_home);
 
 	if (battery_range_warning > worst_warning) {
 		worst_warning = battery_range_warning;

src/modules/navigator/RangeRTLTest.cpp

@@ -83,15 +83,15 @@ TEST(Navigator_and_RTL, interact_correctly)
 class RangeRTL_tth : public ::testing::Test
 {
 public:
-	matrix::Vector3f vehicle_local_pos ;
+	matrix::Vector3f rtl_vector;
-	matrix::Vector3f rtl_point_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_vector  = matrix::Vector3f(0, 0, 0);
 		rtl_point_local_pos  = matrix::Vector3f(0, 0, 0);
 		wind_vel  = matrix::Vector2f(0, 0);
 		vehicle_speed = 5;
@@ -104,7 +104,7 @@ 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);
+	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);
@@ -114,12 +114,12 @@ 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);
+	rtl_vector(0) = rtl_vector(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);
+	float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed);
 
-	// THEN: it should be zero
+	// THEN: it should be ten seconds
 	EXPECT_FLOAT_EQ(tth, 10.f);
 }
 
@@ -128,11 +128,11 @@ 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);
+	rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2);
-	vehicle_local_pos(2) = vehicle_descent_speed * 5;
+	rtl_vector(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);
+	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);
@@ -142,12 +142,12 @@ 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);
+	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(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed);
+	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);
@@ -158,12 +158,12 @@ 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);
+	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(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed);
+	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);
@@ -176,10 +176,10 @@ TEST_F(RangeRTL_tth, ten_seconds_xy_z_wind_across_home)
 
 	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);
+	rtl_vector(0) = rtl_vector(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);
+	float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed);
 
 	// THEN: it should be 10
 	EXPECT_FLOAT_EQ(tth, 10);
@@ -190,12 +190,12 @@ 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);
+	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(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed);
+	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;
@@ -206,12 +206,12 @@ 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);
+	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(vehicle_local_pos, rtl_point_local_pos, wind_vel, vehicle_speed, vehicle_descent_speed);
+	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;

src/modules/navigator/rtl.cpp

@@ -156,7 +156,7 @@ void RTL::find_RTL_destination()
 	}
 
 	// compare to safe landing positions
-	mission_safe_point_s closest_safe_point {} ;
+	mission_safe_point_s closest_safe_point {};
 	mission_stats_entry_s stats;
 	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
 	int num_safe_points = 0;
@@ -216,31 +216,22 @@ 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();
+	float rtl_xy_speed, rtl_z_speed;
+	get_rtl_xy_z_speed(rtl_xy_speed, rtl_z_speed);
 
-	if (local_pos_s.xy_valid && local_pos_s.z_valid) {
+	matrix::Vector3f to_destination_vec;
-		matrix::Vector3f local_pos(local_pos_s.x, local_pos_s.y, local_pos_s.z);
+	get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
+				    &to_destination_vec(0), &to_destination_vec(1));
+	to_destination_vec(2) = _destination.alt - global_position.alt;
 
-		matrix::Vector3f local_destination(0, 0, global_position.alt); // TODO
+	float time_to_home_s = time_to_home(to_destination_vec, get_wind(), rtl_xy_speed, rtl_z_speed);
 
-		if (!map_projection_initialized(&_projection_reference)) {
+	float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get());
-			map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
+	rtl_flight_time_s rtl_flight_time{};
-		}
+	rtl_flight_time.timestamp = hrt_absolute_time();
-
+	rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
-		map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &local_destination(0),
+	rtl_flight_time.rtl_time_s = time_to_home_s;
-				       &local_destination(1));
+	_rtl_flight_time_pub.publish(rtl_flight_time);
-
-		float xy_speed, z_speed;
-		get_rtl_xy_z_speed(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.timestamp = hrt_absolute_time();
-		rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
-		rtl_flight_time.rtl_time_s = time_to_home_s;
-		_rtl_flight_time_pub.publish(rtl_flight_time);
-	}
 }
 
 void RTL::on_activation()
@@ -681,16 +672,21 @@ void RTL::get_rtl_xy_z_speed(float &xy, float &z)
 matrix::Vector2f RTL::get_wind()
 {
 	_wind_estimate_sub.update();
-	matrix::Vector2f wind(_wind_estimate_sub.get().windspeed_north, _wind_estimate_sub.get().windspeed_east);
+	matrix::Vector2f wind;
+
+	if (hrt_absolute_time() - _wind_estimate_sub.get().timestamp < 1_s) {
+		wind(0) = _wind_estimate_sub.get().windspeed_north;
+		wind(1) = _wind_estimate_sub.get().windspeed_east;
+	}
+
 	return wind;
 }
 
-float time_to_home(const matrix::Vector3f &vehicle_local_pos,
+float time_to_home(const matrix::Vector3f &to_home_vec,
-		   const matrix::Vector3f &rtl_point_local_pos,
 		   const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s)
 {
-	const matrix::Vector2f to_home = (rtl_point_local_pos - vehicle_local_pos).xy();
+	const matrix::Vector2f to_home = to_home_vec.xy();
-	const float alt_change = rtl_point_local_pos(2) - vehicle_local_pos(2);
+	const float alt_change = to_home_vec(2);
 	const matrix::Vector2f to_home_dir = to_home.unit_or_zero();
 	const float dist_to_home = to_home.norm();
 
@@ -706,6 +702,7 @@ float time_to_home(const matrix::Vector3f &vehicle_local_pos,
 	}
 
 	// 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;
+	float horiz = dist_to_home / cruise_speed;
-	return time_to_home;
+	float descent = fabsf(alt_change) / vehicle_descent_speed_m_s;
+	return horiz + descent;
 }

src/modules/navigator/rtl.h

@@ -168,12 +168,9 @@ private:
 	param_t _rtl_descent_speed;
 
 	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)};
+	uORB::Publication<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,
+float time_to_home(const matrix::Vector3f &to_home_vec,
-		   const matrix::Vector3f &rtl_point_local_pos,
 		   const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s,
 		   float vehicle_descent_speed_m_s);