Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 18:27:05 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

add tests for waypoint_from_line_and_distance()

Browse Source
This commit is contained in:
kamilritz
2020-08-10 19:20:23 +02:00
committed by Julian Kent
parent 60388a8b6d
commit 98dec6afa6
2 changed files with 84 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
geo/geo.cpp
-1
View File
@@ -288,7 +288,6 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
if (fabsf(dist) < FLT_EPSILON) { if (fabsf(dist) < FLT_EPSILON) {
*lat_target = lat_A; *lat_target = lat_A;
*lon_target = lon_A; *lon_target = lon_A;
} else { } else {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
test/test_geo.cpp
+84 -1
View File
@@ -118,7 +118,7 @@ TEST_F(GeoTest, waypoint_from_heading_and_zero_distance)
TEST_F(GeoTest, waypoint_from_heading_and_negative_distance) TEST_F(GeoTest, waypoint_from_heading_and_negative_distance)
{ {
// GIVEN: a starting waypoint, a heading and a distance of 0 // GIVEN: a starting waypoint, a heading and negative distance
double lat_start = -33; double lat_start = -33;
double lon_start = 18; double lon_start = 18;
float bearing = 0; float bearing = 0;
@@ -135,3 +135,86 @@ TEST_F(GeoTest, waypoint_from_heading_and_negative_distance)
EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target); EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target);
EXPECT_DOUBLE_EQ(lon_start, lon_target); EXPECT_DOUBLE_EQ(lon_start, lon_target);
} }
TEST_F(GeoTest, waypoint_from_heading_and_positive_distance)
{
// GIVEN: a starting waypoint, a heading and positive distance
double lat_start = -33;
double lon_start = 18;
float bearing = 0;
float lat_offset = 0.01f;
float dist = CONSTANTS_RADIUS_OF_EARTH * sin(M_PI/180) * lat_offset;
double lat_target = 0;
double lon_target = 0;
// WHEN: we get the next waypoint
waypoint_from_heading_and_distance(lat_start, lon_start, bearing, dist, &lat_target, &lon_target);
// THEN: it should be the same
EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target);
EXPECT_DOUBLE_EQ(lon_start, lon_target);
}
TEST_F(GeoTest, waypoint_from_line_and_zero_distance)
{
// GIVEN: a starting waypoint, a heading and a distance of 0
double lat_start = -33;
double lon_start = 18;
double lat_end = -32;
double lon_end = 18;
float dist = 0;
double lat_target = 0;
double lon_target = 0;
// WHEN: we get the next waypoint
create_waypoint_from_line_and_dist(lat_start, lon_start, lat_end, lon_end, dist, &lat_target, &lon_target);
// THEN: it should be the same
EXPECT_DOUBLE_EQ(lat_start, lat_target);
EXPECT_DOUBLE_EQ(lon_start, lon_target);
}
TEST_F(GeoTest, waypoint_from_line_and_positive_distance)
{
// GIVEN: a starting waypoint, a heading and a positive distance
double lat_start = -33;
double lon_start = 18;
double lat_offset = 0.01;
double lat_end = lat_start + lat_offset;
double lon_end = 18;
float dist = CONSTANTS_RADIUS_OF_EARTH * sin(M_PI/180) * lat_offset;
double lat_target = 0;
double lon_target = 0;
// WHEN: we get the next waypoint
create_waypoint_from_line_and_dist(lat_start, lon_start, lat_end, lon_end, dist, &lat_target, &lon_target);
// THEN: it should be the same
EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target);
EXPECT_DOUBLE_EQ(lon_start, lon_target);
}
TEST_F(GeoTest, waypoint_from_line_and_negative_distance)
{
// GIVEN: a starting waypoint, a heading and a negative distance
double lat_start = -33;
double lon_start = 18;
double lat_offset = 0.01;
double lat_end = lat_start + lat_offset;
double lon_end = 18;
float dist = -CONSTANTS_RADIUS_OF_EARTH * sin(M_PI/180) * lat_offset;
double lat_target = 0;
double lon_target = 0;
// WHEN: we get the next waypoint
create_waypoint_from_line_and_dist(lat_start, lon_start, lat_end, lon_end, dist, &lat_target, &lon_target);
// THEN: it should be the same
EXPECT_FLOAT_EQ(lat_start - lat_offset, lat_target);
EXPECT_DOUBLE_EQ(lon_start, lon_target);
}