Merge pull request #10459 from Scavanger/Geozones

.vscode/c_cpp_properties.json

@@ -41,4 +41,4 @@
         }
     ],
     "version": 4
-}
+}

.vscode/settings.json

@@ -4,7 +4,7 @@
         "cmath": "c",
         "ranges": "c",
         "navigation.h": "c",
-        "rth_trackback.h": "c"
+        "rth_trackback.h": "c",
         "platform.h": "c",
         "timer.h": "c",
         "bus.h": "c"
@@ -15,4 +15,4 @@
     "editor.expandTabs": true,
     "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4,  BreakBeforeBraces: Mozilla }"
 
-}
+}

docs/Settings.md

@@ -1472,6 +1472,76 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
 
 ---
 
+### geozone_avoid_altitude_range
+
+Altitude range in which an attempt is made to avoid a geozone upwards
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5000 | 0 | 1000000 |
+
+---
+
+### geozone_detection_distance
+
+Distance from which a geozone is detected
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50000 | 0 | 1000000 |
+
+---
+
+### geozone_mr_stop_distance
+
+Distance in which multirotors stops before the border
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 15000 | 0 | 100000 |
+
+---
+
+### geozone_no_way_home_action
+
+Action if RTH with active geozones is unable to calculate a course to home
+
+| Default | Min | Max |
+| --- | --- | --- |
+| RTH |  |  |
+
+---
+
+### geozone_safe_altitude_distance
+
+Vertical distance that must be maintained to the upper and lower limits of the zone.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1000 | 0 | 10000 |
+
+---
+
+### geozone_safehome_as_inclusive
+
+Treat nearest safehome as inclusive geozone
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
+### geozone_safehome_zone_action
+
+Fence action for safehome zone
+
+| Default | Min | Max |
+| --- | --- | --- |
+| NONE |  |  |
+
+---
+
 ### gimbal_pan_channel
 
 Gimbal pan rc channel index. 0 is no channel.

src/main/CMakeLists.txt

@@ -576,6 +576,7 @@ main_sources(COMMON_SRC
     navigation/navigation_pos_estimator_flow.c
     navigation/navigation_private.h
     navigation/navigation_rover_boat.c
+    navigation/navigation_geozone.c
     navigation/sqrt_controller.c
     navigation/sqrt_controller.h
     navigation/rth_trackback.c

src/main/common/vector.h

@@ -29,6 +29,13 @@ typedef union {
     };
 } fpVector3_t;
 
+typedef union {
+    float v[2];
+    struct {
+       float x,y;
+    };
+} fpVector2_t;
+
 typedef struct {
     float m[3][3];
 } fpMat3_t;
@@ -116,3 +123,79 @@ static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t
     *result = ab;
     return result;
 }
+
+static inline fpVector3_t* vectorSub(fpVector3_t* result, const fpVector3_t* a, const fpVector3_t* b)
+{
+    fpVector3_t ab;
+
+    ab.x = a->x - b->x;
+    ab.y = a->y - b->y;
+    ab.z = a->z - b->z;
+
+    *result = ab;
+    return result;
+}
+
+static inline float vectorDotProduct(const fpVector3_t* a, const fpVector3_t* b)
+{
+    return  a->x * b->x + a->y * b->y + a->z * b->z;
+}
+
+static inline float vector2NormSquared(const fpVector2_t * v)
+{
+    return sq(v->x) + sq(v->y);
+}
+
+static inline fpVector2_t* vector2Normalize(fpVector2_t* result, const fpVector2_t* v)
+{
+    float sqa = vector2NormSquared(v);
+    float length = sqrtf(sqa);
+    if (length != 0) {
+        result->x = v->x / length;
+        result->y = v->y / length;
+    } else {
+        result->x = 0;
+        result->y = 0;
+    }
+    return result;
+}
+
+
+static inline fpVector2_t* vector2Sub(fpVector2_t* result, const fpVector2_t* a, const fpVector2_t* b)
+{
+    fpVector2_t ab;
+
+    ab.x = a->x - b->x;
+    ab.y = a->y - b->y;
+
+    *result = ab;
+    return result;
+}
+
+static inline fpVector2_t * vector2Add(fpVector2_t * result, const fpVector2_t * a, const fpVector2_t * b)
+{
+    fpVector2_t ab;
+
+    ab.x = a->x + b->x;
+    ab.y = a->y + b->y;
+
+    *result = ab;
+    return result;
+}
+
+
+static inline float vector2DotProduct(const fpVector2_t* a, const fpVector2_t* b)
+{
+    return  a->x * b->x + a->y * b->y;
+}
+
+static inline fpVector2_t * vector2Scale(fpVector2_t * result, const fpVector2_t * a, const float b)
+{
+    fpVector2_t ab;
+
+    ab.x = a->x * b;
+    ab.y = a->y * b;
+
+    *result = ab;
+    return result;
+}

src/main/config/parameter_group_ids.h

@@ -129,7 +129,10 @@
 #define PG_GIMBAL_CONFIG 1039
 #define PG_GIMBAL_SERIAL_CONFIG 1040
 #define PG_HEADTRACKER_CONFIG 1041
-#define PG_INAV_END PG_HEADTRACKER_CONFIG
+#define PG_GEOZONE_CONFIG 1042
+#define PG_GEOZONES 1043
+#define PG_GEOZONE_VERTICES 1044
+#define PG_INAV_END PG_GEOZONE_VERTICES
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/cli.c

@@ -151,7 +151,7 @@ static uint8_t  commandBatchErrorCount = 0;
 
 // sync this with features_e
 static const char * const featureNames[] = {
-    "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
+    "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE",
     "", "SOFTSERIAL", "GPS", "RPM_FILTERS",
     "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
@@ -1561,6 +1561,285 @@ static void cliSafeHomes(char *cmdline)
 }
 
 #endif
+
+#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 %u %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].isSealevelRef == defaultGeoZone->isSealevelRef
+            && geoZone[i].fenceAction == defaultGeoZone->fenceAction
+            && geoZone[i].vertexCount == defaultGeoZone->vertexCount;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction, defaultGeoZone[i].vertexCount);  
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction, geoZone[i].vertexCount);    
+    }
+} 
+
+static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices)
+{
+    const char *format = "geozone vertex %d %u %d %d";
+    for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
+        bool equalsDefault = false;
+        if (defaultVertices) {
+            equalsDefault = vertices[i].idx == defaultVertices->idx
+            && vertices[i].lat == defaultVertices->lat
+            && vertices[i].lon == defaultVertices->lon
+            && vertices[i].zoneId == defaultVertices->zoneId;
+            
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon);
+        }
+        
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon);    
+    }
+
+    if (!defaultVertices) {
+        uint8_t totalVertices = geozoneGetUsedVerticesCount();
+        cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG);
+    }
+}
+
+static void cliGeozone(char* cmdLine)
+{  
+    if (isEmpty(cmdLine)) {
+        printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL);
+    } else if (sl_strcasecmp(cmdLine, "vertex") == 0) {    
+        printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL);
+    } else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) {
+         const char* ptr = &cmdLine[12];
+         uint8_t zoneId = 0, idx = 0;
+         uint8_t argumentCount = 1;
+
+         if ((ptr = nextArg(ptr))) {
+            zoneId = fastA2I(ptr);
+         } else {
+           geozoneResetVertices(-1, -1);
+           return;
+        }
+
+         if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            idx = fastA2I(ptr);
+        } else {
+            geozoneResetVertices(zoneId, -1);
+            return;
+        }
+
+         if (argumentCount != 2) {
+            cliShowParseError();
+            return;
+        }
+
+        geozoneResetVertices(zoneId, idx);
+
+    } else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) {
+        int32_t lat = 0, lon = 0;
+        int8_t zoneId = 0;
+        int16_t vertexIdx = -1;
+        uint8_t vertexZoneIdx = 0;
+        const char* ptr = cmdLine;
+        uint8_t argumentCount = 1;
+
+        if ((ptr = nextArg(ptr))) {         
+            zoneId = fastA2I(ptr);
+            if (zoneId < 0) {
+                return;
+            }
+
+            if (zoneId >= MAX_GEOZONES_IN_CONFIG) {
+                cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1);
+                return;
+            }
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+         if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            vertexZoneIdx = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            lat = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+        
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            lon = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+        }
+
+        if (argumentCount != 4) {
+            cliShowParseError();
+            return;
+        }
+        
+        for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
+            if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx)  {
+                geoZoneVerticesMutable(i)->lat = lat;
+                geoZoneVerticesMutable(i)->lon = lon;
+                return;
+            }
+        }
+
+        for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
+            if (geoZoneVertices(i)->zoneId == -1) {
+                vertexIdx = i;
+                break;
+            }
+        }
+
+        if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) {
+            cliPrintError("Maximum number of vertices reached.");
+            return;
+        }
+
+        geoZoneVerticesMutable(vertexIdx)->lat = lat;
+        geoZoneVerticesMutable(vertexIdx)->lon = lon;
+        geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId;
+        geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx;  
+        
+        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);
+            geozoneResetVertices(idx, -1);
+        } else {
+            geozoneReset(-1);
+            geozoneResetVertices(-1, -1);
+        } 
+    } else {
+        int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0, vertexCount = 0;
+        int32_t minAltitude = 0, maxAltitude = 0;
+        const char* ptr = cmdLine;
+        uint8_t argumentCount = 1;
+
+        idx = fastA2I(ptr);
+        if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) {
+            cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1);
+            return;
+        }
+        
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            isPolygon = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            isInclusive = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            minAltitude = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            maxAltitude = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            seaLevelRef = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;        
+            fenceAction = fastA2I(ptr);
+            if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) {
+                cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH);
+                return;
+            }
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            vertexCount = fastA2I(ptr);
+            if (vertexCount < 1 || vertexCount > MAX_VERTICES_IN_CONFIG) {
+                cliShowArgumentRangeError("vertex count", 1, MAX_VERTICES_IN_CONFIG);
+                return;
+            }
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+        } 
+
+        if (argumentCount != 8) {
+            cliShowParseError();
+            return;
+        }
+
+        if (isPolygon) {
+            geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON;
+        } else {
+            geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR;
+        }
+
+        if (isInclusive) {
+            geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE;
+        } else {
+            geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE;
+        }
+
+        geoZonesConfigMutable(idx)->maxAltitude = maxAltitude;
+        geoZonesConfigMutable(idx)->minAltitude = minAltitude;
+        geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef;
+        geoZonesConfigMutable(idx)->fenceAction = fenceAction;
+        geoZonesConfigMutable(idx)->vertexCount = vertexCount;
+    }
+}
+#endif
+
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
 static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
 {
@@ -4178,6 +4457,14 @@ static void printConfig(const char *cmdline, bool doDiff)
         printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
 #endif
 
+#if defined(USE_GEOZONE)
+        cliPrintHashLine("geozone");
+        printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0));
+
+        cliPrintHashLine("geozone vertices");
+        printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0));
+#endif
+
         cliPrintHashLine("features");
         printFeature(dumpMask, &featureConfig_Copy, featureConfig());
 
@@ -4561,6 +4848,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
 #endif
     CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
+#ifdef USE_GEOZONE
+    CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone),
+#endif
 #ifdef USE_GPS
     CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
     CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites),

src/main/fc/config.h

@@ -36,7 +36,7 @@ typedef enum {
     FEATURE_VBAT = 1 << 1,
     FEATURE_TX_PROF_SEL = 1 << 2,       // Profile selection by TX stick command
     FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
-    FEATURE_UNUSED_12 = 1 << 4,  //was FEATURE_MOTOR_STOP
+    FEATURE_GEOZONE = 1 << 4,  //was FEATURE_MOTOR_STOP
     FEATURE_UNUSED_1 = 1 << 5,   // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,

src/main/fc/fc_core.c

@@ -278,6 +278,14 @@ static void updateArmingStatus(void)
         }
 #endif
 
+#ifdef USE_GEOZONE
+    if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) {
+        ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
+    } else {
+        DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
+    }
+#endif
+
         /* CHECK: */
         if (
             sensors(SENSOR_ACC) &&

src/main/fc/fc_msp.c

@@ -1772,6 +1772,54 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
 }
 #endif
 
+#ifdef USE_GEOZONE
+static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t idx = sbufReadU8(src);
+    if (idx < MAX_GEOZONES_IN_CONFIG) {
+        sbufWriteU8(dst, idx);
+        sbufWriteU8(dst, geoZonesConfig(idx)->type);
+        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);
+        return MSP_RESULT_ACK;
+    } else {
+        return MSP_RESULT_ERROR;
+    }
+}
+
+static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t zoneId = sbufReadU8(src);
+    const uint8_t vertexId = sbufReadU8(src);
+    if (zoneId < MAX_GEOZONES_IN_CONFIG) {
+        int8_t  vertexIdx = geozoneGetVertexIdx(zoneId, vertexId);
+        if (vertexIdx >= 0) {
+            sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId);
+            sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx);
+            sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat);
+            sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon);
+            if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) {
+                int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1);
+                if (vertexRadiusIdx >= 0) {
+                    sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat);
+                } else {
+                    return MSP_RESULT_ERROR;
+                }
+            }
+            return MSP_RESULT_ACK;
+        } else {
+            return MSP_RESULT_ERROR;
+        } 
+    } else {
+        return MSP_RESULT_ERROR;
+    }
+}
+#endif
+
 static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
     const uint8_t idx = sbufReadU8(src);
     if (idx < MAX_LOGIC_CONDITIONS) {
@@ -3369,6 +3417,50 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         gpsUbloxSendCommand(src->ptr, dataSize, 0);
         break;
 
+#ifdef USE_GEOZONE
+    case MSP2_INAV_SET_GEOZONE:
+        if (dataSize == 14) {
+            uint8_t geozoneId;
+            if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
+                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)->isSealevelRef = sbufReadU8(src);   
+            geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src);
+            geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src);
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+    case MSP2_INAV_SET_GEOZONE_VERTEX:
+        if (dataSize == 10 || dataSize == 14) {
+            uint8_t geozoneId = 0;
+            if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
+                return MSP_RESULT_ERROR;
+            }
+            uint8_t vertexId = sbufReadU8(src);
+            int32_t lat = sbufReadU32(src);
+            int32_t lon = sbufReadU32(src);
+            if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) {
+                return MSP_RESULT_ERROR;
+            }
+
+            if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) {
+                if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) {
+                    return MSP_RESULT_ERROR;
+                }
+            }
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+#endif
+
 #ifdef USE_EZ_TUNE
 
     case MSP2_INAV_EZ_TUNE_SET:
@@ -3947,6 +4039,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = mspFwApproachOutCommand(dst, src);
         break;
 #endif
+#ifdef USE_GEOZONE
+    case MSP2_INAV_GEOZONE:
+        *ret = mspFcGeozoneOutCommand(dst, src);
+        break;
+    case MSP2_INAV_GEOZONE_VERTEX:
+        *ret = mspFcGeozoneVerteciesOutCommand(dst, src);
+        break;
+#endif
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
         tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version

src/main/fc/fc_tasks.c

@@ -336,6 +336,15 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 #endif
 }
 
+#ifdef USE_GEOZONE
+void geozoneUpdateTask(timeUs_t currentTimeUs)
+{
+    if (feature(FEATURE_GEOZONE)) {
+        geozoneUpdate(currentTimeUs);
+    }
+}
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -450,6 +459,11 @@ void fcTasksInit(void)
 #if defined(SITL_BUILD)
     serialProxyStart();
 #endif
+
+#ifdef USE_GEOZONE
+    setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE));
+#endif
+
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -737,4 +751,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_GEOZONE
+    [TASK_GEOZONE] = {
+        .taskName = "GEOZONE",
+        .taskFunc = geozoneUpdateTask,
+        .desiredPeriod = TASK_PERIOD_HZ(5),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 };

src/main/fc/runtime_config.h

@@ -23,7 +23,7 @@ typedef enum {
     WAS_EVER_ARMED                                  = (1 << 3),
     SIMULATOR_MODE_HITL                             = (1 << 4),
     SIMULATOR_MODE_SITL                             = (1 << 5),
-
+    ARMING_DISABLED_GEOZONE                         = (1 << 6),
     ARMING_DISABLED_FAILSAFE_SYSTEM                 = (1 << 7),
     ARMING_DISABLED_NOT_LEVEL                       = (1 << 8),
     ARMING_DISABLED_SENSORS_CALIBRATING             = (1 << 9),
@@ -49,8 +49,8 @@ typedef enum {
     ARMING_DISABLED_DSHOT_BEEPER                    = (1 << 29),
     ARMING_DISABLED_LANDING_DETECTED                = (1 << 30),
 
-    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING |
+    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | 
-                                                       ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
+                                                       ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
                                                        ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED |
                                                        ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE |
                                                        ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
@@ -65,7 +65,8 @@ typedef enum {
 // situations where we might just need the motors to spin so the
 // aircraft can move (even unpredictably) and get unstuck (e.g.
 // crashed into a high tree).
-#define ARMING_DISABLED_EMERGENCY_OVERRIDE  (ARMING_DISABLED_NOT_LEVEL \
+#define ARMING_DISABLED_EMERGENCY_OVERRIDE  (ARMING_DISABLED_GEOZONE \
+                                            | ARMING_DISABLED_NOT_LEVEL \
                                             | ARMING_DISABLED_NAVIGATION_UNSAFE \
                                             | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \
                                             | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \
@@ -106,6 +107,7 @@ typedef enum {
     SOARING_MODE          = (1 << 16),
     ANGLEHOLD_MODE        = (1 << 17),
     NAV_FW_AUTOLAND       = (1 << 18),
+    NAV_SEND_TO           = (1 << 19),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;

src/main/fc/settings.yaml

@@ -204,6 +204,11 @@ tables:
   - name: default_altitude_source
     values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"]
     enum: navDefaultAltitudeSensor_e
+  - name: fence_action
+    values: ["NONE", "AVOID", "POS_HOLD", "RTH"]
+    enum: fenceAction_e
+  - name: geozone_rth_no_way_home
+    values: [RTH, EMRG_LAND]
 
 constants:
   RPYL_PID_MIN: 0
@@ -4362,3 +4367,49 @@ groups:
       field: roll_ratio
       min: 0
       max: 5
+  - name: PG_GEOZONE_CONFIG
+    type: geozone_config_t
+    headers: ["navigation/navigation.h"]
+    condition: USE_GEOZONE && USE_GPS
+    members:
+      - name: geozone_detection_distance
+        description: "Distance from which a geozone is detected"
+        default_value: 50000
+        field: fenceDetectionDistance
+        min: 0
+        max: 1000000
+      - name: geozone_avoid_altitude_range
+        description: "Altitude range in which an attempt is made to avoid a geozone upwards"
+        default_value: 5000
+        field: avoidAltitudeRange
+        min: 0
+        max: 1000000
+      - name: geozone_safe_altitude_distance
+        description: "Vertical distance that must be maintained to the upper and lower limits of the zone."
+        default_value: 1000
+        field: safeAltitudeDistance
+        min: 0
+        max: 10000
+      - name: geozone_safehome_as_inclusive 
+        description: "Treat nearest safehome as inclusive geozone"
+        type: bool
+        field: nearestSafeHomeAsInclusivZone
+        default_value: OFF
+      - name: geozone_safehome_zone_action
+        description:  "Fence action for safehome zone"
+        default_value: "NONE"
+        field: safeHomeFenceAction    
+        table: fence_action
+        type: uint8_t
+      - name: geozone_mr_stop_distance
+        description: "Distance in which multirotors stops before the border"
+        default_value: 15000
+        min: 0
+        max: 100000
+        field: copterFenceStopDistance
+      - name: geozone_no_way_home_action
+        description: "Action if RTH with active geozones is unable to calculate a course to home"
+        default_value: RTH
+        field: noWayHomeAction
+        table: geozone_rth_no_way_home
+        type: uint8_t    

src/main/io/osd.c

@@ -866,6 +866,14 @@ static const char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM);
         case ARMING_DISABLED_DSHOT_BEEPER:
             return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER);
+
+        case ARMING_DISABLED_GEOZONE:
+#ifdef USE_GEOZONE
+            return OSD_MESSAGE_STR(OSD_MSG_NFZ);
+#else
+            FALLTHROUGH;
+#endif
+
             // Cases without message
         case ARMING_DISABLED_LANDING_DETECTED:
             FALLTHROUGH;
@@ -2381,6 +2389,11 @@ static bool osdDrawSingleElement(uint8_t item)
             if (FLIGHT_MODE(NAV_FW_AUTOLAND))
                 p = "LAND";
             else
+#endif
+#ifdef USE_GEOZONE
+            if (FLIGHT_MODE(NAV_SEND_TO))
+                p = "AUTO";
+            else 
 #endif
             if (FLIGHT_MODE(FAILSAFE_MODE))
                 p = "!FS!";
@@ -3889,6 +3902,52 @@ static bool osdDrawSingleElement(uint8_t item)
             clearMultiFunction = true;
             break;
         }
+#if defined(USE_GEOZONE)
+        case OSD_COURSE_TO_FENCE:
+        {
+            if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
+                int16_t panHomeDirOffset = 0;
+                if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
+                    panHomeDirOffset = osdGetPanServoOffset();
+                }
+                int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
+                int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset;
+                osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);            
+            } else {
+                if (isGeozoneActive()) {
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+                }
+                displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr);
+            }
+        break;
+        }  
+        
+        case OSD_H_DIST_TO_FENCE:
+        {
+            if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
+                char buff2[12];
+                osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0, 3);
+                tfp_sprintf(buff, "FD %s", buff2 );
+            } else {
+                strcpy(buff, "FD ---");
+            }
+        }
+        break;
+
+        case OSD_V_DIST_TO_FENCE:
+        {
+            if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
+                char buff2[12];
+                osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone));
+                tfp_sprintf(buff, "FD%s", buff2);
+                displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr);
+            } else {
+                strcpy(buff, "FD ---");
+            }
+
+            break;
+        }
+#endif
 
     default:
         return false;
@@ -5946,6 +6005,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     messages[messageCount++] = safehomeMessage;
                 }
 #endif
+
+#ifdef USE_GEOZONE
+                if (geozone.avoidInRTHInProgress) {
+                    messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH;
+                }
+#endif
                 if (FLIGHT_MODE(FAILSAFE_MODE)) {   // In FS mode while armed
                     if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
                         uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
@@ -5970,6 +6035,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             } else if (STATE(LANDING_DETECTED)) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else {
+#ifdef USE_GEOZONE
+            char buf[12], buf1[12];
+            switch (geozone.messageState) {
+                case GEOZONE_MESSAGE_STATE_NFZ:
+                    messages[messageCount++] = OSD_MSG_NFZ;
+                    break;
+                case GEOZONE_MESSAGE_STATE_LEAVING_FZ:      
+                    osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
+                    tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
+                    messages[messageCount++] = messageBuf;
+                    break;
+                case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
+                    messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
+                    break;
+                case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:    
+                osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
+                    if (geozone.zoneInfo == INT32_MAX) {
+                        tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
+                    } else {
+                        osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
+                    }
+                    tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
+                    messages[messageCount++] = messageBuf;
+                    break;
+                case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
+                    messages[messageCount++] = OSD_MSG_AVOIDING_FB;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
+                    messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
+                    messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
+                    messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_POS_HOLD:
+                    messages[messageCount++] = OSD_MSG_AVOIDING_FB;
+                    if (!geozone.sticksLocked) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;         
+                case GEOZONE_MESSAGE_STATE_NONE:
+                    break;
+            }
+#endif
                 /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
                 /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
                 if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
@@ -6008,6 +6131,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
+
                 } else if (STATE(MULTIROTOR)) {     /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
                     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
                         if (posControl.cruise.multicopterSpeed >= 50.0f) {

src/main/io/osd.h

@@ -132,6 +132,19 @@
 #define OSD_MSG_LOITERING_SAFEHOME  "LOITERING AROUND SAFEHOME"
 #endif
 
+#if defined(USE_GEOZONE)
+#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_AVOIDING_FB         "AVOIDING FENCE BREACH"
+#define OSD_MSG_RETURN_TO_ZONE      "RETURN TO FZ"
+#define OSD_MSG_FLYOUT_NFZ          "FLY OUT NFZ"
+#define OSD_MSG_AVOIDING_ALT_BREACH "REACHED ZONE ALTITUDE LIMIT"
+#define OSD_MSG_AVOID_ZONES_RTH     "AVOIDING NO FLY ZONES"
+#define OSD_MSG_GEOZONE_ACTION      "PERFORM ACTION IN %s %s"
+#endif
+
 typedef enum {
     OSD_RSSI_VALUE,
     OSD_MAIN_BATT_VOLTAGE,
@@ -296,6 +309,9 @@ typedef enum {
     OSD_RX_POWER_DOWNLINK, // 160
     OSD_RX_BAND,
     OSD_RX_MODE,
+    OSD_COURSE_TO_FENCE,
+    OSD_H_DIST_TO_FENCE,
+    OSD_V_DIST_TO_FENCE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -478,6 +494,10 @@ typedef struct osdConfig_s {
     uint16_t adsb_ignore_plane_above_me_limit;          // in metres
 #endif
     uint8_t  radar_peers_display_time;                  // in seconds
+#ifdef USE_GEOZONE
+    uint8_t geozoneDistanceWarning;                     // Distance to fence or action
+    bool geozoneDistanceType;                            // Shows a countdown timer or distance to fence/action
+#endif
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/io/osd_dji_hd.c

@@ -525,6 +525,8 @@ static char * osdArmingDisabledReasonMessage(void)
         case ARMING_DISABLED_DSHOT_BEEPER:
             return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
             // Cases without message
+        case ARMING_DISABLED_GEOZONE:
+            return OSD_MESSAGE_STR("NO FLY ZONE");
         case ARMING_DISABLED_LANDING_DETECTED:
             FALLTHROUGH;
         case ARMING_DISABLED_CMS_MENU:

src/main/msp/msp_protocol_v2_inav.h

@@ -114,4 +114,9 @@
 #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2102
 
 #define MSP2_INAV_SERVO_CONFIG                  0x2200
-#define MSP2_INAV_SET_SERVO_CONFIG              0x2201
+#define MSP2_INAV_SET_SERVO_CONFIG              0x2201
+
+#define MSP2_INAV_GEOZONE                      0x2210
+#define MSP2_INAV_SET_GEOZONE                  0x2211
+#define MSP2_INAV_GEOZONE_VERTEX               0x2212
+#define MSP2_INAV_SET_GEOZONE_VERTEX           0x2213

src/main/navigation/navigation.c

@@ -356,6 +356,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
 #endif
+#ifdef USE_GEOZONE
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState); 
+#endif
 
 static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
     /** Idle state ******************************************************/
@@ -377,6 +382,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]              = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -449,6 +455,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
     /** CRUISE_HOLD mode ************************************************/
@@ -485,6 +492,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -507,6 +515,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -544,6 +553,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -566,6 +576,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -637,15 +648,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwState = MW_NAV_STATE_RTH_ENROUTE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HEAD_HOME,           // re-process the state
+            [NAV_FSM_EVENT_TIMEOUT]                             = NAV_STATE_RTH_HEAD_HOME,           // re-process the state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
+            [NAV_FSM_EVENT_SUCCESS]                             = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                      = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]                   = NAV_STATE_ALTHOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]                = NAV_STATE_POSHOLD_3D_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]         = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]               = NAV_STATE_COURSE_HOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]                    = NAV_STATE_CRUISE_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]              = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]                   = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE]  = NAV_STATE_RTH_INITIALIZE,
         }
     },
 
@@ -1178,6 +1190,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 #endif
+
+#ifdef USE_GEOZONE
+    [NAV_STATE_SEND_TO_INITALIZE] = {
+        .persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE,
+        .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE,
+        .timeoutMs = 0,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_SEND_TO,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_SEND_TO_IN_PROGESS,
+            [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+        } 
+    }, 
+
+    [NAV_STATE_SEND_TO_IN_PROGESS] = {
+        .persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES,
+        .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS,
+        .timeoutMs = 10,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_SEND_TO,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_SEND_TO_IN_PROGESS,   // re-process the state
+            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_SEND_TO_FINISHED, 
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+        }
+    },
+     [NAV_STATE_SEND_TO_FINISHED] = {
+        .persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED,
+        .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED,
+        .timeoutMs = 0,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_SEND_TO,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_SEND_TO_FINISHED,   // re-process the state
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+        }
+    },
+#endif
 };
 
 static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
@@ -1444,6 +1513,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
     // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
     if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
         // Prepare controllers
+#ifdef USE_GEOZONE
+        geozoneResetRTH();
+        geozoneSetupRTH();
+#endif
         resetPositionController();
         resetAltitudeController(false);     // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
         setupAltitudeController();
@@ -1610,22 +1683,55 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
 
     // If we have position sensor - continue home
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
-        fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
+#ifdef USE_GEOZONE
-
+        // Check for NFZ in our way
-        if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
+        int8_t wpCount = geozoneCheckForNFZAtCourse(true);
-            // Successfully reached position target - update XYZ-position
+        if (wpCount > 0) {
-            setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+            calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
-
-            posControl.landingDelay = 0;
-
-            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;
+        } else if (geozone.avoidInRTHInProgress) {
+            if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {                
+                if (geoZoneIsLastRthWaypoint()) {
+                    // Already at Home?
+                    fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
+                    if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
+                        setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+                        return NAV_FSM_EVENT_SUCCESS;
+                    }
+                    
+                    posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
+                    return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
+                } else {
+                    geozoneAdvanceRthAvoidWaypoint();
+                    calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
+                    return NAV_FSM_EVENT_NONE;
+                }
+            } 
+            setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+            return NAV_FSM_EVENT_NONE;
+        } else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) {
+            return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
+        } else {
+#endif
+            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);
+
+                posControl.landingDelay = 0;
+
+                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;
+            }
+#ifdef USE_GEOZONE
         }
+#endif
     }
     /* Position sensor failure timeout - land */
     else if (checkForPositionSensorTimeout()) {
@@ -1797,6 +1903,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
         updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT);  // FIXME
     }
 
+#ifdef USE_GEOZONE
+    geozoneResetRTH();
+#endif
+
     // Prevent I-terms growing when already landed
     pidResetErrorAccumulators();
     return NAV_FSM_EVENT_NONE;
@@ -2452,6 +2562,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
 }
 #endif
 
+#ifdef USE_GEOZONE
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+    
+    if (checkForPositionSensorTimeout()) {
+        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
+    }
+
+    resetPositionController();
+    resetAltitudeController(false);
+
+     if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
+        setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
+    } else {
+        setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+    }
+
+    posControl.sendTo.startTime = millis();
+    return NAV_FSM_EVENT_SUCCESS;
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+
+    // "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately
+    if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) {
+        posControl.sendTo.lockSticks = false;
+    }
+
+    if (!posControl.sendTo.lockSticks && areSticksDeflected()) {
+        abortSendTo();
+        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
+    }
+
+    if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) {
+        if (posControl.sendTo.altitudeTargetRange > 0) {
+            if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z +  posControl.sendTo.altitudeTargetRange)) {
+                return NAV_FSM_EVENT_SUCCESS;
+            } else {
+                return NAV_FSM_EVENT_NONE;
+            }
+        }
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+    return NAV_FSM_EVENT_NONE;
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+    posControl.sendTo.lockSticks = false;
+    posControl.flags.sendToActive = false;
+    return NAV_FSM_EVENT_NONE;
+}
+#endif
+
 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
 {
     navigationFSMState_t previousState;
@@ -2539,6 +2707,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
     switch (mode) {
         case RTH_HOME_ENROUTE_INITIAL:
             posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
+#ifdef USE_GEOZONE
+            if (geozone.currentzoneMaxAltitude > 0) {
+                posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z);
+            }
+#endif
             break;
 
         case RTH_HOME_ENROUTE_PROPORTIONAL:
@@ -2552,6 +2725,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
                     posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
                 }
             }
+#ifdef USE_GEOZONE
+            if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) {
+                posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance;
+            }
+#endif
             break;
 
         case RTH_HOME_ENROUTE_FINAL:
@@ -2757,6 +2935,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
     return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
 }
 
+/*-----------------------------------------------------------
+ * Calculates 2D distance between two points
+ *-----------------------------------------------------------*/
+float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos)
+{
+	const float deltaX = destinationPos->x - startPos->x;
+	const float deltaY = destinationPos->y - startPos->y;
+
+	return calc_length_pythagorean_2D(deltaX, deltaY);
+}
+
 /*-----------------------------------------------------------
  * Calculates distance and bearing to destination point
  *-----------------------------------------------------------*/
@@ -2944,6 +3133,11 @@ static void updateDesiredRTHAltitude(void)
         posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
         posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
     }
+#if defined(USE_GEOZONE)
+    if (geozone.homeHasMaxAltitue) {
+        posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude);
+    }
+#endif
 }
 
 /*-----------------------------------------------------------
@@ -3154,6 +3348,9 @@ void updateHomePosition(void)
                     setHome = true;
                     break;
             }
+#ifdef USE_GEOZONE
+            geozoneUpdateMaxHomeAltitude();
+#endif
         }
     }
     else {
@@ -3409,8 +3606,16 @@ bool isProbablyStillFlying(void)
  * Z-position controller
  *-----------------------------------------------------------*/
 float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
-{
+{    
+    
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
+    
+#ifdef USE_GEOZONE
+    if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || 
+        (geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) {
+        return 0.0f;
+    }
+#endif
     float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
 
     if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
@@ -4050,6 +4255,16 @@ static void processNavigationRCAdjustments(void)
     /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
 
+#ifdef USE_GEOZONE
+    if (geozone.sticksLocked) {
+        posControl.flags.isAdjustingAltitude = false;
+        posControl.flags.isAdjustingPosition = false;
+        posControl.flags.isAdjustingHeading = false;
+
+        return;
+    }
+#endif
+
     if (FLIGHT_MODE(FAILSAFE_MODE)) {
         if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
             resetMulticopterBrakingMode();
@@ -4095,6 +4310,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
         posControl.activeWaypointIndex = posControl.startWpIndex;
         // Reset RTH trackback
         resetRthTrackBack();
+    
+#ifdef USE_GEOZONE
+        posControl.flags.sendToActive = false;
+#endif
 
         return;
     }
@@ -4145,7 +4364,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
 void switchNavigationFlightModes(void)
 {
     const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
-    const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
+    const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes);
     DISABLE_FLIGHT_MODE(disabledFlightModes);
     ENABLE_FLIGHT_MODE(enabledNavFlightModes);
 }
@@ -4296,6 +4515,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             return NAV_FSM_EVENT_SWITCH_TO_RTH;
         }
 
+#ifdef USE_GEOZONE
+        if (posControl.flags.sendToActive) {
+            return NAV_FSM_EVENT_SWITCH_TO_SEND_TO;
+        }
+
+
+        if (posControl.flags.forcedPosholdActive) {
+            return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
+        }
+#endif
+
         /* WP mission activation control:
          * canActivateWaypoint & waypointWasActivated are used to prevent WP mission
          * auto restarting after interruption by Manual or RTH modes.
@@ -4769,6 +4999,14 @@ void navigationInit(void)
 #ifdef USE_MULTI_MISSION
     posControl.multiMissionCount = 0;
 #endif
+
+#ifdef USE_GEOZONE
+    posControl.flags.sendToActive = false;
+    posControl.sendTo.lockSticks = false;
+    posControl.sendTo.lockStickTime = 0;
+    posControl.sendTo.startTime = 0;
+    posControl.sendTo.targetRange = 0;
+#endif
     /* Set initial surface invalid */
     posControl.actualState.surfaceMin = -1.0f;
 
@@ -4851,6 +5089,40 @@ rthState_e getStateOfForcedRTH(void)
     }
 }
 
+
+#ifdef USE_GEOZONE
+// "Send to" is not to intended to be activated by user, only by external event
+void activateSendTo(void)
+{
+    if (!geozone.avoidInRTHInProgress) {
+        abortFixedWingLaunch();
+        posControl.flags.sendToActive = true;
+        navProcessFSMEvents(selectNavEventFromBoxModeInput());
+    }
+}
+
+void abortSendTo(void)
+{
+    posControl.flags.sendToActive = false;
+    navProcessFSMEvents(selectNavEventFromBoxModeInput());
+}
+
+void activateForcedPosHold(void)
+{
+    if (!geozone.avoidInRTHInProgress) { 
+        abortFixedWingLaunch();
+        posControl.flags.forcedPosholdActive = true;
+        navProcessFSMEvents(selectNavEventFromBoxModeInput());
+    }
+}
+
+void abortForcedPosHold(void)
+{
+    posControl.flags.forcedPosholdActive = false;
+    navProcessFSMEvents(selectNavEventFromBoxModeInput());
+}
+#endif
+
 /*-----------------------------------------------------------
  * Ability to execute Emergency Landing on external event
  *-----------------------------------------------------------*/

src/main/navigation/navigation.h

@@ -116,6 +116,123 @@ void resetFwAutolandApproach(int8_t idx);
 
 #endif
 
+#if defined(USE_GEOZONE)
+
+#ifndef USE_GPS
+    #error "Geozone needs GPS support"
+#endif
+
+typedef enum {
+    GEOZONE_MESSAGE_STATE_NONE,
+    GEOZONE_MESSAGE_STATE_NFZ,
+    GEOZONE_MESSAGE_STATE_LEAVING_FZ,
+    GEOZONE_MESSAGE_STATE_OUTSIDE_FZ,
+    GEOZONE_MESSAGE_STATE_ENTERING_NFZ,
+    GEOZONE_MESSAGE_STATE_AVOIDING_FB,
+    GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE,
+    GEOZONE_MESSAGE_STATE_FLYOUT_NFZ,
+    GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH,
+    GEOZONE_MESSAGE_STATE_POS_HOLD
+} geozoneMessageState_e;
+
+enum fenceAction_e {
+    GEOFENCE_ACTION_NONE,
+    GEOFENCE_ACTION_AVOID,
+    GEOFENCE_ACTION_POS_HOLD,
+    GEOFENCE_ACTION_RTH,
+};
+
+enum noWayHomeAction {
+    NO_WAY_HOME_ACTION_RTH,
+    NO_WAY_HOME_ACTION_EMRG_LAND,
+};
+
+#define GEOZONE_SHAPE_CIRCULAR 0
+#define GEOZONE_SHAPE_POLYGON  1
+
+#define GEOZONE_TYPE_EXCLUSIVE 0
+#define GEOZONE_TYPE_INCLUSIVE 1
+
+typedef struct geoZoneConfig_s
+{
+    uint8_t shape;
+    uint8_t type;
+    int32_t minAltitude;
+    int32_t maxAltitude;
+    bool isSealevelRef;
+    uint8_t fenceAction;
+    uint8_t vertexCount;
+} geoZoneConfig_t;
+
+typedef struct geozone_config_s
+{
+    uint32_t fenceDetectionDistance;
+    uint16_t avoidAltitudeRange;
+    uint16_t safeAltitudeDistance;
+    bool nearestSafeHomeAsInclusivZone;
+    uint8_t safeHomeFenceAction;
+    uint32_t copterFenceStopDistance;
+    uint8_t noWayHomeAction;
+} geozone_config_t;
+
+typedef struct vertexConfig_s
+{
+    int8_t zoneId;
+    uint8_t idx;
+    int32_t lat;
+    int32_t lon;
+} vertexConfig_t;
+
+PG_DECLARE(geozone_config_t, geoZoneConfig);
+PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig);
+PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices);
+
+typedef struct geozone_s {
+    bool insideFz;
+    bool insideNfz;
+    uint32_t distanceToZoneBorder3d;
+    int32_t vertDistanceToZoneBorder;
+    geozoneMessageState_e messageState;
+    int32_t directionToNearestZone;
+    int32_t distanceHorToNearestZone;
+    int32_t distanceVertToNearestZone;
+    int32_t zoneInfo;
+    int32_t currentzoneMaxAltitude; 
+    int32_t currentzoneMinAltitude;
+    bool nearestHorZoneHasAction;
+    bool sticksLocked;
+    int8_t loiterDir;
+    bool avoidInRTHInProgress;
+    int32_t maxHomeAltitude;
+    bool homeHasMaxAltitue;
+} geozone_t;
+
+extern geozone_t geozone;
+
+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 geozoneIsBlockingArming(void);
+void geozoneAdvanceRthAvoidWaypoint(void);
+int8_t geozoneCheckForNFZAtCourse(bool isRTH);
+bool geoZoneIsLastRthWaypoint(void);
+fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void);
+void geozoneSetupRTH(void);
+void geozoneResetRTH(void);
+void geozoneUpdateMaxHomeAltitude(void);
+uint32_t geozoneGetDetectionDistance(void);
+
+void activateSendTo(void);
+void abortSendTo(void);
+void activateForcedPosHold(void);
+void abortForcedPosHold(void);
+
+#endif
+
 #ifndef NAV_MAX_WAYPOINTS
 #define NAV_MAX_WAYPOINTS 15
 #endif

src/main/navigation/navigation_fixedwing.c

@@ -250,6 +250,12 @@ static int8_t loiterDirection(void) {
         dir *= -1;
     }
 
+#ifdef USE_GEOZONE
+    if (geozone.loiterDir != 0) {
+        dir = geozone.loiterDir;
+    }
+#endif
+
     return dir;
 }
 

src/main/navigation/navigation_private.h

@@ -113,6 +113,11 @@ typedef struct navigationFlags_s {
     bool rthTrackbackActive;                // Activation status of RTH trackback
     bool wpTurnSmoothingActive;             // Activation status WP turn smoothing
     bool manualEmergLandActive;             // Activation status of manual emergency landing
+
+#ifdef USE_GEOZONE
+    bool sendToActive;
+    bool forcedPosholdActive;
+#endif
 } navigationFlags_t;
 
 typedef struct {
@@ -160,6 +165,7 @@ typedef enum {
     NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ,
     NAV_FSM_EVENT_SWITCH_TO_MIXERAT,
     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING,
+    NAV_FSM_EVENT_SWITCH_TO_SEND_TO,
 
     NAV_FSM_EVENT_STATE_SPECIFIC_1,             // State-specific event
     NAV_FSM_EVENT_STATE_SPECIFIC_2,             // State-specific event
@@ -245,6 +251,10 @@ typedef enum {
     NAV_PERSISTENT_ID_FW_LANDING_FLARE                          = 46,
     NAV_PERSISTENT_ID_FW_LANDING_ABORT                          = 47,
     NAV_PERSISTENT_ID_FW_LANDING_FINISHED                       = 48,
+
+    NAV_PERSISTENT_ID_SEND_TO_INITALIZE                         = 49,
+    NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES                        = 50,
+    NAV_PERSISTENT_ID_SEND_TO_FINISHED                          = 51
 } navigationPersistentId_e;
 
 typedef enum {
@@ -304,6 +314,10 @@ typedef enum {
     NAV_STATE_MIXERAT_IN_PROGRESS,
     NAV_STATE_MIXERAT_ABORT,
 
+    NAV_STATE_SEND_TO_INITALIZE,
+    NAV_STATE_SEND_TO_IN_PROGESS,
+    NAV_STATE_SEND_TO_FINISHED,
+
     NAV_STATE_COUNT,
 } navigationFSMState_t;
 
@@ -406,6 +420,17 @@ typedef enum {
     RTH_HOME_FINAL_LAND,            // Home position and altitude
 } rthTargetMode_e;
 
+#ifdef USE_GEOZONE
+typedef struct navSendTo_s {
+    fpVector3_t targetPos;
+    uint16_t altitudeTargetRange;   // 0 for only "2D" 
+    uint32_t targetRange;
+    bool lockSticks;
+    uint32_t lockStickTime;
+    timeMs_t startTime;
+} navSendTo_t;
+#endif
+
 typedef struct {
     fpVector3_t nearestSafeHome;    // The nearestSafeHome found during arming
     uint32_t    distance;           // distance to the nearest safehome
@@ -478,6 +503,10 @@ typedef struct {
     fwLandState_t fwLandState;
 #endif
 
+#ifdef USE_GEOZONE
+    navSendTo_t                  sendTo; // Used for Geozones
+#endif
+
     /* Internals & statistics */
     int16_t                     rcAdjustment[4];
     float                       totalTripDistance;
@@ -502,7 +531,9 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 
 bool isThrustFacingDownwards(void);
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
+void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
+float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos);
 
 bool isLandingDetected(void);
 void resetLandingDetector(void);

src/main/scheduler/scheduler.h

@@ -137,6 +137,10 @@ typedef enum {
     TASK_TELEMETRY_SBUS2,
 #endif
 
+#if defined (USE_GEOZONE) && defined(USE_GPS)
+    TASK_GEOZONE,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/target/SITL/target.h

@@ -80,6 +80,9 @@
 #define USE_HEADTRACKER_MSP
 
 #undef USE_DASHBOARD
+#define USE_GEOZONE
+#define MAX_GEOZONES_IN_CONFIG 63
+#define MAX_VERTICES_IN_CONFIG 126
 
 #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!?
 #undef USE_VCP

src/main/target/common.h

@@ -209,6 +209,11 @@
 #define USE_34CHANNELS
 #define MAX_MIXER_PROFILE_COUNT 2
 #define USE_SMARTPORT_MASTER
+#ifdef USE_GPS
+#define USE_GEOZONE
+#define MAX_GEOZONES_IN_CONFIG 63
+#define MAX_VERTICES_IN_CONFIG 126
+#endif
 #elif !defined(STM32F7)
 #define MAX_MIXER_PROFILE_COUNT 1
 #endif
@@ -221,3 +226,4 @@
 
 #define USE_EZ_TUNE
 #define USE_ADAPTIVE_FILTER
+