Geozones (aka Geofence)

src/main/fc/cli.c

@@ -1565,19 +1565,20 @@ static void cliSafeHomes(char *cmdline)
 #if defined(USE_GEOZONE)
 static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone)
 {
-    const char *format = "geozone %u %u %u %d %d %u";
+    const char *format = "geozone %u %u %u %d %d %d %u";
     for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) {
         bool equalsDefault = false;
         if (defaultGeoZone) {
             equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction
             && geoZone[i].shape == defaultGeoZone->shape
             && geoZone[i].type == defaultGeoZone->type
             && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude
-            && geoZone[i].minAltitude == defaultGeoZone->minAltitude;
+            && geoZone[i].minAltitude == defaultGeoZone->minAltitude
+            && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef;
 
-            cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction);  
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction);  
         }
-        cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction);    
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction);    
     }
 } 
 
@@ -1722,8 +1723,16 @@ static void cliGeozone(char* cmdLine)
         uint8_t totalVertices = geozoneGetUsedVerticesCount();
         cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG);
 
+    } else if (sl_strncasecmp(cmdLine, "reset", 5) == 0) {
+        const char* ptr = &cmdLine[5];
+        if ((ptr = nextArg(ptr))) {
+            int idx = fastA2I(ptr);
+            geozoneReset(idx);
+        } else {
+            geozoneReset(-1);
+        } 
     } else {
-        int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0;
+        int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0;
         int32_t minAltitude = 0, maxAltitude = 0;
         const char* ptr = cmdLine;
         uint8_t argumentCount = 1;
@@ -1766,6 +1775,14 @@ static void cliGeozone(char* cmdLine)
             return;
         }
 
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            seaLevelRef = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
         if ((ptr = nextArg(ptr))){
             argumentCount++;        
             fenceAction = fastA2I(ptr);
@@ -1782,7 +1799,7 @@ static void cliGeozone(char* cmdLine)
             argumentCount++;
         } 
 
-        if (argumentCount != 6) {
+        if (argumentCount != 7) {
             cliShowParseError();
             return;
         }
@@ -1801,6 +1818,7 @@ static void cliGeozone(char* cmdLine)
 
         geoZonesConfigMutable(idx)->maxAltitude = maxAltitude;
         geoZonesConfigMutable(idx)->minAltitude = minAltitude;
+        geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef;
         geoZonesConfigMutable(idx)->fenceAction = fenceAction;
     }
 }

src/main/fc/fc_core.c

@@ -278,12 +278,12 @@ static void updateArmingStatus(void)
         }
 #endif
 
-#if defined(USE_GEOZONE) && defined (USE_GPS)
-        if (geozoneIsInsideNFZ()) {
-            ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
-        } else {
-            DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
-        }
+#ifdef USE_GEOZONE
+    if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) {
+        ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
+    } else {
+        DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
+    }
 #endif
 
         /* CHECK: */

src/main/fc/fc_msp.c

@@ -1771,7 +1771,7 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
 }
 #endif
 
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
 static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src)
 {
     const uint8_t idx = sbufReadU8(src);
@@ -1780,6 +1780,7 @@ static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src)
         sbufWriteU8(dst, geoZonesConfig(idx)->shape);
         sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude);
         sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude);
+        sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef);
         sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction);
         sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount);
         sbufWriteU8(dst, idx);
@@ -3365,9 +3366,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         gpsUbloxSendCommand(src->ptr, dataSize, 0);
         break;
 
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
     case MSP2_INAV_SET_GEOZONE:
-        if (dataSize == 13) {
+        if (dataSize == 14) {
             uint8_t geozoneId;
             if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
                 return MSP_RESULT_ERROR;
@@ -3377,7 +3378,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); 
             geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src);
             geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src);
-            geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src);   
+            geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src);  
+            geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src);   
             geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src);
             geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src);
         } else {
@@ -3986,7 +3988,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = mspFwApproachOutCommand(dst, src);
         break;
 #endif
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
     case MSP2_INAV_GEOZONE:
         *ret = mspFcGeozoneOutCommand(dst, src);
         break;

src/main/fc/fc_tasks.c

@@ -339,7 +339,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 #endif
 }
 
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
 void geozoneUpdateTask(timeUs_t currentTimeUs)
 {
     if (feature(FEATURE_GEOZONE)) {
@@ -463,7 +463,7 @@ void fcTasksInit(void)
     serialProxyStart();
 #endif
 
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
     setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE));
 #endif
 
@@ -754,7 +754,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
     [TASK_GEOZONE] = {
         .taskName = "GEOZONE",
         .taskFunc = geozoneUpdateTask,

src/main/io/osd.c

@@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2);
 
 void osdStartedSaveProcess(void) {

src/main/io/osd.h

@@ -136,7 +136,7 @@
 #define OSD_MSG_NFZ                 "NO FLY ZONE"
 #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"
+#define OSD_MSG_ENTERING_NFZ        "ENTERING NFZ IN %s %s"
 #define OSD_MSG_AVOIDING_FB         "AVOIDING FENCE BREACH"
 #define OSD_MSG_RETURN_TO_ZONE      "RETURN TO FZ"
 #define OSD_MSG_FLYOUT_NFZ          "FLY OUT NFZ"

src/main/navigation/navigation.c

@@ -1713,22 +1713,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
             return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
         } else {
 #endif
-        fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
+            fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
 
-        if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
-            // Successfully reached position target - update XYZ-position
-            setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+            if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
+                // Successfully reached position target - update XYZ-position
+                setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 
-            posControl.landingDelay = 0;
+                posControl.landingDelay = 0;
 
-            if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
-                posControl.rthState.rthLinearDescentActive = false;
+                if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
+                    posControl.rthState.rthLinearDescentActive = false;
 
-            return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
-        } else {
-            setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
-            return NAV_FSM_EVENT_NONE;
-        }
+                return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
+            } else {
+                setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
+                return NAV_FSM_EVENT_NONE;
+            }
 #ifdef USE_GEOZONE
         }
 #endif
@@ -3348,7 +3348,7 @@ void updateHomePosition(void)
                     setHome = true;
                     break;
             }
-#if defined(USE_GEOZONE) && defined (USE_GPS)
+#ifdef USE_GEOZONE
             geozoneUpdateMaxHomeAltitude();
 #endif
         }

src/main/navigation/navigation.h

@@ -118,6 +118,9 @@ void resetFwAutolandApproach(int8_t idx);
 
 #if defined(USE_GEOZONE)
 
+#ifndef USE_GPS
+    #error "Geozone needs GPS support"
+#endif
 
 typedef enum {
     GEOZONE_MESSAGE_STATE_NONE,
@@ -156,6 +159,7 @@ typedef struct geoZoneConfig_s
     uint8_t type;
     int32_t minAltitude;
     int32_t maxAltitude;
+    bool isSealevelRef;
     uint8_t fenceAction;
     uint8_t vertexCount;
 } geoZoneConfig_t;
@@ -209,9 +213,10 @@ bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon
 int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId);
 bool isGeozoneActive(void);
 uint8_t geozoneGetUsedVerticesCount(void);
+void geozoneReset(int8_t idx);
 void geozoneResetVertices(int8_t zoneId, int16_t idx);
 void geozoneUpdate(timeUs_t curentTimeUs);
-bool geozoneIsInsideNFZ(void);
+bool geozoneIsBlockingArming(void);
 void geozoneAdvanceRthAvoidWaypoint(void);
 int8_t geozoneCheckForNFZAtCourse(bool isRTH);
 bool geoZoneIsLastRthWaypoint(void);