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

GeofenceBreachAvoidanceTest: added tests for max dist to home

Browse Source 
Signed-off-by: Julian Kent <julian@auterion.com>
This commit is contained in:
RomanBapst
2020-07-10 09:59:12 +03:00
committed by Julian Kent
parent 3fa15cec91
commit f97dcde4e2
1 changed files with 66 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
+66 -4
View File
@@ -85,7 +85,7 @@ TEST(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
geofence_violation_type_u gf_violation; geofence_violation_type_u gf_violation;
gf_violation.flags.fence_violation = true; gf_violation.flags.fence_violation = true;
gf_avoidance.setTestPointDistance(20.0f); gf_avoidance.setHorizontalTestPointDistance(20.0f);
gf_avoidance.setTestPointBearing(0.0f); gf_avoidance.setTestPointBearing(0.0f);
gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0); gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0);
@@ -146,7 +146,7 @@ TEST(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
geofence_violation_type_u gf_violation; geofence_violation_type_u gf_violation;
gf_violation.flags.fence_violation = true; gf_violation.flags.fence_violation = true;
gf_avoidance.setTestPointDistance(30.0f); gf_avoidance.setHorizontalTestPointDistance(30.0f);
gf_avoidance.setTestPointBearing(0.0f); gf_avoidance.setTestPointBearing(0.0f);
gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0); gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0);
// vehicle is approaching the fence at a crazy velocity // vehicle is approaching the fence at a crazy velocity
@@ -158,7 +158,7 @@ TEST(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f,
20.0f - gf_avoidance.getMinDistToFenceMulticopter()); 20.0f - gf_avoidance.getMinHorDistToFenceMulticopter());
float error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0), float error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0),
loiter_point_predicted(1)); loiter_point_predicted(1));
@@ -215,7 +215,6 @@ TEST(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter)
geofence_violation_type_u gf_violation; geofence_violation_type_u gf_violation;
gf_violation.flags.max_altitude_exceeded = true; gf_violation.flags.max_altitude_exceeded = true;
gf_avoidance.setClimbRate(climbrate); gf_avoidance.setClimbRate(climbrate);
gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl);
gf_avoidance.computeVerticalBrakingDistanceMultirotor(); gf_avoidance.computeVerticalBrakingDistanceMultirotor();
@@ -231,3 +230,66 @@ TEST(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter)
EXPECT_EQ(loiter_alt_amsl, current_alt_amsl); EXPECT_EQ(loiter_alt_amsl, current_alt_amsl);
} }
TEST(GeofenceBreachAvoidance, maxDistToHomeViolationMulticopter)
{
GeofenceBreachAvoidance gf_avoidance;
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));
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
const float hor_vel = 8.0f;
const float test_point_distance = 30.0f;
const float test_point_bearing = 0.0f;
const float max_dist_to_home = 100.0f;
gf_avoidance.setHorizontalVelocity(hor_vel);
gf_avoidance.computeBrakingDistanceMultirotor();
gf_avoidance.setHorizontalTestPointDistance(test_point_distance);
gf_avoidance.setTestPointBearing(test_point_bearing);
Vector2d current_pos = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, 90.0f);
gf_avoidance.setCurrentPosition(current_pos(0), current_pos(1), 0);
gf_avoidance.setMaxHorDistHome(max_dist_to_home);
gf_avoidance.setHomePosition(home_global(0), home_global(1), 0);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing,
max_dist_to_home - gf_avoidance.getMinHorDistToFenceMulticopter());
EXPECT_EQ(loiter_point_predicted, loiter_point_lat_lon);
}
TEST(GeofenceBreachAvoidance, maxDistToHomeViolationFixedWing)
{
GeofenceBreachAvoidance gf_avoidance;
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));
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
const float test_point_distance = 30.0f;
const float max_dist_to_home = 100.0f;
const float test_point_bearing = 0.0f;
gf_avoidance.setHorizontalTestPointDistance(test_point_distance);
gf_avoidance.setTestPointBearing(test_point_bearing);
Vector2d current_pos = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, 90.0f);
gf_avoidance.setCurrentPosition(current_pos(0), current_pos(1), 0);
gf_avoidance.setMaxHorDistHome(max_dist_to_home);
gf_avoidance.setHomePosition(home_global(0), home_global(1), 0);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing,
max_dist_to_home - 2.0f * test_point_distance);
EXPECT_EQ(loiter_point_predicted, loiter_point_lat_lon);
}