feat(navigator): check loiter circles against geofences (#28761)

* feat(navigator): check loiter circles against geofences

* fix(navigator): check the loiter after the last mission position

* fix(navigator): harden and speed up geofence path checks

Reuse path subdivision across bounded edge batches to reduce long-path
checking cost without growing Navigator's stack. Shift complete polygon
edges when checking loiter radii across the antimeridian.

Reload cached fence points when the storage bank or count changes, even
if the content ID is unchanged. Retain focused regressions for these
cases and remove duplicate malformed-cache coverage.

Assisted-by: Codex
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* refactor(navigator): drop the geofence edge batching

Batching polygon edges only saves work for legs longer than about 9 km,
which are split into pieces, and a fence holds at most 64 points. It cost
264 B of flash and 192 B of static RAM on fmu-v5x, plus a scratch buffer
shared by every Geofence instance. Check one edge at a time again; loiter
circles keep the whole-edge antimeridian shift.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* refactor(navigator): check loiter circles against Home in the path batch

The loiter item and the end-of-mission loiter each validated the radius,
checked the distance from Home and reported through their own failure.
Geofence::checkPaths already validates the radius, so it now also keeps
each circle within GF_MAX_HOR_DIST, and the mission side only adds
circles. The loiter message is only built with path checks, so boards
without them no longer carry its string, and isCloserThanMaxDistToHome
keeps its original signature.

An infinite radius now breaches every configured limit instead of making
the batch invalid. A NaN NAV_LOITER_RAD still rejects the mission. Loiter
test cases are followed by a waypoint, so the end-of-mission check no
longer hides the loiter item check.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* fix(navigator): skip the mission-end Hold check before a Return item

A mission that ends with a Return item switches to Return instead of
holding at its last position, so the mission-end Hold loiter is not flown
and must not reject the mission. Without Home, Return is refused and the
mission ends in Hold, so the check stays. Return may still loiter to
climb; this check does not cover that.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* fix(navigator): keep loiter circle checks conservative near the poles

The circle check used the longitude scale at the circle centre, which
overstates distances on the poleward side by up to 0.19 r^2 tan(lat) / R,
8 m for a 10 km circle at 70 deg, enough to miss a fence edge. Taking the
scale at the circle's poleward edge never overstates a distance within
the radius. The regression covers the 70 deg case.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* fix(navigator): keep the mission-end loiter check before a Return item

A fixed-wing below its return altitude starts Return by climbing on a
loiter around its position, so skipping the circle check before a Return
item let through missions that breach the fence during that climb. Check
the circle at the last position as for the mission-end Hold, with or
without Home.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* test(navigator): drop the Return mission-end case

Without the Return exemption, a trailing Return item is skipped like any
other command item, so the case repeated WaypointCirclesAtDefaultRadius.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

---------

Signed-off-by: jonas <jonas.perolini@rigi.tech>
Co-authored-by: jonas <jonas.perolini@rigi.tech>
This commit is contained in:
Jonas Peroliniandjonas authored and GitHub committed 2026-10-23 12:12:17 -08:00
1 parent dafea92a4f
commit eac044c917
6 files changed
+513 -52

No files matched your search

+45 -16
View File
@@ -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 &center = path.end;
const double radius = static_cast<double>(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<double>(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;
}
}
+6 -2
View File
@@ -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
@@ -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<uint16_t>(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<uint16_t>(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<unsigned>(waypoint));
events::send<uint16_t>(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<unsigned>(waypoint));
events::send<uint16_t>(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<unsigned>(waypoint));
events::send<uint16_t>(events::ID("navigator_mis_geofence_path_violation"),
{events::Log::Error, events::LogInternal::Info},
"Geofence breach on path to waypoint {1}", waypoint);
}
return false;
}
}
@@ -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();
@@ -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<LoiterFenceCase>
{
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<LoiterFenceCase> &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<InvalidRadiusCase> {};
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<InvalidRadiusCase> &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<Geofence::PathCheck, Geofence::MAX_PATH_CHECKS> paths;
std::array<bool, Geofence::MAX_PATH_CHECKS> 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<uint8_t *>(&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<InvalidFenceCase> {};
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.
@@ -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<MissionLoiterCase> {};
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<mission_item_s> 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<MissionLoiterCase> &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<bool> {};
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<mission_item_s> items(preceding_positions, waypoint({0.f, 800.f}));
items.push_back(loiter(950.f, 60.f));
EXPECT_FALSE(missionFeasible(items));
expectLoiterViolation(static_cast<int16_t>(items.size()));
EXPECT_TRUE(missionFeasible({loiter(950.f, 40.f)}));
}
INSTANTIATE_TEST_SUITE_P(MissionLoiterBatches, MissionGeofenceLoiterBatchTest, ::testing::Bool(),
[](const ::testing::TestParamInfo<bool> &test_info)
{
return test_info.param ? "CircleStartsNextBatch" : "CircleEndsFirstBatch";
});
class MissionGeofenceLoiterOrderTest : public MissionGeofenceLoiterTest,
public ::testing::WithParamInterface<bool> {};
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<bool> &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<MissionEndCase> {};
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<mission_item_s> 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<MissionEndCase> &test_info)
{
return test_info.param.name;
});