PX4-Autopilot
PX4-Autopilot copied to clipboard
PX4
geofence outside buffer zone
Describe problem solved by this pull request
This PR builds on #20055 and switches to an outer geofence (the secondary geofence lies outside the primary). It also makes GF_ACTION the primary geofence enabling param, allowing it to work without a GF_BUFFER_ACTION defined (in the previous PR both must be defined for either to work), and fully disabling both fences by disabling GF_ACTION. It also includes logic to avoid sneaking out of a secondary geofence polygon's corners that was possible using the orignal distance to line calculation.
Describe your solution
This is the same solution as #20055 but switches the order, so that the original geofence behaves as normal, and the buffer distance creates an outer fence that can be triggered after an initial geofence breech has occurred.
Describe possible alternatives
The previous PR #20055 implemented the fence as an inner fence. This uses the same approach to create an outer fence.
Test data / coverage
This has been tested in SITL using QGC to manually fly a drone out of a geofence in two scenarios:
- Primary geofence set to WARN and secondary set to LAND. This allowed testing concave geofence polygons ensuring that the secondary geofence could be triggered even after recovering from the primary.
- Primary geofence set to LAND and secondary set to TERMINATE. This tested the use case that a drone should attempt to land if the geofence is breeched, but if the secondary is reached we need to get the drone out of the air immediately.
- Primary geofence set to WARN and secondary set to TERMINATE. This allowed testing that if a primary geofence was violated but recovered, the secondary could still terminate flight. This was tested on a concave geofence polygon, shown here.
Additional context
Replaces #20055 and should not impact #18821 since the outer geofence behavior is only encountered if the inner geofence has been breeched. If the inner geofence has not been breeched, behavior is identical to before.
Of course, let's be sure that works because without it a drone can sneak out of corners, or appear artificially close to a segment if it was before start. Also take a look at the vertical fence calculation. I modified what was in the original PR because if a drone's altitude fell within the vertical window the buffer zone check always returned true.
Didn't do much with the other logic, most of it was close to what the outer fence required aside from a bit of reordering conditionals.
Hi @xinnad ,
I modified what was in the original PR because if a drone's altitude fell within the vertical window the buffer zone check always returned true.
I might be confused about what you mean. If within the vertical window of the buffer zone the correct return value should always be true, right? The method call is named isInsideBufferZone(); hopefully, it should always return true if the drone's altitude is within the vertical buffer.
Apologies, am I missing something?
Hi @xinnad ,
I modified what was in the original PR because if a drone's altitude fell within the vertical window the buffer zone check always returned true.
I might be confused about what you mean. If within the vertical window of the buffer zone the correct return value should always be true, right? The method call is named
isInsideBufferZone(); hopefully, it should always return true if the drone's altitude is within the vertical buffer.Apologies, am I missing something?
I think the behavior from the original geofence is "if the vertical window is set, the drone must be within the vertical limit, as well as within the polygon." By returning true after checking the vertical limit, we never check that the drone is also within the polygon geofence buffer zone, so the behavior is that the buffer is ignored if the drone's altitude is correct allowing it to fly out of the polygon without checking. Does that match what you're thinking?
I think the behavior from the original geofence is "if the vertical window is set, the drone must be within the vertical limit, as well as within the polygon." By returning true after checking the vertical limit, we never check that the drone is also within the polygon geofence buffer zone, so the behavior is that the buffer is ignored if the drone's altitude is correct allowing it to fly out of the polygon without checking. Does that match what you're thinking?
Existing code returns immediately if altitude check fails: https://github.com/PX4/PX4-Autopilot/blob/cfc579542e1bad253c26fdc90fb432baf5ffcd07/src/modules/navigator/geofence.cpp#L330-L335
Is there a need to check the polygon/circle if alt fails? Behavior is the same regardless of whether vertical or horizontal bounds are compromised. I re-checked the logic in #20055 and it functions as expected, returning true for being inside the buffer zone.
Regardless, because of the logic changes in this PR, the logic in this PR also works.
Hi @xinnad, I think I've spotted an easy way to make it user configurable whether internal or external, so let's have it both ways! Give me a day or two to get that work done.
@xinnad , inner and outer buffer zones are now available in #20055 by allowing positive and negative buffer distances. I've also fixed the cases for exclusion fences that were incorrect previously.
@mcsauder after some discussion with @AlexKlimaj and @dagar the idea of supporting both inner and outer geofence buffers seems to introduce an obvious place for confusion. Changing language from things like a "secondary geofence" or a "geofence buffer" to an "emergency geofence" seems to clear things up well, eliminates the need for any inner geofence logic, and logically plays nicely with gf_predict. With the new language, the two go hand in hand, since gf_predict only operates on the original geofence and the emergency geofence can still kick in outside the original should gf_predict fail to halt a drone in time. Will look through the logic on exclusion fences in the original PR to ensure that's working correctly.
Hi @xinnad, you've probably noticed that PR #20055 has changed substantially in the past week. PR #20172 also alters things further. I agree that inner and outer causes confusion. If you look at the current code in #20055 you'll see that it is only an outer (secondary) geofence. In light of the upheaval that #20172 causes, I am going to wait until it is merged to further progress that PR. Also, in light of the decreased datamanager reads and other improvements in #20055, I would not recommend this PR in it's current state. PR #20055 is fully functional execpt for avoidance, which is failing at present. I'll finish fixing as soon as the commander re-write is in upstream.
@mcsauder I think it's a straightforward fix over here. Give me a couple minutes to push the little fix for exclusion fences and we're probably in good shape. Good catch on the exclusion fences, I needed to verify that was working and it falls nicely into the logic we have.
@mcsauder this version is tested with both inclusion and exclusion for polygons and circles. The emergency geofence is triggered GF_EMERGENCY_DST meters after encountering the primary, so the emergency fence is outside the primary for an inclusion fence and inside the primary for an exclusion fence.
@dagar suggested only engaging the emergency fence if it's GF_EMERGENCY_ACT is more severe than the primary. After some more thought I think this is the right call since we could currently recover into a less severe mode (i.e. the primary fence is set to RTL but the emergency fence set to HOLD is reached preventing the drone from returning). I'm headed out for a bit but can get this in later. Aside from that this PR should implement the functionality for an emergency fence for inclusion/exclusion for polygons/circles, so feel free to take a look and test and see if I missed anything.