Geozones (aka Geofence)

[Scavanger]

• Exclusive (non-flight zones - NFZ) and inclusive (flight zones - FZ) Zones
• Round and polygonal zones
• Actions are triggerd in assisted modes only: Angle, Horizon, Cruise and RTH
• Max. 63 zones with 126 corner points, the corner points can be freely distributed among the zones
• NFZ and FZ can be nested as desired
• Actions when a zone is violated:
  • Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary “billiard ball effect”
  • Hold: Position in front of the boundary is held
  • RTH: Triggers return to home
  • None: No action (only info in OSD)
• “Smart RTH”: If geo-zones are in the way, an attempt is made to calculate a course to avoid non-flight zones,
• Arming is blocked in non-flight zones
• 2 OSD elements that display the horizontal or vertical next boundary

Not for F722 FC, as it is already full to the brim.

Documentation still to come :)

[stronnag]

cli.c. FEATURE names still contains MOTOR_STOP. Is GEOZONE in the wrong place ?

[stronnag]

Please enable by default for the SITL:

#define USE_GEOZONE
#define MAX_GEOZONES_IN_CONFIG 63
#define MAX_VERTICES_IN_CONFIG 126

[Scavanger]

@stronnag Good catches.

[stronnag]

@stronnag Good catches.

I have stuff I'd like to test against this .....

[mmosca]

Looks good to me. Haven't test flown it.

[b14ckyy]

Just for the records:

We are using the general code of Geozoning for nearly a year now in a commercial environment on the basis of INAV 7.1.2. So functional it works flawless.
The integration in INAV 8.0 was so far tested by @Scavanger in SITL but as the general code has not changed except a small vertical ceiling tweak, I don't expect a logic failure. Will test HITL over the Weekend and then see when I get to real world flying again. Probably not before the 15th.

[mmosca]

Is there a configurator counterpart?

[stronnag]

For the geozone item, the order in which elements are read and written over MSP differs (index is first in one case and last in the other). It would be more consistent to use the same element order for both reading and writing unless there is some overriding technical reason to differ.

I don't quite follow. Can you show me the corresponding code positions?

static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src)
{
    const uint8_t idx = sbufReadU8(src);
    if (idx < MAX_GEOZONES_IN_CONFIG) {
        sbufWriteU8(dst, geoZonesConfig(idx)->type);
        sbufWriteU8(dst, geoZonesConfig(idx)->shape);
        sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude);
        sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude);
        sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction);
        sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount);
        sbufWriteU8(dst, idx);   // **** FInal item sent back to the consumer **
        return MSP_RESULT_ACK;
    } else {
        return MSP_RESULT_ERROR;
    }
}

and

    case MSP2_INAV_SET_GEOZONE:
        if (dataSize == 13) {
            uint8_t geozoneId;
            if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { // **** first item read from the consumer
                return MSP_RESULT_ERROR;
            }
            
            geozoneResetVertices(geozoneId, -1);
            geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); 
            geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src);
            geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src);
            geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src);   
            geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src);
            geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src);
        } else {
            return MSP_RESULT_ERROR;
        }
        break;

In the first instance MSP2_INAV_GEOZONE , the index is the final item written. In the second instance MSP2_INAV_SET_GEOZONE, it is the first item read. Otherewise the items are the same.

In the past, we've tried to keep the in / out "structures" the same. This makes life easier for consumers. Otherwise they might assume the "structures" are the same, and massively corrupt the internal dataset (and then ask for a CLI command it fix the mess).

[stronnag]

For example, for Waypoints, Safehomes, FW Approach, we send and receive "structured item" elements in the same order.

[Scavanger]

@stronnag
Ah now, you're right, I've changed it. I didn't notice it because I don't evaluate the return value in the configurator at all, but for other applications it should be consistent, I agree.

[stronnag]

Thanks. I will change mwp's implementation.

[Scavanger]

Thanks for the input, I implemented your suggestions and added the possibility to specify minimum and maximum altitude as absolute altitude (sea level as refernz).

[b14ckyy]

Thanks. I will change mwp's implementation.

that means INAV 8 will need the GTK4 version then, right? Just validating as I guess it will then not work with the GTK3 version anymore?

[stronnag]

Thanks. I will change mwp's implementation.

that means INAV 8 will need the GTK4 version then, right? Just validating as I guess it will then not work with the GTK3 version anymore?

Correct. Even before the review changes:

• The MSP has changed since the earlier (draft / provisional) documentation (both function ID and the number of functions (now separate MSP for ZONE and VERTEX rather than a combined message)). This is a good change, as the prior implementation could have theoretically created a very large message that might have broken some low capability telemetry devices.
• The FEATURE Id has changed.

So the Gtk3 version cannot work with INAV8 (the FEATURE Id alone will prevent that, even before the new MSP (and then the changed new MSP).

The current development branch mwp (GTK4) will work with INAV 8 (the state of this branch at time of writing).

[stronnag]

(erroneous report removed)
unless I'm doing something stupid ... I was ...

[stronnag]

OK, I'm stupid. Forgot to hard reset the SITL EEPROM after the last update.

[b14ckyy]

Any further changes needed here or any complaints? Or are we ready to throw it in?

[stronnag]

Any further changes needed here or any complaints? Or are we ready to throw it in?

As far as configuration / MSP / round-tripping settings, I'm content that it's fit for purpose.
The only way we'll find any more issues is by having it more generally available.

[stronnag]

Is there anything more Andy wants to do before merging (like push some documentation ....) ?

[b14ckyy]

Documentation should not be a big deal. If I remember right Andreas made a documentation for the Client version that should be just copy and paste but I am happy to help with that in a separate PR.

[Scavanger]

@stronnag

OK, I'm stupid. Forgot to hard reset the SITL EEPROM after the last update.

Ah ok. :)
I have made a small improvement to the CLI interface, but I will also point this out in the documentation:

I recommend NOT to configure geozones via CLI, it is very easy to create an invalid configuration (e.g. the vertices have to be
saved consecutively counterclockwise or the number of saved vertices (in the gezones config) differs from the actual ones (in the vertex config)), the CLI should only be used to save and restore the configuration.

[stronnag]

So just to sure that I understand this:

(e.g. the vertices have to be saved consecutively counterclockwise or the number of saved vertices (in the gezones config) differs from the actual ones (in the vertex config)),

Here the user has created the red polygon with points approximately N, W, S, E

The user then creates the green polygon, and drags points 1, 3 to the opposite sides so we have the green polygon, with points approximately N, E, S, W. Because the user has dragged the points around, this now becomes an invalid zone (the green shape)?

Note: You can do this without restriction in the Configurator as well.

Resulting CLI:

# geozone 
geozone 0 1 0 0 0 0 0
geozone 1 1 1 0 0 0 0

# geozone vertex
geozone vertex 0 0 543576459 -45277198
geozone vertex 0 1 543563590 -45322989
geozone vertex 0 2 543541043 -45297853
geozone vertex 0 3 543554153 -45262893
geozone vertex 1 0 543587359 -45235912
geozone vertex 1 1 543572953 -45215814
geozone vertex 1 2 543558933 -45232994
geozone vertex 1 3 543572947 -45257052

If I reload it from EEPROM (MSP2). the number of vertices is consistent.

Are there any more hidden restrictions I should know about?

[Scavanger]

@stronnag
Perhaps wecan add a check in the Configurator to see if the polygon is arranged correctly.

It is important that the polygons are arranged against the clock hand and in ascending order (parameter index in geozone vertex zone). The order in which they are stored is completely irrelevant (the config array can also be completely fragmented) as long as the index is correct.

For the RTH algorithm it is also important that for overlapping zones there are at least two vertices of a zone with a minimum distance of 1 x loiter radius (fixed wings) or “geozone_mr_stop_distance” (multirotor) to the border of the other zone.

[stronnag]

@stronnag Perhaps wecan add a check in the Configurator to see if the polygon is arranged correctly.

It is important that the polygons are arranged against the clock hand and in ascending order (parameter index in geozone vertex zone). The order in which they are stored is completely irrelevant (the config array can also be completely fragmented) as long as the index is correct.

For the RTH algorithm it is also important that for overlapping zones there are at least two vertices of a zone with a minimum distance of 1 x loiter radius (fixed wings) or “geozone_mr_stop_distance” (multirotor) to the border of the other zone.

I think some are needed. Previously, mwp had no rules about shape direction etc. If I uploaded the zones shown below to the FC, then downloaded them into the configurator then a Configurator upload would corrupt the FC dataset such that CLI reset commands were needed. I think the Configurator uploaded the zone with the original vertex count, but stopped uploading vertices when it hit one that it considered invalid. So the FC datastore was left inconsistent.

Now mwp now performs some checks and cancels invalid uploads completely. I guess I could just reverse clockwise shapes automagically, but complete cancel might get the user to learn the rules ...

(1 is the green diamond, 2 is the red crossing-lines shape, 3 is rightmost). The left red diamond (0) is ccw and valid.

[b14ckyy]

Maybe I missed something but why is the right most one invalid? The thick green one? Having U-Shaped or L-Shaped zones worked fine so far.

[Scavanger]

@stronnag
I am currently implementing the same functions in the Configurator.

Marc is right, convex or concave zones are no problem, complex (i.e. with crossed lines) ploygons are not allowed.

[stronnag]

Maybe I missed something but why is the right most one invalid? The thick green one? Having U-Shaped or L-Shaped zones worked fine so far.

Because there is NO documentation on how this complex function is supposed to work and the check shown was too strict.

[b14ckyy]

Working on it https://github.com/b14ckyy/inav/blob/Geozones.md/docs/Geozones.md

[stronnag]

Working on it https://github.com/b14ckyy/inav/blob/Geozones.md/docs/Geozones.md

Excellent! mwp zone checking is now working correctly (mea culpa).