Fix reading the stored geozones back on startup

docs/Geozones.md

@@ -101,6 +101,7 @@ The most important feature for safety is the automatic path planning for RTH (St
 - If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m.
 - There is a chance that Smart RTH cannot find a path around NFZ areas, if there are multiple very big zones blocking the path. Due to hardware limitations, the amount of waypoints that Smart RTH can create are limited. Many Zones with very long border lines (>500m) cause additional waypoints.
 - It is not recommended to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can lead to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine.
+- A zone whose vertices are incomplete - fewer points than its `geozone` entry declares, which a hand-edited CLI config or an interrupted upload can produce - blocks arming. The OSD shows `GEOZONE MISCONFIGURED` on the arming screen and `GEOZONE CONFIG ERR` as a warning, and the CLI `status` command lists the reason. Fix the zone or run `geozone reset`; the aircraft does not arm with a partial zone.
 
 ## CLI
 The Geozone Information are stored in two separate data arrays. The first array holds the main Geozone Information and settings. The second array holds the Geozone vertices. 

src/main/fc/cli.c

@@ -4305,6 +4305,11 @@ static void cliStatus(char *cmdline)
     }
 #endif
 
+#if defined(USE_GEOZONE)
+    if ((armingFlags & ARMING_DISABLED_GEOZONE) && geozoneIsConfigInvalid()) {
+        cliPrintErrorLinef("Geozone vertices are incomplete, no zone is active");
+    }
+#endif
 
 #else
     cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);

src/main/io/osd.c

@@ -933,6 +933,10 @@ static const char * osdArmingDisabledReasonMessage(void)
 
         case ARMING_DISABLED_GEOZONE:
 #ifdef USE_GEOZONE
+            // Check the exact reason
+            if (geozoneIsConfigInvalid()) {
+                return OSD_MESSAGE_STR(OSD_MSG_GEOZONE_MISCONFIG);
+            }
             return OSD_MESSAGE_STR(OSD_MSG_NFZ);
 #else
             FALLTHROUGH;

src/main/io/osd.h

@@ -161,6 +161,7 @@
 
 #if defined(USE_GEOZONE)
 #define OSD_MSG_NFZ                 "NO FLY ZONE"
+#define OSD_MSG_GEOZONE_MISCONFIG   "GEOZONE MISCONFIGURED"
 #define OSD_MSG_LEAVING_FZ          "LEAVING FZ IN %s"
 #define OSD_MSG_OUTSIDE_FZ          "OUTSIDE FZ"
 #define OSD_MSG_ENTERING_NFZ        "ENTERING NFZ IN %s %s"

src/main/io/osd_dji_hd.c

@@ -519,6 +519,11 @@ static char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
             // Cases without message
         case ARMING_DISABLED_GEOZONE:
+#ifdef USE_GEOZONE
+            if (geozoneIsConfigInvalid()) {
+                return OSD_MESSAGE_STR("GEOZONE CONFIG ERR");
+            }
+#endif
             return OSD_MESSAGE_STR("NO FLY ZONE");
         case ARMING_DISABLED_LANDING_DETECTED:
             FALLTHROUGH;

src/main/navigation/navigation.h

@@ -217,6 +217,7 @@ void geozoneReset(int8_t idx);
 void geozoneResetVertices(int8_t zoneId, int16_t idx);
 void geozoneUpdate(timeUs_t curentTimeUs);
 bool geozoneIsBlockingArming(void);
+bool geozoneIsConfigInvalid(void);
 void geozoneAdvanceRthAvoidWaypoint(void);
 int8_t geozoneCheckForNFZAtCourse(bool isRTH);
 bool geoZoneIsLastRthWaypoint(void);

src/main/navigation/navigation_geozone.c

@@ -113,6 +113,7 @@ static geoZoneRuntimeConfig_t *nearestHorZone = NULL;
 static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL;
 static fpVector3_t avoidingPoint;
 static bool geozoneIsEnabled = false;
+static bool configIsInvalid = false;
 static fpVector3_t rthWaypoints[MAX_RTH_WAYPOINTS];
 static uint8_t rthWaypointIndex = 0;
 static int8_t rthWaypointCount = 0;
@@ -1606,33 +1607,40 @@ static void endFenceAction(void)
 static void geoZoneInit(void)
 {
     activeGeoZonesCount = 0;
+    configIsInvalid = false;
     uint8_t expectedVertices = 0, configuredVertices = 0;
     for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++)
     {
         if (geoZonesConfig(i)->vertexCount > 0) {
-            memcpy(&activeGeoZones[activeGeoZonesCount].config, geoZonesConfig(i), sizeof(geoZoneRuntimeConfig_t));
-            if (activeGeoZones[i].config.maxAltitude == 0) {
-                activeGeoZones[i].config.maxAltitude = INT32_MAX;
+            // activeGeoZones is packed, zones without vertices are skipped, so the config index i is not the runtime index
+            geoZoneRuntimeConfig_t *zone = &activeGeoZones[activeGeoZonesCount];
+
+            memcpy(&zone->config, geoZonesConfig(i), sizeof(geoZoneConfig_t));
+            zone->radius = 0;
+            zone->verticesLocal = NULL;
+
+            if (zone->config.maxAltitude == 0) {
+                zone->config.maxAltitude = INT32_MAX;
             }
 
-            if (activeGeoZones[i].config.isSealevelRef) {
-                
-                if (activeGeoZones[i].config.maxAltitude != 0) {
-                    activeGeoZones[i].config.maxAltitude -= GPS_home.alt;
+            if (zone->config.isSealevelRef) {
+
+                if (zone->config.maxAltitude != 0) {
+                    zone->config.maxAltitude -= GPS_home.alt;
                 }
-                
-                if (activeGeoZones[i].config.minAltitude != 0) {
-                    activeGeoZones[i].config.minAltitude -= GPS_home.alt;
+
+                if (zone->config.minAltitude != 0) {
+                    zone->config.minAltitude -= GPS_home.alt;
                 }
             }
-            
-            activeGeoZones[i].isInfZone = activeGeoZones[i].config.maxAltitude == INT32_MAX && activeGeoZones[i].config.minAltitude == 0;
-            
-            if (!STATE(AIRPLANE) && activeGeoZones[i].config.fenceAction == GEOFENCE_ACTION_AVOID) {
-                activeGeoZones[i].config.fenceAction = GEOFENCE_ACTION_POS_HOLD;
+
+            zone->isInfZone = zone->config.maxAltitude == INT32_MAX && zone->config.minAltitude == 0;
+
+            if (!STATE(AIRPLANE) && zone->config.fenceAction == GEOFENCE_ACTION_AVOID) {
+                zone->config.fenceAction = GEOFENCE_ACTION_POS_HOLD;
             }
 
-            activeGeoZones[activeGeoZonesCount].enable = true;
+            zone->enable = true;
             activeGeoZonesCount++;
         }
         expectedVertices += geoZonesConfig(i)->vertexCount;
@@ -1644,29 +1652,36 @@ static void geoZoneInit(void)
             gpsLocation_t vertexLoc;
             fpVector3_t posLocal3;
 
-            if (geoZoneVertices(i)->zoneId >= 0 && geoZoneVertices(i)->zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx <= MAX_VERTICES_IN_CONFIG) {         
+            const int8_t zoneId = geoZoneVertices(i)->zoneId;
+            if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx < geoZonesConfig(zoneId)->vertexCount) {
                 configuredVertices++;
-                if (geoZonesConfig(geoZoneVertices(i)->zoneId)->shape == GEOZONE_SHAPE_CIRCULAR && geoZoneVertices(i)->idx == 1) {
-                    activeGeoZones[geoZoneVertices(i)->zoneId].radius = geoZoneVertices(i)->lat;
-                    activeGeoZones[geoZoneVertices(i)->zoneId].config.vertexCount = 1;
+
+                // Map the config zone id onto the packed runtime index and onto the start of the zones vertices
+                uint8_t zoneIdx = 0, vertexIdx = 0;
+                for (uint8_t j = 0; j < zoneId; j++) {
+                    if (geoZonesConfig(j)->vertexCount > 0) {
+                        vertexIdx += geoZonesConfig(j)->vertexCount;
+                        zoneIdx++;
+                    }
+                }
+
+                if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR && geoZoneVertices(i)->idx == 1) {
+                    activeGeoZones[zoneIdx].radius = geoZoneVertices(i)->lat;
+                    activeGeoZones[zoneIdx].config.vertexCount = 1;
                     continue;
                 }
-                
+
                 vertexLoc.lat = geoZoneVertices(i)->lat;
                 vertexLoc.lon = geoZoneVertices(i)->lon;
                 geoConvertGeodeticToLocal(&posLocal3, &posControl.gpsOrigin, &vertexLoc, GEO_ALT_ABSOLUTE);
 
-                uint8_t vertexIdx = 0;
-                for (uint8_t j = 0; j < geoZoneVertices(i)->zoneId; j++) {
-                    vertexIdx += activeGeoZones[j].config.vertexCount;
-                }
                 vertexIdx += geoZoneVertices(i)->idx;
 
                 verticesLocal[vertexIdx].x = posLocal3.x;
                 verticesLocal[vertexIdx].y = posLocal3.y;
 
                 if (geoZoneVertices(i)->idx == 0) {
-                    activeGeoZones[geoZoneVertices(i)->zoneId].verticesLocal = &verticesLocal[vertexIdx];
+                    activeGeoZones[zoneIdx].verticesLocal = &verticesLocal[vertexIdx];
                 }
             }
         }
@@ -1689,6 +1704,20 @@ static void geoZoneInit(void)
         configuredVertices++;
     }
 
+    // Vertices are missing or do not belong to any zone, the fences can not be reconstructed.
+    // Report this instead of taking off with an incomplete set of zones.
+    bool verticesAreComplete = expectedVertices == configuredVertices;
+    for (uint8_t i = 0; i < activeGeoZonesCount && verticesAreComplete; i++) {
+        verticesAreComplete = activeGeoZones[i].verticesLocal != NULL;
+    }
+
+    if (!verticesAreComplete) {
+        configIsInvalid = true;
+        setTaskEnabled(TASK_GEOZONE, false);
+        geozoneIsEnabled = false;
+        return;
+    }
+
     updateCurrentZones();
     uint8_t newActiveZoneCount = activeGeoZonesCount;
     for (uint8_t i = 0; i < activeGeoZonesCount; i++) {
@@ -1718,7 +1747,7 @@ static void geoZoneInit(void)
     }
 
     activeGeoZonesCount = newActiveZoneCount;
-    if (activeGeoZonesCount == 0 || expectedVertices != configuredVertices) {
+    if (activeGeoZonesCount == 0) {
         setTaskEnabled(TASK_GEOZONE, false);
         geozoneIsEnabled = false;
         return;
@@ -2071,9 +2100,19 @@ void geozoneUpdateMaxHomeAltitude(void) {
     }
 }
 
-// Avoid arming in NFZ 
+bool geozoneIsConfigInvalid(void)
+{
+    return isInitalised && configIsInvalid;
+}
+
+// Avoid arming in NFZ
 bool geozoneIsBlockingArming(void)
 {
+    // The configured zones could not be loaded, don't take off without the fences the user set up
+    if (geozoneIsConfigInvalid()) {
+        return true;
+    }
+
     // Do not generate arming flags unless we are sure about them
     if (!isInitalised || !geozoneIsEnabled || activeGeoZonesCount == 0)  {
         return false;