forked from Archive/PX4-Autopilot
navigator: Remove deprecated RangeRTL unit tests
This commit is contained in:
Matthias Grob
parent
c522a8b15a
commit
ea3b99e964
|
|
@ -56,5 +56,3 @@ px4_add_module(
|
|||
geofence_breach_avoidance
|
||||
motion_planning
|
||||
)
|
||||
|
||||
px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman)
|
||||
|
|
|
|||
|
|
@ -1,219 +0,0 @@
|
|||
#define MODULE_NAME "Navigator"
|
||||
|
||||
#include "navigator.h"
|
||||
#include "rtl.h"
|
||||
|
||||
#include <future>
|
||||
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
TEST(Navigator_and_RTL, interact_correctly)
|
||||
{
|
||||
Navigator n;
|
||||
RTL rtl(&n);
|
||||
|
||||
|
||||
home_position_s home_pos{};
|
||||
home_pos.valid_hpos = true;
|
||||
home_pos.valid_lpos = true;
|
||||
home_pos.valid_alt = true;
|
||||
home_pos.timestamp = 1000;
|
||||
|
||||
vehicle_global_position_s glob_pos{};
|
||||
|
||||
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 Navigator's 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_vstatus() = v_status;
|
||||
|
||||
uORB::SubscriptionData<rtl_flight_time_s> _rtl_flight_time_sub{ORB_ID(rtl_flight_time)};
|
||||
ASSERT_FALSE(_rtl_flight_time_sub.update());
|
||||
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);
|
||||
|
||||
// WHEN: we set the vehicle type to multirotor
|
||||
v_status.vehicle_type = n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
float xy, z;
|
||||
rtl.get_rtl_xy_z_speed(xy, z);
|
||||
|
||||
// THEN: the RTL speed should correspond to multirotor parameters
|
||||
float xy_desired, z_desired;
|
||||
param_get(param_handle(px4::params::MPC_XY_CRUISE), &xy_desired);
|
||||
param_get(param_handle(px4::params::MPC_Z_VEL_MAX_DN), &z_desired);
|
||||
EXPECT_FLOAT_EQ(xy, xy_desired);
|
||||
EXPECT_FLOAT_EQ(z, z_desired);
|
||||
|
||||
// WHEN: it is a fixed wing
|
||||
n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
|
||||
rtl.get_rtl_xy_z_speed(xy, z);
|
||||
|
||||
// THEN: it should be fixed wing parameters
|
||||
param_get(param_handle(px4::params::FW_AIRSPD_TRIM), &xy_desired);
|
||||
param_get(param_handle(px4::params::FW_T_SINK_MIN), &z_desired);
|
||||
EXPECT_FLOAT_EQ(xy, xy_desired);
|
||||
EXPECT_FLOAT_EQ(z, z_desired);
|
||||
|
||||
// WHEN: it is rover
|
||||
n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
|
||||
rtl.get_rtl_xy_z_speed(xy, z);
|
||||
|
||||
// THEN: it should be rover parameters, and z should just be large (no RTL time in Z -> high speed)
|
||||
param_get(param_handle(px4::params::GND_SPEED_THR_SC), &xy_desired);
|
||||
EXPECT_FLOAT_EQ(xy, xy_desired);
|
||||
EXPECT_GT(z, 1000);
|
||||
}
|
||||
|
||||
class RangeRTL_tth : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
matrix::Vector3f rtl_vector;
|
||||
matrix::Vector3f rtl_point_local_pos;
|
||||
matrix::Vector2f wind_vel;
|
||||
float vehicle_speed;
|
||||
float vehicle_descent_speed;
|
||||
|
||||
void SetUp() override
|
||||
{
|
||||
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;
|
||||
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(rtl_vector, 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;
|
||||
rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2);
|
||||
|
||||
// WHEN: we get the tth
|
||||
float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed);
|
||||
|
||||
// THEN: it should be ten seconds
|
||||
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;
|
||||
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(rtl_vector, 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;
|
||||
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(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);
|
||||
}
|
||||
|
||||
TEST_F(RangeRTL_tth, ten_seconds_xy_upwind_to_home)
|
||||
{
|
||||
// GIVEN: 10 seconds of distance
|
||||
vehicle_speed = 4.2f;
|
||||
vehicle_descent_speed = 1.2f;
|
||||
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(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);
|
||||
}
|
||||
|
||||
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.;
|
||||
rtl_vector(0) = rtl_vector(1) = -(4 * 10) / sqrtf(2);
|
||||
|
||||
// WHEN: we get the tth
|
||||
float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed);
|
||||
|
||||
// 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;
|
||||
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(rtl_vector, 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;
|
||||
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(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed);
|
||||
|
||||
// THEN: it should never get home
|
||||
EXPECT_TRUE(std::isinf(tth)) << tth;
|
||||
}
|
||||
Loading…
Reference in New Issue