mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
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;
|
||||
};
|
||||
|
||||
Reference in new issue
Block a user