diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index c00c0cc9b9a..1fb71111ef1 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -100,6 +100,13 @@ static matrix::Vector3d unitVector(const matrix::Vector2d &lat_lon) return matrix::Vector3d(cos(lat) * cos(lon), cos(lat) * sin(lon), sin(lat)); } +// Whole turns that move an edge to its copy nearest reference_lon, e.g. an edge at -179.5 deg moves to 180.5 deg. +// Away from the antimeridian the shift is zero. +static double edgeLongitudeShift(const matrix::Vector2d &start, const matrix::Vector2d &end, double reference_lon) +{ + return 360.0 * round((0.5 * (start(1) + end(1)) - reference_lon) / 360.0); +} + // Compare a path and a fence edge in one continuous longitude range, so paths may cross the antimeridian. // Fence edges never cross it: the loader rejects them, since the point check could not handle them either. static bool pathTouchesEdge(const Geofence::PathCheck &path, unsigned pieces, const matrix::Vector2d &edge_start, @@ -112,11 +119,7 @@ static bool pathTouchesEdge(const Geofence::PathCheck &path, unsigned pieces, co path_end(1) += path_end(1) < path.start(1) ? 360.0 : -360.0; } - // Move the edge by whole turns to the copy nearest the path, e.g. an edge at -179.5 deg moves to 180.5 deg. - // Away from the antimeridian the shift is zero. - const double path_middle = 0.5 * (path.start(1) + path_end(1)); - const double edge_middle = 0.5 * (edge_start(1) + edge_end(1)); - const double shift = 360.0 * round((edge_middle - path_middle) / 360.0); + const double shift = edgeLongitudeShift(edge_start, edge_end, 0.5 * (path.start(1) + path_end(1))); const matrix::Vector2d shifted_start{edge_start(0), edge_start(1) - shift}; const matrix::Vector2d shifted_end{edge_end(0), edge_end(1) - shift}; @@ -163,6 +166,23 @@ static bool pathTouchesEdge(const Geofence::PathCheck &path, unsigned pieces, co return false; } +// Boundary contact counts, so the circle at the path end touches the edge when the distance is at most its radius. +static bool circleTouchesEdge(const Geofence::PathCheck &path, const matrix::Vector2d &edge_start, + const matrix::Vector2d &edge_end) +{ + const matrix::Vector2d ¢er = path.end; + const double radius = static_cast(path.end_radius); + // Fixed metre-per-degree scales keep the edge straight in a frame centred on the circle. The longitude + // scale at the circle's poleward edge never overstates a distance within the radius. + const double north_scale = math::radians(1.0) * CONSTANTS_RADIUS_OF_EARTH; + const double east_scale = north_scale * cos(math::min(math::radians(fabs(center(0))) + radius / CONSTANTS_RADIUS_OF_EARTH, + kHalfPi)); + const double shift = edgeLongitudeShift(edge_start, edge_end, center(1)); + const matrix::Vector2d a{(edge_start(0) - center(0)) *north_scale, (edge_start(1) - shift - center(1)) *east_scale}; + const matrix::Vector2d b{(edge_end(0) - center(0)) *north_scale, (edge_end(1) - shift - center(1)) *east_scale}; + return geofence_utils::pointToSegmentDistanceSquared(matrix::Vector2d {}, a, b) <= math::sq(radius); +} + static uint32_t crc32_for_fence_point(const mission_fence_point_s &fence_point, uint32_t prev_crc32) { union { @@ -236,17 +256,16 @@ void Geofence::run() if (_dataman_client.lastOperationCompleted(success)) { - if (success) { - // A failed response also overwrites the read buffer, so adopt it only now. - _stats = _stats_read; - } - if (!success) { _error_state = DatamanState::ReadWait; _dataman_state = DatamanState::Error; - } else if (_opaque_id != _stats.opaque_id || !_fence_loaded) { + } else if (!_fence_loaded || _opaque_id != _stats_read.opaque_id + || _stats.dataman_id != _stats_read.dataman_id || _stats.num_items != _stats_read.num_items) { + // A failed response also overwrites the read buffer. Adopt it only on success, + // and reload when the cache's storage bank changes even if the fence ID does not. + _stats = _stats_read; _opaque_id = _stats.opaque_id; _fence_loaded = false; @@ -654,18 +673,23 @@ bool Geofence::checkPaths(const PathCheck *paths, size_t num_paths, bool *result { uint8_t pieces[MAX_PATH_CHECKS]; bool pieces_calculated = false; + const float max_home_distance = _navigator->home_global_position_valid() ? _param_gf_max_hor_dist.get() : 0.f; + const home_position_s &home = *_navigator->get_home_position(); for (size_t i = 0; i < num_paths; ++i) { const auto &start = paths[i].start; const auto &end = paths[i].end; - if (!start.isAllFinite() || !end.isAllFinite() + // An infinite radius is valid and breaches every configured limit. + if (!start.isAllFinite() || !end.isAllFinite() || !(paths[i].end_radius >= 0.f) || fabs(start(0)) > 90.0 || fabs(end(0)) > 90.0 || fabs(start(1)) > 180.0 || fabs(end(1)) > 180.0) { return false; } - results[i] = true; + // The caller checks path ends against Home; a circle must keep its whole radius inside. + results[i] = paths[i].end_radius <= 0.f || max_home_distance <= FLT_EPSILON + || get_distance_to_next_waypoint(home.lat, home.lon, end(0), end(1)) + paths[i].end_radius < max_home_distance; } for (int p = 0; p < _num_polygons; ++p) { @@ -748,7 +772,8 @@ bool Geofence::checkPolygonPaths(const PolygonInfo &polygon, const PathCheck *pa } for (size_t i = 0; i < num_paths; ++i) { - if (results[i] && pathTouchesEdge(paths[i], pieces[i], previous, next)) { + if (results[i] && (pathTouchesEdge(paths[i], pieces[i], previous, next) + || (paths[i].end_radius > 0.f && circleTouchesEdge(paths[i], previous, next)))) { results[i] = false; } } @@ -796,10 +821,13 @@ bool Geofence::checkCirclePaths(const PolygonInfo &polygon, const PathCheck *pat // Match insideCircle() at each endpoint. const bool start_inside = (start - center).norm_squared() < point_radius_squared; const bool end_inside = (end - center).norm_squared() < point_radius_squared; + const double end_radius = static_cast(paths[i].end_radius); if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { // The double checks reject boundary contact rounded inside by the float point check. - results[i] = start_inside && end_inside && a.norm_squared() < radius_squared && b.norm_squared() < radius_squared; + const double remaining_radius = radius - end_radius; + results[i] = start_inside && end_inside && a.norm_squared() < radius_squared + && remaining_radius > 0.0 && b.norm_squared() < remaining_radius * remaining_radius; } else if (start_inside || end_inside) { results[i] = false; @@ -807,7 +835,8 @@ bool Geofence::checkCirclePaths(const PolygonInfo &polygon, const PathCheck *pat } else { const double distance_squared = geofence_utils::pointToSegmentDistanceSquared( matrix::Vector2d(center), matrix::Vector2d(start), matrix::Vector2d(end)); - results[i] = distance_squared > radius_squared; + const double clearance = radius + end_radius; + results[i] = distance_squared > radius_squared && b.norm_squared() > clearance * clearance; } } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index d0720a334e7..01f2540c43e 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -103,13 +103,17 @@ public: struct PathCheck { matrix::Vector2d start; ///< latitude, longitude in degrees matrix::Vector2d end; + float end_radius{0.f}; ///< Circle radius at end in metres; zero checks only the path. }; /** - * Check 1..MAX_PATH_CHECKS paths against horizontal fences. Boundary contact is a breach. + * Check 1..MAX_PATH_CHECKS paths and optional circles at their ends against horizontal fences. + * Circles are also checked against the maximum distance from Home. + * Boundary contact is a breach, including contact by a circle. Callers choose the circles; + * a fixed-wing waypoint turned into a loiter to reach its altitude cannot be predicted here. * At least one endpoint of each path must be valid under the existing point check. * For a connected chain, checking the first point is enough if all preceding paths pass. - * Check anchors and paths against the same loaded fence; check Home and altitude limits separately. + * Check anchors and paths against the same loaded fence; check positions against Home and altitude limits separately. * Uses cached fence data only. Paths may cross the antimeridian. * Polygon checks approximate great-circle paths with a 2 m subdivision target; this is not a clearance margin. * Great circles approximate local guidance near the EKF origin. Paths that could reach a pole or exceed diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 20e28fcce15..967e78246a2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -130,11 +130,18 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, enum class Failure { None, DatamanRead, NoHome, Waypoint } failure = Failure::None; size_t failed_item = 0; #if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + const vehicle_status_s &status = *_navigator->get_vstatus(); + // A VTOL may transition to fixed-wing after upload. + const bool circling_vehicle = status.is_vtol || status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + /* Check mission positions and the paths between them. */ static GeofencePathBatch batch{}; // keep the shared batch off the stack. batch.count = 0; matrix::Vector2d previous_position{}; bool have_previous_position = false; + // Radius of the loiter flown at the last position if the mission ends there, see setEndOfMissionItems(). + float end_loiter_radius = 0.f; + size_t end_loiter_item = 0; #endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS for (size_t i = 0; i < mission.count; i++) { @@ -203,19 +210,44 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, have_previous_position = true; } - batch.paths[batch.count] = {previous_position, position}; - batch.mission_indices[batch.count++] = static_cast(i); - previous_position = position; - - if (batch.count == kGeofencePathBatchSize && !checkGeofencePathBatch(batch)) { + if (!addGeofencePath(batch, {previous_position, position}, i)) { return false; } + previous_position = position; + end_loiter_radius = 0.f; + + if (!circling_vehicle || missionitem.nav_cmd == NAV_CMD_LAND || missionitem.nav_cmd == NAV_CMD_VTOL_LAND) { + continue; + } + + // Same radius as mission_item_to_position_setpoint(). + const float radius = fabsf(missionitem.loiter_radius) > FLT_EPSILON ? fabsf(missionitem.loiter_radius) : + _navigator->get_default_loiter_rad(); + + if (missionitem.nav_cmd == NAV_CMD_LOITER_UNLIMITED || missionitem.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT + || missionitem.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + // Keep the circle after its incoming leg so the first breach is reported in mission order. + if (!addGeofencePath(batch, {position, position, radius}, i)) { + return false; + } + + } else { + end_loiter_radius = radius; + end_loiter_item = i; + } + #endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS } #if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + // A NaN radius stays pending so the path check rejects it. + if (failure == Failure::None && !(end_loiter_radius <= 0.f) + && !addGeofencePath(batch, {previous_position, previous_position, end_loiter_radius}, end_loiter_item)) { + return false; + } + // Report an earlier buffered path breach before the current item's failure. if (!checkGeofencePathBatch(batch)) { return false; @@ -248,6 +280,14 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, } #if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) +bool MissionFeasibilityChecker::addGeofencePath(GeofencePathBatch &batch, const Geofence::PathCheck &path, + size_t mission_index) +{ + batch.paths[batch.count] = path; + batch.mission_indices[batch.count++] = static_cast(mission_index); + return batch.count < kGeofencePathBatchSize || checkGeofencePathBatch(batch); +} + bool MissionFeasibilityChecker::checkGeofencePathBatch(GeofencePathBatch &batch) { if (batch.count == 0) { @@ -262,11 +302,22 @@ bool MissionFeasibilityChecker::checkGeofencePathBatch(GeofencePathBatch &batch) for (size_t i = 0; i < batch.count; ++i) { if (!batch.results[i]) { const uint16_t waypoint = batch.mission_indices[i] + 1; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence breach on path to waypoint %u\t", - static_cast(waypoint)); - events::send(events::ID("navigator_mis_geofence_path_violation"), - {events::Log::Error, events::LogInternal::Info}, - "Geofence breach on path to waypoint {1}", waypoint); + + if (batch.paths[i].end_radius > 0.f) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence breach by loiter circle of waypoint %u\t", + static_cast(waypoint)); + events::send(events::ID("navigator_mis_geofence_loiter_breach"), + {events::Log::Error, events::LogInternal::Info}, + "Geofence breach by loiter circle of waypoint {1}", waypoint); + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence breach on path to waypoint %u\t", + static_cast(waypoint)); + events::send(events::ID("navigator_mis_geofence_path_violation"), + {events::Log::Error, events::LogInternal::Info}, + "Geofence breach on path to waypoint {1}", waypoint); + } + return false; } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index ea82cf9f04a..9093aba7770 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -69,6 +69,7 @@ private: bool checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid); #if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + bool addGeofencePath(GeofencePathBatch &batch, const Geofence::PathCheck &path, size_t mission_index); bool checkGeofencePathBatch(GeofencePathBatch &batch); #endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS void logGeofenceUnavailable(); diff --git a/src/modules/navigator/test/test_geofence_paths.cpp b/src/modules/navigator/test/test_geofence_paths.cpp index c3dd599fa73..2e3c61ea93d 100644 --- a/src/modules/navigator/test/test_geofence_paths.cpp +++ b/src/modules/navigator/test/test_geofence_paths.cpp @@ -175,13 +175,108 @@ INSTANTIATE_TEST_SUITE_P(Paths, GeofencePathTest, ::testing::Values( return test_info.param.name; }); +struct LoiterFenceCase { + const char *name; + bool circle_fence; + bool inclusion; +}; + +class GeofenceLoiterTest : public navigator_test::GeofenceTestBase, + public ::testing::WithParamInterface +{ +public: + GeofenceLoiterTest() : GeofenceTestBase(0.0, 0.0) {} + +protected: + void SetUp() override { ASSERT_TRUE(resetFence()); } +}; + +TEST_P(GeofenceLoiterTest, ChecksCircleClearanceInMixedBatch) +{ + const LoiterFenceCase &test = GetParam(); + FencePoints points; + + // Each fence has its nearest boundary 75 m from the circle centre at Home. + if (test.circle_fence) { + points = circle(test.inclusion, {0.f, test.inclusion ? -25.f : 100.f}, test.inclusion ? 100.f : 25.f); + + } else if (test.inclusion) { + points = polygon(true, {{-75.f, -75.f}, {75.f, -75.f}, {75.f, 75.f}, {-75.f, 75.f}}); + + } else { + points = polygon(false, {{-25.f, 75.f}, {25.f, 75.f}, {25.f, 125.f}, {-25.f, 125.f}}); + } + + ASSERT_TRUE(loadFence(points)); + const auto center = position(0.f, 0.f); + // Check a point, a clear circle, boundary contact, a circle that encloses the entire exclusion and an infinite one. + const Geofence::PathCheck paths[] {{center, center}, {center, center, 50.f}, {center, center, 75.f}, {center, center, 150.f}, + {center, center, INFINITY} + }; + bool clear[5] {}; + ASSERT_TRUE(_fence.checkPathBatch(paths, 5, clear)); + EXPECT_TRUE(clear[0]); + EXPECT_TRUE(clear[1]); + EXPECT_FALSE(clear[2]); + EXPECT_FALSE(clear[3]); + EXPECT_FALSE(clear[4]); +} + +INSTANTIATE_TEST_SUITE_P(LoiterFences, GeofenceLoiterTest, ::testing::Values( + LoiterFenceCase{"InclusionPolygon", false, true}, + LoiterFenceCase{"ExclusionPolygon", false, false}, + LoiterFenceCase{"InclusionCircle", true, true}, + LoiterFenceCase{"ExclusionCircle", true, false}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + +TEST_F(GeofenceTest, CircleClearanceUsesMetresAtItsLatitude) +{ + // At 47 degrees latitude, the short east/west clearance needs longitude scaling. + ASSERT_TRUE(loadFence(polygon(true, {{-1000.f, -100.f}, {1000.f, -100.f}, {1000.f, 100.f}, {-1000.f, 100.f}}))); + const auto center = position(0.f, 0.f); + const Geofence::PathCheck paths[] {{center, center, 90.f}, {center, center, 110.f}}; + bool clear[2] {}; + ASSERT_TRUE(_fence.checkPathBatch(paths, 2, clear)); + EXPECT_TRUE(clear[0]); + EXPECT_FALSE(clear[1]); +} + +struct InvalidRadiusCase { + const char *name; + float radius; +}; + +class InvalidGeofenceRadiusTest : public GeofenceTest, public ::testing::WithParamInterface {}; + +TEST_P(InvalidGeofenceRadiusTest, RejectsWholeBatch) +{ + ASSERT_TRUE(loadFence({})); + Geofence::PathCheck paths[] {path({0.f, 0.f}, {100.f, 0.f}), path({100.f, 0.f}, {100.f, 0.f})}; + paths[1].end_radius = GetParam().radius; + bool clear[2] {true, true}; + EXPECT_FALSE(_fence.checkPathBatch(paths, 2, clear)); + EXPECT_FALSE(clear[0]); + EXPECT_FALSE(clear[1]); +} + +INSTANTIATE_TEST_SUITE_P(InvalidRadii, InvalidGeofenceRadiusTest, ::testing::Values( + InvalidRadiusCase{"Negative", -1.f}, + InvalidRadiusCase{"NaN", NAN}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + TEST_F(GeofenceTest, FullBatchMatchesIndividualChecks) { - ASSERT_TRUE(loadFence(exclusionSquare())); + ASSERT_TRUE(loadFence(exclusionRing(DM_KEY_FENCE_POINTS_MAX))); std::array paths; std::array clear{}; - // Alternate between crossing the square and passing 10 m north of it. + // Alternate between crossing the full-size polygon and passing 10 m north of it. for (size_t i = 0; i < paths.size(); ++i) { const float north = i % 2 == 0 ? 0.f : 60.f; paths[i] = path({north, 100.f}, {north, 500.f}); @@ -275,6 +370,17 @@ TEST_P(GeofenceFileTest, ImportedFenceAndSubsequentRefreshBecomeReady) ASSERT_TRUE(_fence.checkPathBatch(paths, 2, clear)); EXPECT_TRUE(clear[0]); EXPECT_FALSE(clear[1]); + + // Importing identical contents preserves the fence ID but switches the storage bank. + ASSERT_EQ(_fence.loadFromFile(_filename), PX4_OK); + ASSERT_TRUE(_dataman_client.readSync(DM_KEY_FENCE_POINTS_STATE, 0, + reinterpret_cast(&stats), sizeof(stats))); + EXPECT_EQ(stats.opaque_id, _fence_id); + EXPECT_EQ(stats.dataman_id, GetParam()); + ASSERT_TRUE(waitForFence()); + ASSERT_TRUE(_fence.checkPathBatch(paths, 2, clear)); + EXPECT_TRUE(clear[0]); + EXPECT_FALSE(clear[1]); } INSTANTIATE_TEST_SUITE_P(StoredBanks, GeofenceFileTest, @@ -413,29 +519,10 @@ struct InvalidFenceCase { class InvalidGeofenceBatchTest : public GeofenceTest, public ::testing::WithParamInterface {}; -TEST_P(InvalidGeofenceBatchTest, RejectsWholeBatch) +TEST_F(GeofenceTest, PolygonVertexCountExceedingCacheRejectsBatch) { - FencePoints points = exclusionSquare(); - ASSERT_TRUE(loadFence(points)); - - if (GetParam().fault == InvalidFenceData::VertexCount) { - GeofenceTestPeer::setVertexCount(_fence, 5); - - } else { - mission_fence_point_s &point = points.back(); - - if (GetParam().fault == InvalidFenceData::Longitude) { - point.lon = NAN; - - } else if (GetParam().fault == InvalidFenceData::Frame) { - point.frame = NAV_FRAME_LOCAL_NED; - - } else { - point.nav_cmd = NAV_CMD_WAYPOINT; - } - - ASSERT_TRUE(GeofenceTestPeer::replaceCachedPoint(_fence, points.size() - 1, point)); - } + ASSERT_TRUE(loadFence(exclusionSquare())); + GeofenceTestPeer::setVertexCount(_fence, 5); const Geofence::PathCheck paths[] {path({100.f, 100.f}, {100.f, 500.f}), path({0.f, 100.f}, {0.f, 500.f})}; bool clear[2] {true, true}; @@ -868,6 +955,61 @@ TEST_F(AntimeridianGeofenceTest, ShortPathsMayCrossTheAntimeridian) } } +TEST_F(AntimeridianGeofenceTest, LoiterRadiusCrossesTheAntimeridian) +{ + for (double side : {-1.0, 1.0}) { + SCOPED_TRACE(side); + const matrix::Vector2d center{47.0, side * 179.999}; + const Geofence::PathCheck paths[] {{center, center, 90.f}, {center, center, 120.f}}; + // The nearest edge across the antimeridian is about 99 m from the centre. + ASSERT_TRUE(loadFence(exclusionAt(47.0, -side * 179.9995))); + ASSERT_TRUE(_fence.checkPointAgainstAllGeofences(center(0), center(1), 500.f)); + bool clear[2] {}; + ASSERT_TRUE(_fence.checkPathBatch(paths, 2, clear)); + EXPECT_TRUE(clear[0]); + EXPECT_FALSE(clear[1]); + + // Shift both edge vertices together: independently wrapping them can turn this + // distant edge across the centre's antipodal longitude into a nearby edge. + ASSERT_TRUE(loadFence(exclusionAt(47.0, -side * 0.001))); + ASSERT_TRUE(_fence.checkPathBatch(paths, 2, clear)); + EXPECT_TRUE(clear[0]); + EXPECT_TRUE(clear[1]); + } +} + +class HighLatitudeGeofenceTest : public GeofenceTest +{ +protected: + HighLatitudeGeofenceTest() : GeofenceTest(70.0, 10.0) {} +}; + +TEST_F(HighLatitudeGeofenceTest, LargeCircleTouchesThePolewardEdge) +{ + // The first edge is 9,998 m north-east of Home. A longitude scale fixed at 70 deg reads 10,006 m there. + const double vertices[3][2] {{70.02985, 10.26065}, {70.07391, 10.16934}, {70.12, 10.4}}; + FencePoints points; + + for (const auto &vertex : vertices) { + mission_fence_point_s point{}; + point.nav_cmd = NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION; + point.frame = NAV_FRAME_GLOBAL; + point.lat = vertex[0]; + point.lon = vertex[1]; + point.vertex_count = 3; + points.push_back(point); + } + + ASSERT_TRUE(loadFence(points)); + const matrix::Vector2d center{70.0, 10.0}; + ASSERT_TRUE(_fence.checkPointAgainstAllGeofences(center(0), center(1), 500.f)); + const Geofence::PathCheck paths[] {{center, center, 9900.f}, {center, center, 10000.f}}; + bool clear[2] {}; + ASSERT_TRUE(_fence.checkPathBatch(paths, 2, clear)); + EXPECT_TRUE(clear[0]); + EXPECT_FALSE(clear[1]); +} + TEST_F(AntimeridianGeofenceTest, MultiPiecePathsMayCrossTheAntimeridian) { // This 30 km leg needs several pieces and bows about 19 m north of the straight lat/lon line. diff --git a/src/modules/navigator/test/test_mission_feasibility_geofence.cpp b/src/modules/navigator/test/test_mission_feasibility_geofence.cpp index 5e92bdcf259..e5f62c94032 100644 --- a/src/modules/navigator/test/test_mission_feasibility_geofence.cpp +++ b/src/modules/navigator/test/test_mission_feasibility_geofence.cpp @@ -433,3 +433,237 @@ TEST_F(MissionFeasibilityGeofenceTest, UnreadableMissionStorageRejectsUpload) EXPECT_TRUE(logContains("dataman read failed at item 0")); EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_dm_read_fail"), 0)); } + +class MissionGeofenceLoiterTest : public MissionFeasibilityGeofenceTest +{ +protected: + bool configureVehicle(uint8_t type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING, bool is_vtol = false) + { + // These missions exercise fences without a takeoff or landing sequence. + const int32_t takeoff_land_requirement = 0; + + if (param_set(param_find("MIS_TKO_LAND_REQ"), &takeoff_land_requirement) != PX4_OK) { + return false; + } + + vehicle_status_s status = *_navigator.get_vstatus(); + status.vehicle_type = type; + status.is_vtol = is_vtol; + *_navigator.get_vstatus() = status; + _status_pub.publish(status); + return true; + } + + mission_item_s loiter(float east, float radius, uint16_t command = NAV_CMD_LOITER_UNLIMITED) const + { + mission_item_s item = waypoint({0.f, east}); + item.nav_cmd = command; + item.loiter_radius = radius; + return item; + } + + void expectLoiterViolation(int16_t waypoint_index) + { + const std::string message = "Geofence breach by loiter circle of waypoint " + std::to_string(waypoint_index); + EXPECT_TRUE(logContains(message.c_str())); + EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_loiter_breach"), waypoint_index)); + } +}; + +struct MissionLoiterCase { + const char *name; + uint16_t command; + float radius; + bool feasible; + uint8_t vehicle_type{vehicle_status_s::VEHICLE_TYPE_FIXED_WING}; + bool is_vtol{false}; + float default_radius{40.f}; + bool followed_by_waypoint{false}; +}; + +class MissionGeofenceLoiterRadiusTest : public MissionGeofenceLoiterTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceLoiterRadiusTest, ChecksRadiusForCirclingVehicles) +{ + const MissionLoiterCase &test = GetParam(); + ASSERT_TRUE(configureVehicle(test.vehicle_type, test.is_vtol)); + ASSERT_EQ(param_set(param_find("NAV_LOITER_RAD"), &test.default_radius), PX4_OK); + _navigator.updateParams(); + ASSERT_TRUE(loadFence(inclusionSquare())); + + // The centre is 50 m inside the eastern edge: 40 m fits, 60 m crosses it. + std::vector items{waypoint({0.f, 800.f}), changeSpeed(), loiter(950.f, test.radius, test.command)}; + + if (test.followed_by_waypoint) { + items.push_back(waypoint({0.f, 800.f})); + } + + EXPECT_EQ(missionFeasible(items), test.feasible); + + if (!test.feasible) { + expectLoiterViolation(3); + } +} + +INSTANTIATE_TEST_SUITE_P(MissionLoiterRadii, MissionGeofenceLoiterRadiusTest, ::testing::Values( + MissionLoiterCase{"CircleFits", NAV_CMD_LOITER_UNLIMITED, 40.f, true}, + MissionLoiterCase{"UnlimitedLoiterCrosses", NAV_CMD_LOITER_UNLIMITED, 60.f, false}, + MissionLoiterCase{"TimedLoiterNegativeRadius", NAV_CMD_LOITER_TIME_LIMIT, -60.f, false, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false, 40.f, true}, + MissionLoiterCase{"LoiterToAltitudeCrosses", NAV_CMD_LOITER_TO_ALT, 60.f, false, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false, 40.f, true}, + MissionLoiterCase{"ZeroUsesConfiguredDefault", NAV_CMD_LOITER_UNLIMITED, 0.f, true}, + MissionLoiterCase{"ZeroDefaultCrosses", NAV_CMD_LOITER_UNLIMITED, 0.f, false, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false, 60.f}, + MissionLoiterCase{"NaNUsesDefault", NAV_CMD_LOITER_UNLIMITED, NAN, false, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false, 60.f}, + MissionLoiterCase{"InfiniteRadiusRejected", NAV_CMD_LOITER_UNLIMITED, INFINITY, false}, + MissionLoiterCase{"MulticopterHovers", NAV_CMD_LOITER_UNLIMITED, 60.f, true, + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING}, + MissionLoiterCase{"VtolMayTransition", NAV_CMD_LOITER_UNLIMITED, 60.f, false, + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, true}, + MissionLoiterCase{"WaypointIgnoresRadius", NAV_CMD_WAYPOINT, 60.f, true, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false, 40.f, true}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + +TEST_F(MissionGeofenceLoiterTest, EmptyFenceStillChecksCircleDistanceFromHome) +{ + ASSERT_TRUE(configureVehicle()); + const float maximum = 300.f; + ASSERT_EQ(param_set(param_find("GF_MAX_HOR_DIST"), &maximum), PX4_OK); + _navigator.updateParams(); + + // Both centres pass; only the 60 m circle reaches beyond the Home limit. + EXPECT_TRUE(missionFeasible({loiter(250.f, 40.f)})); + EXPECT_FALSE(missionFeasible({loiter(250.f, 60.f)})); + expectLoiterViolation(1); +} + +TEST_F(MissionGeofenceLoiterTest, NaNDefaultRadiusRejectsTheEndLoiter) +{ + ASSERT_TRUE(configureVehicle()); + const float radius = NAN; + ASSERT_EQ(param_set(param_find("NAV_LOITER_RAD"), &radius), PX4_OK); + _navigator.updateParams(); + ASSERT_TRUE(loadFence(inclusionSquare())); + + // The mission ends in a loiter whose circle cannot be checked. + EXPECT_FALSE(missionFeasible({waypoint({0.f, -500.f}), waypoint({0.f, 0.f})})); + EXPECT_TRUE(logContains("geofence path check unavailable")); +} + +class MissionGeofenceLoiterBatchTest : public MissionGeofenceLoiterTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceLoiterBatchTest, ChecksCircleAtBatchBoundaryAndResetsForNextMission) +{ + ASSERT_TRUE(configureVehicle()); + ASSERT_TRUE(loadFence(inclusionSquare())); + // The circle either fills the first batch or follows an incoming leg that fills it. + const size_t preceding_positions = Geofence::MAX_PATH_CHECKS - (GetParam() ? 1 : 2); + std::vector items(preceding_positions, waypoint({0.f, 800.f})); + items.push_back(loiter(950.f, 60.f)); + EXPECT_FALSE(missionFeasible(items)); + expectLoiterViolation(static_cast(items.size())); + EXPECT_TRUE(missionFeasible({loiter(950.f, 40.f)})); +} + +INSTANTIATE_TEST_SUITE_P(MissionLoiterBatches, MissionGeofenceLoiterBatchTest, ::testing::Bool(), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param ? "CircleStartsNextBatch" : "CircleEndsFirstBatch"; +}); + +class MissionGeofenceLoiterOrderTest : public MissionGeofenceLoiterTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceLoiterOrderTest, ReportsFirstBreachInMissionOrder) +{ + ASSERT_TRUE(configureVehicle()); + ASSERT_TRUE(loadFence(exclusionSquare())); + + // The leg crosses the exclusion. A circle on either end also overlaps it. + if (GetParam()) { + EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f}), loiter(500.f, 160.f)})); + expectPathViolation(2); + + } else { + EXPECT_FALSE(missionFeasible({loiter(100.f, 160.f), waypoint({0.f, 500.f})})); + expectLoiterViolation(1); + } +} + +INSTANTIATE_TEST_SUITE_P(MissionLoiterOrder, MissionGeofenceLoiterOrderTest, ::testing::Bool(), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param ? "IncomingPathBeforeCircle" : "CircleBeforeFollowingPath"; +}); + +enum class MissionEnd { Waypoint, Land, Loiter }; + +struct MissionEndCase { + const char *name; + MissionEnd last_item; + float default_radius; + uint8_t vehicle_type; + bool feasible; +}; + +class MissionGeofenceEndLoiterTest : public MissionGeofenceLoiterTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceEndLoiterTest, ChecksTheLoiterAfterTheLastPosition) +{ + const MissionEndCase &test = GetParam(); + ASSERT_TRUE(configureVehicle(test.vehicle_type)); + ASSERT_EQ(param_set(param_find("NAV_LOITER_RAD"), &test.default_radius), PX4_OK); + _navigator.updateParams(); + ASSERT_TRUE(loadFence(inclusionSquare())); + + // The mission ends 50 m inside the eastern edge, approached from far enough west for a landing. + std::vector items{waypoint({0.f, -500.f})}; + mission_item_s last = waypoint({0.f, 950.f}); + + switch (test.last_item) { + case MissionEnd::Waypoint: + break; + + case MissionEnd::Land: + last.nav_cmd = NAV_CMD_LAND; + last.altitude = kAltitude - 100.f; + break; + + case MissionEnd::Loiter: + last = loiter(950.f, 40.f); + break; + } + + items.push_back(last); + // End-of-mission loiter belongs to the last position, even with trailing command items. + items.push_back(changeSpeed()); + EXPECT_EQ(missionFeasible(items), test.feasible); + + if (!test.feasible) { + expectLoiterViolation(2); + } +} + +INSTANTIATE_TEST_SUITE_P(MissionEnds, MissionGeofenceEndLoiterTest, ::testing::Values( + MissionEndCase{"WaypointCirclesAtDefaultRadius", MissionEnd::Waypoint, 60.f, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false}, + MissionEndCase{"WaypointFitsWithSmallerDefault", MissionEnd::Waypoint, 40.f, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, true}, + MissionEndCase{"LandingDoesNotLoiter", MissionEnd::Land, 60.f, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, true}, + MissionEndCase{"ExplicitLoiterUsesItsOwnRadius", MissionEnd::Loiter, 60.f, + vehicle_status_s::VEHICLE_TYPE_FIXED_WING, true}, + MissionEndCase{"MulticopterHoversAtTheEnd", MissionEnd::Waypoint, 60.f, + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, true}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +});