refactor(navigator): name the planner's vehicle check after the geofence

vehicleMargin() returned a margin of 0 for a vehicle that is already
closer to the fence than GF_CONT_H_MARGIN. The helper is now
vehicleOutsideContGeofence(), and the caller says what follows from
it: the segments of such a vehicle are checked against the emergency
geofence only.

The class comment names the three cases by the two geofences, and the
comment on recovering() says what a recovery is and gives an example.

No behaviour change.

Assisted-by: Claude:claude-fable-5-2
Signed-off-by: jonas <jonspero@me.com>
This commit is contained in:
jonas committed 2026-11-20 17:46:35 +01:00
1 parent 66e93e0c39
commit ba8d70e0a2
2 files changed
+22 -16

No files matched your search

@@ -81,18 +81,20 @@ bool checkBatch(Geofence &geofence, PathBatch &batch)
} // namespace
// Polygons and circles only: a vehicle above the altitude limits or too far from Home does not recover, so its
// segments are still checked.
// The vehicle breaches a polygon or circle of the emergency geofence, for example 20 m outside an inclusion polygon.
// Every way back in crosses that boundary, so segments from the vehicle are accepted without a fence check.
// Too high, too far from Home or outside the contingency geofence is no recovery: those segments are checked.
bool GeofencePathChecker::recovering(const Position &vehicle) const
{
return !_geofence.isInsideHorizontalFences(vehicle.lat, vehicle.lon, vehicle.alt);
}
// The margin a segment from the vehicle keeps: GF_CONT_H_MARGIN, or 0 if the vehicle is already closer to the fence.
// Example: the Return that GF_CONT_ACTION starts inside the margin is checked against the fence itself.
float GeofencePathChecker::vehicleMargin(const Position &vehicle) const
// A vehicle that is already outside the contingency geofence cannot keep it: it is closer to the emergency geofence
// than GF_CONT_H_MARGIN. Its segments are checked against the emergency geofence only.
// Example: the Return that GF_CONT_ACTION starts there may fly back through the margin, but not across the fence.
bool GeofencePathChecker::vehicleOutsideContGeofence(const Position &vehicle) const
{
return _geofence.isInsideContGeofence(vehicle.lat, vehicle.lon) ? _geofence.getContGeofenceMargin() : 0.f;
return !_geofence.isInsideContGeofence(vehicle.lat, vehicle.lon);
}
bool GeofencePathChecker::pruneCandidates(ProjectionReferenceBatch &batch) const
@@ -135,7 +137,8 @@ bool GeofencePathChecker::pruneCandidates(ProjectionReferenceBatch &batch) const
}
// Segments keep GF_CONT_H_MARGIN from the polygons and circles, like the legs of a mission.
const float margin = is_vehicle ? vehicleMargin(anchor) : _geofence.getContGeofenceMargin();
const bool emergency_only = is_vehicle && vehicleOutsideContGeofence(anchor);
const float margin = emergency_only ? 0.f : _geofence.getContGeofenceMargin();
for (uint8_t j = 0; j < reference.candidate_buffer.count; ++j) {
const Position &projection = reference.candidate_buffer.candidates[j].projection;
@@ -195,7 +198,9 @@ bool GeofencePathChecker::checkPath(const Position &vehicle, const Position &end
return true;
}
const Geofence::PathCheck path{{vehicle.lat, vehicle.lon}, {end.lat, end.lon}, 0.f, vehicleMargin(vehicle)};
const bool emergency_only = vehicleOutsideContGeofence(vehicle);
const float margin = emergency_only ? 0.f : _geofence.getContGeofenceMargin();
const Geofence::PathCheck path{{vehicle.lat, vehicle.lon}, {end.lat, end.lon}, 0.f, margin};
return _geofence.checkPathBatch(&path, 1, &clear);
}
@@ -44,13 +44,14 @@ namespace mission_route
* Checks route segments against the polygons and circles of the loaded Navigator geofence.
*
* A segment that starts at the vehicle is checked by where the vehicle is:
* - Inside the fence and further from it than GF_CONT_H_MARGIN: the segment keeps the margin, like a mission leg.
* - Inside the fence but closer to it than the margin: the segment is checked against the fence itself, over its
* whole length. Example: the Return that GF_CONT_ACTION starts there may fly back through the margin, but not
* across the fence.
* - Outside the fence (outside an inclusion area or inside an exclusion area): the segment is not checked, so
* that the vehicle can fly back in. This is the recovery. It applies to every checkPath() segment.
* A segment that starts at a safe point always keeps the margin, also while the vehicle recovers.
* - Inside the contingency geofence: the segment keeps it, like a mission leg. It stays further from the fence
* than GF_CONT_H_MARGIN.
* - Outside the contingency geofence but inside the emergency geofence: the segment is checked against the
* emergency geofence only, over its whole length. Example: the Return that GF_CONT_ACTION starts there may fly
* back through the margin, but not across the emergency geofence.
* - Outside the emergency geofence (outside an inclusion area or inside an exclusion area): the segment is not
* checked, so that the vehicle can fly back in. This is the recovery. It applies to every checkPath() segment.
* A segment that starts at a safe point always keeps the contingency geofence, also while the vehicle recovers.
*
* The altitude and Home limits are not checked on segments, and a vehicle that only breaches these is not in
* recovery. Mission legs passed them in the mission check and the limits are convex, so points on the legs are
@@ -69,7 +70,7 @@ public:
private:
bool recovering(const Position &vehicle) const;
float vehicleMargin(const Position &vehicle) const;
bool vehicleOutsideContGeofence(const Position &vehicle) const;
Geofence &_geofence;
};