fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 14:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Test the navigator side, and fix another trig edge case

Browse Source
This commit is contained in:
Julian Kent
2020-07-29 14:03:55 +02:00
committed by Lorenz Meier
parent a736ba2435
commit 100c64c790
3 changed files with 100 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/RangeRTLTest.cpp
+71 -1
View File
@@ -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;
}