fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:59:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Negative distances go in the opposite direction

Browse Source
This commit is contained in:
Julian Kent 2020-07-07 21:48:15 +02:00 committed by Daniel Agar
parent b7e49f20e4
commit 9ea3468091
2 changed files with 41 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
test/test_geo.cpp
View File

@ -95,3 +95,43 @@ TEST_F(GeoTest, projectReproject)
EXPECT_FLOAT_EQ(lat, lat_new);
EXPECT_FLOAT_EQ(lon, lon_new);
}
TEST_F(GeoTest, waypoint_from_heading_and_zero_distance)
{
// GIVEN: a starting waypoint, a heading and a distance of 0
double lat_start = -33;
double lon_start = 18;
float bearing = 0;
float dist = 0;
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_DOUBLE_EQ(lat_start, lat_target);
EXPECT_DOUBLE_EQ(lon_start, lon_target);
}
TEST_F(GeoTest, waypoint_from_heading_and_negative_distance)
{
// GIVEN: a starting waypoint, a heading and a distance of 0
double lat_start = -33;
double lon_start = 18;
float bearing = 0;
float lat_offset = -0.01f;
float dist = CONSTANTS_RADIUS_OF_EARTH * std::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);
}