Geozones (aka Geofence)

.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 }"
 
-}
+}

cmake/sitl.cmake

@@ -69,6 +69,7 @@ if(NOT MACOSX)
     endif()
 else()
     set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
+        -Wno-error=double-promotion
     )
 endif()
 

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