diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 15c8ee97a6..ef1198902d 100644 --- a/src/modules/commander/Commander.cpp +++ b/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() ? - _rtl_flight_time_sub.get().rtl_limit_fraction : 0; + float battery_usage_to_home = 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) { - float battery_at_home = battery_level_fraction - battery_to_home; + uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE; + + 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; diff --git a/src/modules/navigator/RangeRTLTest.cpp b/src/modules/navigator/RangeRTLTest.cpp index 73eb0683c4..3267ac68e4 100644 --- a/src/modules/navigator/RangeRTLTest.cpp +++ b/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_point_local_pos ; + matrix::Vector3f rtl_vector; + 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); - vehicle_local_pos(2) = vehicle_descent_speed * 5; + 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(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; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index efd34b206a..4112530be6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/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 local_pos(local_pos_s.x, local_pos_s.y, local_pos_s.z); + matrix::Vector3f to_destination_vec; + 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)) { - 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)); - - 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); - } + 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, - const matrix::Vector3f &rtl_point_local_pos, +float time_to_home(const matrix::Vector3f &to_home_vec, 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 float alt_change = rtl_point_local_pos(2) - vehicle_local_pos(2); + const matrix::Vector2f to_home = to_home_vec.xy(); + 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; - return time_to_home; + float horiz = dist_to_home / cruise_speed; + float descent = fabsf(alt_change) / vehicle_descent_speed_m_s; + return horiz + descent; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index ecb04cab48..4e943dfbdd 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -168,12 +168,9 @@ private: param_t _rtl_descent_speed; 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] + uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; }; -float time_to_home(const matrix::Vector3f &vehicle_local_pos, - const matrix::Vector3f &rtl_point_local_pos, +float time_to_home(const matrix::Vector3f &to_home_vec, const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s);