diff --git a/docs/Geozones.md b/docs/Geozones.md index 483faaef05a..89a8fd4b27b 100644 --- a/docs/Geozones.md +++ b/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. diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 718dadca046..bf4e81800b8 100644 --- a/src/main/fc/cli.c +++ b/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); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e5bbcf49037..c86018f0b9c 100644 --- a/src/main/io/osd.c +++ b/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; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index f31b277a7d8..e34995d298b 100644 --- a/src/main/io/osd.h +++ b/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" diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 110430e167c..10782ace70f 100644 --- a/src/main/io/osd_dji_hd.c +++ b/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; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d0713d401c5..37923ffd07a 100644 --- a/src/main/navigation/navigation.h +++ b/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); diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index dfc7539859b..68b122ba1a5 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/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;