diff --git a/src/modules/navigator/mission_route_geofence.cpp b/src/modules/navigator/mission_route_geofence.cpp index 430907d4398..432217d2585 100644 --- a/src/modules/navigator/mission_route_geofence.cpp +++ b/src/modules/navigator/mission_route_geofence.cpp @@ -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); } diff --git a/src/modules/navigator/mission_route_geofence.h b/src/modules/navigator/mission_route_geofence.h index 684bf198020..43d770e1ced 100644 --- a/src/modules/navigator/mission_route_geofence.h +++ b/src/modules/navigator/mission_route_geofence.h @@ -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; };