fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

geo: Moved the map_projection_* functions and state into a self-contained C++ class

Browse Source
This commit is contained in:
Thomas Debrunner
2021-12-06 14:30:14 +01:00
committed by Matthias Grob
parent fe23718e2c
commit 8db7a6225b
38 changed files with 324 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
+8 -15
View File
@@ -52,17 +52,15 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
Vector2f waypoint_north_east_local(1.0, 1.0);
waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5;
Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5);
float x, y;
map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y);
Vector2f waypoint_north_east_reprojected(x, y);
Vector2f waypoint_north_east_reprojected = ref.project(waypoint_north_east_global(0), waypoint_north_east_global(1));
EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0));
EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1));
@@ -72,8 +70,7 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5);
map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y);
Vector2f waypoint_south_west_reprojected(x, y);
Vector2f waypoint_south_west_reprojected = ref.project(waypoint_southwest_global(0), waypoint_southwest_global(1));
EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0));
EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1));
@@ -88,9 +85,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.fence_violation = true;
@@ -136,9 +132,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{42.1, 8.2};
param_t param = param_handle(px4::params::MPC_ACC_HOR);
@@ -245,9 +240,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
@@ -278,9 +272,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;