From ec46d9ee917e9b4781a4bf6cac186c8bbdf1a7f0 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sat, 2 Nov 2024 10:51:48 -0300 Subject: [PATCH 01/17] Inital commit --- .vscode/c_cpp_properties.json | 16 +- .vscode/settings.json | 8 +- .vscode/tasks.json | 61 +- src/main/CMakeLists.txt | 1 + src/main/common/vector.h | 83 + src/main/config/parameter_group_ids.h | 5 +- src/main/fc/cli.c | 258 ++- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 8 + src/main/fc/fc_msp.c | 98 + src/main/fc/fc_tasks.c | 23 + src/main/fc/runtime_config.h | 9 +- src/main/fc/settings.yaml | 52 + src/main/io/osd.c | 119 +- src/main/io/osd.h | 20 + src/main/io/osd_dji_hd.c | 2 + src/main/msp/msp_protocol_v2_inav.h | 7 +- src/main/navigation/navigation.c | 261 ++- src/main/navigation/navigation.h | 113 ++ src/main/navigation/navigation_fixedwing.c | 6 + src/main/navigation/navigation_geozone.c | 1598 +++++++++++++++++ .../navigation_geozone_calculations.h | 480 +++++ src/main/navigation/navigation_private.h | 31 + src/main/scheduler/scheduler.h | 4 + src/main/target/common.h | 1 + 25 files changed, 3243 insertions(+), 23 deletions(-) create mode 100755 src/main/navigation/navigation_geozone.c create mode 100755 src/main/navigation/navigation_geozone_calculations.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3a8d8d1c8ca..35984a9998c 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -4,7 +4,8 @@ "name": "Linux", "includePath": [ "${workspaceRoot}", - "${workspaceRoot}/src/main/**" + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/debug_SITL/src/main/target/SITL/SITL/*" ], "browse": { "limitSymbolsToIncludedHeaders": false, @@ -12,10 +13,14 @@ "${workspaceRoot}/**" ] }, - "intelliSenseMode": "msvc-x64", + "intelliSenseMode": "gcc-arm", "cStandard": "c11", "cppStandard": "c++17", "defines": [ + "uint16_t=unsigned short", + "int16_t=short", + "uint32_t=unsigned int", + "MCU_FLASH_SIZE=1024", "USE_OSD", "USE_GYRO_NOTCH_1", "USE_GYRO_NOTCH_2", @@ -35,9 +40,10 @@ "USE_PCF8574", "USE_ESC_SENSOR", "USE_ADAPTIVE_FILTER", - "MCU_FLASH_SIZE 1024", - ], - "configurationProvider": "ms-vscode.cmake-tools" + "SITL_BUILD", + "USE_GEOZONE" + ] + //"configurationProvider": "ms-vscode.cmake-tools" } ], "version": 4 diff --git a/.vscode/settings.json b/.vscode/settings.json index d1eb2357c02..f5f2ba120c9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,10 +4,14 @@ "cmath": "c", "ranges": "c", "navigation.h": "c", - "rth_trackback.h": "c" + "rth_trackback.h": "c", "platform.h": "c", "timer.h": "c", - "bus.h": "c" + "bus.h": "c", + "controlrate_profile.h": "c", + "settings.h": "c", + "settings_generated.h": "c", + "parameter_group.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 7be2cd3d176..9c109097adc 100755 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,9 +4,9 @@ "version": "2.0.0", "tasks": [ { - "label": "Build AOCODARCH7DUAL", + "label": "Build Matek F722-SE", "type": "shell", - "command": "make AOCODARCH7DUAL", + "command": "make MATEKF722SE", "group": "build", "problemMatcher": [], "options": { @@ -14,12 +14,11 @@ } }, { - "label": "Build AOCODARCH7DUAL", + "label": "Build Matek F722", "type": "shell", - "command": "make AOCODARCH7DUAL", + "command": "make MATEKF722", "group": { - "kind": "build", - "isDefault": true + "kind": "build" }, "problemMatcher": [], "options": { @@ -36,6 +35,56 @@ "options": { "cwd": "${workspaceFolder}/build" } + }, + { + "label": "CMAKE Build SITL", + "type": "shell", + "command": "mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "CMAKE Debug SITL", + "type": "shell", + "command": "mkdir -p debug_SITL && cd debug_SITL && cmake -DCMAKE_BUILD_TYPE=Debug -DSITL=ON ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "CMAKE Release SITL", + "type": "shell", + "command": "mkdir -p release_SITL && cd release_SITL && cmake -DCMAKE_BUILD_TYPE=Release -DSITL=ON ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "Debug SITL", + "type": "shell", + "command": "make", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/debug_SITL" + } + }, + { + "label": "Release SITL", + "type": "shell", + "command": "make", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/release_SITL" + } } ] } \ No newline at end of file diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e10985..7f188974cd9 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 449a0973b3d..737c29d56a7 100644 --- a/src/main/common/vector.h +++ b/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; +} diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index b4062201c72..26715ab3cec 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ee5cb289a94..87db75cb509 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -152,7 +152,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", - "", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", @@ -1561,6 +1561,251 @@ 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"; + 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; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction); + } +} + +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; + geoZonesConfigMutable(zoneId)->vertexCount++; + + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + + } else { + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 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++; + 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++; + } + + if (argumentCount != 6) { + 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)->fenceAction = fenceAction; + } +} +#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) { @@ -4167,6 +4412,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()); @@ -4550,6 +4803,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), diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ddb6a826b83..17c8ded8c1e 100644 --- a/src/main/fc/config.h +++ b/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, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f7603eee692..ba29a03d5e0 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,6 +278,14 @@ 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); + } +#endif + /* CHECK: */ if ( sensors(SENSOR_ACC) && diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e7a7a4aa3a6..201f2b11bfc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1768,6 +1768,53 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) +static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, geoZonesConfig(idx)->type); + sbufWriteU8(dst, geoZonesConfig(idx)->shape); + sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); + sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); + sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); + sbufWriteU8(dst, idx); + 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) { @@ -3301,6 +3348,49 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; +#if defined(USE_GEOZONE) && defined (USE_GPS) + case MSP2_INAV_SET_GEOZONE: + if (dataSize == 13) { + 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)->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: @@ -3862,6 +3952,14 @@ 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) + 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 diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db526..df335ccb428 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -339,6 +339,15 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } +#if defined(USE_GEOZONE) && defined (USE_GPS) +void geozoneUpdateTask(timeUs_t currentTimeUs) +{ + if (feature(FEATURE_GEOZONE)) { + geozoneUpdate(currentTimeUs); + } +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -453,6 +462,11 @@ void fcTasksInit(void) #if defined(SITL_BUILD) serialProxyStart(); #endif + +#if defined(USE_GEOZONE) && defined (USE_GPS) + setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); +#endif + } cfTask_t cfTasks[TASK_COUNT] = { @@ -740,4 +754,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + [TASK_GEOZONE] = { + .taskName = "GEOZONE", + .taskFunc = geozoneUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(5), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 974f90f8c4d..637ea867e64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -24,6 +24,7 @@ typedef enum { 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 +50,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_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | + 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 +66,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 +108,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; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 74f525ce5f1..8cc9b1232dd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,6 +198,11 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_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 @@ -3776,6 +3781,7 @@ groups: condition: USE_OSD || USE_DJI_HD_OSD members: - name: osd_speed_source + condition: USE_GEOZONE description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" default_value: "GROUND" field: speedSource @@ -4314,3 +4320,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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fff22479f06..efcfa394f65 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,6 +866,8 @@ 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: + return OSD_MESSAGE_STR(OSD_MSG_NFZ); // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; @@ -2336,6 +2338,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!"; @@ -3797,6 +3804,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); + 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; @@ -4998,7 +5051,7 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX) uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; - char outBuff[12]; + char outBuff[20]; const float max_gforce = accGetMeasuredMaxG(); const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); @@ -5784,6 +5837,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()); @@ -5846,6 +5905,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } +#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); + 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); + 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_LOITER: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a73db015793..fd0f4028bb5 100644 --- a/src/main/io/osd.h +++ b/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, @@ -287,6 +300,9 @@ typedef enum { OSD_ADSB_INFO, OSD_BLACKBOX, OSD_FORMATION_FLIGHT, //153 + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -467,6 +483,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); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b018d8f9068..c0120c7faa1 100644 --- a/src/main/io/osd_dji_hd.c +++ b/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: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 8b4a09e87af..b8c808cb904 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -112,4 +112,9 @@ #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2101 #define MSP2_INAV_SERVO_CONFIG 0x2200 -#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 \ No newline at end of file +#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 \ No newline at end of file diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4ae4cf228db..bbf8e7910f0 100644 --- a/src/main/navigation/navigation.c +++ b/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, } }, @@ -1178,6 +1189,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 +1512,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,6 +1682,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { +#ifdef USE_GEOZONE + // Check for NFZ in our way + int8_t wpCount = geozoneCheckForNFZAtCourse(true); + if (wpCount > 0) { + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + 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)) { @@ -1626,6 +1728,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio 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 +1902,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 +2561,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 +2706,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 +2724,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 +2934,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 +3132,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 +3347,9 @@ void updateHomePosition(void) setHome = true; break; } +#if defined(USE_GEOZONE) && defined (USE_GPS) + geozoneUpdateMaxHomeAltitude(); +#endif } } else { @@ -3409,7 +3605,14 @@ bool isProbablyStillFlying(void) * Z-position controller *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) -{ +{ +#ifdef USE_GEOZONE + if ((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 + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; @@ -4095,6 +4298,10 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; +#endif return; } @@ -4145,7 +4352,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 +4503,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } + if (posControl.flags.sendToActive) { + return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; + } + + if (posControl.flags.forcedPosholdActive) { + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + } + /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission * auto restarting after interruption by Manual or RTH modes. @@ -4769,6 +4984,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 +5074,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 *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b13..6e2ca8fbcbc 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -116,6 +116,119 @@ void resetFwAutolandApproach(int8_t idx); #endif +#if defined(USE_GEOZONE) + +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 + +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_LOITER +} 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; + 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 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 geozoneResetVertices(int8_t zoneId, int16_t idx); +void geozoneUpdate(timeUs_t curentTimeUs); +bool geozoneIsInsideNFZ(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 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ff38b2e17e5..87165cd7d6f 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/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; } diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c new file mode 100755 index 00000000000..8a2505a0ff9 --- /dev/null +++ b/src/main/navigation/navigation_geozone.c @@ -0,0 +1,1598 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#include "common/utils.h" +#include "common/vector.h" +#include "common/printf.h" + +#include "config/config_reset.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" + +#include "flight/imu.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#if defined(USE_GEOZONE) && defined (USE_GPS) + +#include "navigation_geozone_calculations.h" + +#define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) +#define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home + +#define MAX_DISTANCE_FLY_OVER_POINTS 50000 +#define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) +#define POS_DETECTION_DISTANCE 7500 +#define STICK_LOCK_MIN_TIME 1500 +#define AVOID_TIMEOUT 30000 +#define MAX_LOCAL_VERTICES 128 +#define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m +#define STICK_MOVE_THRESHOULD 40 +#define MAX_RTH_WAYPOINTS (MAX_VERTICES / 2) +#define GEOZONE_INACTIVE INT8_MAX +#define RTH_OVERRIDE_TIMEOUT 1000 + +#define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) + +typedef enum { + GEOZONE_ACTION_STATE_NONE, + GEOZONE_ACTION_STATE_AVOIDING, + GEOZONE_ACTION_STATE_AVOIDING_UPWARD, + GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE, + GEOZONE_ACTION_STATE_RETURN_TO_FZ, + GEOZONE_ACTION_STATE_FLYOUT_NFZ, + GEOZONE_ACTION_STATE_POSHOLD, + GEOZONE_ACTION_STATE_RTH +} geozoneActionState_e; + +typedef struct geoZoneRuntimeConfig_s +{ + geoZoneConfig_t config; + bool enable; + bool isInfZone; + uint32_t radius; + fpVector2_t *verticesLocal; +} geoZoneRuntimeConfig_t; + +typedef struct pathPoint_s pathPoint_t; +struct pathPoint_s { + fpVector3_t point; + pathPoint_t* prev; + float distance; + bool visited; +}; + +static bool isInitalised = false; +static geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; +static fpVector2_t verticesLocal[MAX_VERTICES]; +static uint8_t currentZoneCount = 0; + +static bool isAtLeastOneInclusiveZoneActive = false; +static geoZoneRuntimeConfig_t activeGeoZones[MAX_GEOZONES]; +static uint8_t activeGeoZonesCount = 0; +static geoZoneConfig_t safeHomeGeozoneConfig; +static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; +static timeMs_t actionStartTime = 0; +static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; +static fpVector3_t avoidingPoint; +static bool geozoneIsEnabled = false; +static fpVector3_t rthWaypoints[MAX_RTH_WAYPOINTS]; +static uint8_t rthWaypointIndex = 0; +static int8_t rthWaypointCount = 0; +static bool aboveOrUnderZone = false; +static timeMs_t rthOverrideStartTime; +static bool noZoneRTH = false; +static bool rthHomeSwitchLastState = false; +static bool lockRTZ = false; + +geozone_t geozone; + +PG_REGISTER_WITH_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, PG_GEOZONE_CONFIG, 0); + +PG_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, + .fenceDetectionDistance = SETTING_GEOZONE_DETECTION_DISTANCE_DEFAULT, + .avoidAltitudeRange = SETTING_GEOZONE_AVOID_ALTITUDE_RANGE_DEFAULT, + .safeAltitudeDistance = SETTING_GEOZONE_SAFE_ALTITUDE_DISTANCE_DEFAULT, + .nearestSafeHomeAsInclusivZone = SETTING_GEOZONE_SAFEHOME_AS_INCLUSIVE_DEFAULT, + .copterFenceStopDistance = SETTING_GEOZONE_MR_STOP_DISTANCE_DEFAULT, + .noWayHomeAction = SETTING_GEOZONE_NO_WAY_HOME_ACTION_DEFAULT +); + +PG_REGISTER_ARRAY_WITH_RESET_FN(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig, PG_GEOZONES, 1); + +void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) +{ + for (int i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + RESET_CONFIG(geoZoneConfig_t, &instance[i], + .shape = GEOZONE_TYPE_EXCLUSIVE, + .type = GEOZONE_SHAPE_CIRCULAR, + .minAltitude = 0, + .maxAltitude = 0, + .fenceAction = GEOFENCE_ACTION_NONE, + .vertexCount = 0 + ); + } +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices, PG_GEOZONE_VERTICES, 0); + +void pgResetFn_geoZoneVertices(vertexConfig_t *instance) +{ + for (int i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + RESET_CONFIG(vertexConfig_t, &instance[i], + .zoneId = -1, + .idx = 0, + .lat = 0, + .lon = 0 + ); + } +} + +uint8_t geozoneGetUsedVerticesCount(void) +{ + uint8_t count = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + count += geoZonesConfig(i)->vertexCount; + } + return count; +} + +void geozoneResetVertices(const int8_t zoneId, const int16_t idx) +{ + if (zoneId < 0 && idx < 0) { + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + } + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx < 0) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx >= 0 && idx < MAX_VERTICES_IN_CONFIG) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == idx) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + break; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount--; + } + } +} + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon) +{ + if (vertexId < MAX_VERTICES_IN_CONFIG) + { + int16_t vertexConfigIdx = -1; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexConfigIdx = i; + break; + } + } + if (vertexConfigIdx >= 0) { + geoZoneVerticesMutable(vertexConfigIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexConfigIdx)->idx = vertexId; + geoZoneVerticesMutable(vertexConfigIdx)->lat = lat; + geoZoneVerticesMutable(vertexConfigIdx)->lon = lon; + return true; + } + } + return false; +} + +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) +{ + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexId) { + return i; + } + } + return -1; +} + +static bool areSticksDeflectdFromChannelValue(void) +{ + return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; +} + +static bool isNonGeozoneModeFromBoxInput(void) +{ + return !(IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)); +} + +static bool isPointOnBorder(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos) +{ + fpVector2_t *prev = &zone->verticesLocal[zone->config.vertexCount -1]; + fpVector2_t *current; + bool isOnBorder = false; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + current = &zone->verticesLocal[i]; + if (isPointOnLine2(prev, current, (fpVector2_t*)pos)) { + isOnBorder = true; + break; + } + prev = current; + } + + if (isOnBorder) { + return (pos->z >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos->z <= zone->config.maxAltitude; + } + + return isOnBorder; +} + +static bool isInZoneAltitudeRange(geoZoneRuntimeConfig_t *zone, const float pos) +{ + return (pos >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos <= zone->config.maxAltitude; +} + +static bool isInGeozone(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos, bool ignoreAltitude) +{ + if (activeGeoZonesCount == 0) { + return false; + } + + bool isIn2D = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + isIn2D = isPointInPloygon((fpVector2_t*)pos, zone->verticesLocal, zone->config.vertexCount) || isPointOnBorder(zone, pos); + } else { // cylindric + isIn2D = isPointInCircle((fpVector2_t*)pos, &zone->verticesLocal[0], zone->radius); + } + + if (isIn2D && !ignoreAltitude) { + return isInZoneAltitudeRange(zone, pos->z); + } + + return isIn2D; +} + +static bool isPointInAnyOtherZone(const geoZoneRuntimeConfig_t *zone, uint8_t type, const fpVector3_t *pos) +{ + bool isInZone = false; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (zone != &activeGeoZones[i] && activeGeoZones[i].config.type == type && isInGeozone(&activeGeoZones[i], pos, false)) { + isInZone = true; + break; + } + } + return isInZone; +} + +static uint8_t getZonesForPos(geoZoneRuntimeConfig_t *zones[], const fpVector3_t *pos, const bool ignoreAltitude) +{ + uint8_t count = 0; + for (int i = 0; i < activeGeoZonesCount; i++) { + if (isInGeozone(&activeGeoZones[i], pos, ignoreAltitude)) { + zones[count++] = &activeGeoZones[i]; + } + } + return count; +} + +static uint8_t getCurrentZones(geoZoneRuntimeConfig_t *zones[], const bool ignoreAltitude) +{ + return getZonesForPos(zones, &navGetCurrentActualPositionAndVelocity()->pos, ignoreAltitude); +} + +static int geoZoneRTComp(const void *a, const void *b) +{ + geoZoneRuntimeConfig_t *zoneA = (geoZoneRuntimeConfig_t*)a; + geoZoneRuntimeConfig_t *zoneB = (geoZoneRuntimeConfig_t*)b; + + if (zoneA->enable == zoneB->enable) { + return 0; + } else if (zoneA->enable) { + return -1; + } else { + return 1; + } +} + +// in cm and cms/s +static uint32_t calcTime(const int32_t distance, const int32_t speed) +{ + if (speed <= 0) { + return 0; + } + + return distance / speed; +} + +static void calcPreviewPoint(fpVector3_t *target, const int32_t distance) +{ + calculateFarAwayTarget(target, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), distance); + target->z = getEstimatedActualPosition(Z) + calcTime(geoZoneConfig()->fenceDetectionDistance, gpsSol.groundSpeed) * getEstimatedActualVelocity(Z); +} + +static bool calcIntersectionForZone(fpVector3_t *intersection, float *distance, geoZoneRuntimeConfig_t *zone, const fpVector3_t *start, const fpVector3_t *end) +{ + bool hasIntersection = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + if (calcLine3dPolygonIntersection( + intersection, + distance, + start, + end, + zone->verticesLocal, + zone->config.vertexCount, + zone->config.minAltitude, + zone->config.maxAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + fpVector3_t circleCenter = { .x = zone->verticesLocal[0].x, .y = zone->verticesLocal[0].y, .z = zone->config.minAltitude }; + if (calcLineCylinderIntersection( + intersection, + distance, + start, + end, + &circleCenter, + zone->radius, + zone->config.maxAltitude - zone->config.minAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } + + if (hasIntersection && isPointOnLine3(start, end, intersection)){ + return true; + } + *distance = -1; + return false; +} + +static int32_t calcBouceCoursePolygon(const int32_t course, const fpVector2_t* pos, const fpVector2_t *intersect, const fpVector2_t* p1, const fpVector2_t* p2) +{ + int32_t newCourse = 0; + int32_t angelp1 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p1, pos, intersect)); + int32_t angelp2 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p2, pos, intersect)); + if (angelp1 < angelp2) { + if (isPointRightFromLine(pos, intersect, p1)) { + newCourse = course - 2 * angelp1; + } + else { + newCourse = course + 2 * angelp1; + } + } + else { + if (isPointRightFromLine(pos, intersect, p2)) { + newCourse = course - 2 * angelp2; + } + else { + newCourse = course + 2 * angelp2; + } + } + return wrap_36000(newCourse); +} + +static int32_t calcBouceCourseCircle(const int32_t course, const fpVector2_t* pos, const fpVector2_t* intersect, const fpVector2_t* mid) +{ + int32_t newCourse = 0; + int32_t angel = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(mid, pos, intersect)); + if (isPointRightFromLine(pos, mid, intersect)) { + newCourse = course + 2 * (angel - 9000); + } + else { + newCourse = course - 2 * (angel - 9000); + } + return wrap_36000(newCourse); +} + +static bool findNearestIntersectionZone(geoZoneRuntimeConfig_t **intersectZone, fpVector3_t *intersection, float *distance, const float detectionDistance, const fpVector3_t *start, const fpVector3_t *end) +{ + geoZoneRuntimeConfig_t *zone = NULL; + fpVector3_t intersect; + float distanceToZone = FLT_MAX; + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + float currentDistance = FLT_MAX; + if (!calcIntersectionForZone( + ¤tIntersect, + ¤tDistance, + &activeGeoZones[i], + start, + end)) { + continue; + } + + if (currentDistance < distanceToZone) { + distanceToZone = currentDistance; + zone = &activeGeoZones[i]; + intersect = currentIntersect; + } + } + + if (zone && distanceToZone < detectionDistance) { + *intersectZone = zone; + *distance = distanceToZone; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + + return false; +} + +static bool isPointDirectReachable(const fpVector3_t* start, const fpVector3_t *point) +{ + float currentDistance = 0; + bool pointIsInOverlappingZone = false, pointIsInExclusiveZone = false, hasIntersection = false; + + /* + if (start->x == point->x && start->y == point->y) { + return false; + } + */ + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + + if (!calcIntersectionForZone(¤tIntersect, ¤tDistance, &activeGeoZones[i], start, point)) { + continue; + } + hasIntersection = true; + + // Intersct a exclusive Zone? + geoZoneRuntimeConfig_t *intersectZones[MAX_GEOZONES]; + uint8_t intersectZonesCount = getZonesForPos(intersectZones, ¤tIntersect, false); + for (uint8_t j = 0; j < intersectZonesCount; j++) { + if (intersectZones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE ) { + pointIsInExclusiveZone = true; + break; + } + } + + if (pointIsInExclusiveZone) { + break; + } + + // We targeting a exit point (in min two zones) + if (intersectZonesCount < 2) { + break; + } + + geoZoneRuntimeConfig_t *startZones[MAX_GEOZONES]; + uint8_t startZonesCount = getZonesForPos(startZones, start, false); + geoZoneRuntimeConfig_t *endZones[MAX_GEOZONES]; + uint8_t endZonesCount = getZonesForPos(endZones, point, false); + + for (uint8_t j = 0; j < intersectZonesCount; j++) { + for (uint8_t k = 0; k < startZonesCount; k++) { + for (uint8_t l = 0; l < endZonesCount; l++) { + if (intersectZones[j] == startZones[k] && intersectZones[j] == endZones[l]) { + pointIsInOverlappingZone = true; + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + } + + return !pointIsInExclusiveZone && (!hasIntersection || pointIsInOverlappingZone); +} + +uint32_t geozoneGetDetectionDistance(void) +{ + uint32_t detctionDistance = 0; + if (STATE(AIRPLANE)) { + detctionDistance = navConfig()->fw.loiter_radius * 1.5; + } else { + detctionDistance = geoZoneConfig()->copterFenceStopDistance; + } + return detctionDistance; +} + +static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) +{ + int32_t course; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t intersect; + bool found = false; + fpVector2_t *p1 = &zone->verticesLocal[zone->config.vertexCount - 1]; + fpVector2_t *p2; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + p2 = &zone->verticesLocal[i]; + if (calcLineLineIntersection(&intersect, p1, p2, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)prevPoint, true)) { + found = true; + break; + } + p1 = p2; + } + + if (!found) { + return -1; + } + course = calcBouceCoursePolygon(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &intersect, p1, p2); + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + course = calcBouceCourseCircle(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)intersection, &zone->verticesLocal[0]); + } + return course; +} + +static bool initPathPoint(pathPoint_t *pathPoints, const fpVector3_t pos, uint8_t *idx) +{ + if (*idx + 1 >= MAX_PATH_PONITS) { + return false; + } + + pathPoints[*idx].distance = FLT_MAX; + pathPoints[*idx].visited = false; + pathPoints[(*idx)++].point = pos; + + return true; +} + +static bool isPosInGreenAlt(geoZoneRuntimeConfig_t *zones[], const uint8_t zoneCount, const float alt) +{ + bool isInNfz = false, isInFz = false; + for (uint8_t j = 0; j < zoneCount; j++) { + if(isInZoneAltitudeRange(zones[j], alt)){ + isInFz = zones[j]->config.type == GEOZONE_TYPE_INCLUSIVE; + isInNfz = zones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE; + } + } + return !isInNfz && (!isAtLeastOneInclusiveZoneActive || isInFz); +} + +static bool checkPathPointOrSetAlt(fpVector3_t *pos) +{ + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, pos, true); + + if (zoneCount == 0) { + return !isAtLeastOneInclusiveZoneActive; + } + + if (zoneCount == 1 && zones[0]->config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + + bool isInExclusice = false; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(zones[i], pos, false)) { + isInExclusice = true; + if (zones[i]->config.minAltitude != 0) { + float min = zones[i]->config.minAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + if (isPosInGreenAlt(zones, zoneCount, min)) { + pos->z = min; + return true; + } + } + + if (!zones[i]->isInfZone || zones[i]->config.maxAltitude < INT32_MAX) { + float max = zones[i]->config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + if(isPosInGreenAlt(zones, zoneCount, max)) { + pos->z = max; + return true; + } + } + } + } + + return !isInExclusice; +} + +// Return value: 0 - Target direct reachable; -1 No way; >= 1 Waypoints to target +#define CIRCLE_POLY_SIDES 6 +static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fpVector3_t* target) +{ + pathPoint_t pathPoints[MAX_PATH_PONITS]; + uint8_t pathPointCount = 0, waypointCount = 0; + fpVector3_t start = *point; + + if (isPointDirectReachable(&start, target)) { + return 0; + } + + float offset = geozoneGetDetectionDistance(); + if (geozone.distanceVertToNearestZone <= offset) { + int bearing = wrap_36000(geozone.directionToNearestZone + 18000); + start.x += offset * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); + start.y += offset * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); + } + + pathPoints[pathPointCount].visited = true; + pathPoints[pathPointCount].distance = 0; + pathPoints[pathPointCount++].point = start; + + for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { + fpVector2_t *verticesZone; + fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; + uint8_t verticesZoneCount; + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + verticesZone = activeGeoZones[i].verticesLocal; + verticesZoneCount = activeGeoZones[i].config.vertexCount; + } else { + generatePolygonFromCircle(verticesCirclePoly, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius, CIRCLE_POLY_SIDES); + verticesZone = verticesCirclePoly; + verticesZoneCount = CIRCLE_POLY_SIDES; + } + + fpVector2_t safeZone[MAX_VERTICES]; + offset = geozoneGetDetectionDistance() * 2 / 3; + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + offset *= -1; + } + + float zMin = start.z, zMax = 0; + if (!isInZoneAltitudeRange(&activeGeoZones[i], start.z) && activeGeoZones[i].config.minAltitude > 0) { + zMin = activeGeoZones[i].config.minAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + } else if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + generateOffsetPolygon(safeZone, verticesZone, verticesZoneCount, offset); + fpVector2_t *prev = &safeZone[verticesZoneCount - 1]; + fpVector2_t *current; + for (uint8_t j = 0; j < verticesZoneCount; j++) { + current = &safeZone[j]; + + if (zMax > 0 ) { + fpVector3_t max = { .x = current->x, .y = current->y, .z = zMax }; + if (checkPathPointOrSetAlt(&max)) { + if (!initPathPoint(pathPoints, max, &pathPointCount)) { + return -1; + } + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + // Set some "fly over points" + float dist = calculateDistance2(prev, current); + if (dist > MAX_DISTANCE_FLY_OVER_POINTS) { + uint8_t sectionCount = (uint8_t)(dist / MAX_DISTANCE_FLY_OVER_POINTS); + float dist = MAX_DISTANCE_FLY_OVER_POINTS; + for (uint8_t k = 0; k < sectionCount; k++) { + fpVector3_t flyOverPoint; + calcPointOnLine((fpVector2_t*)&flyOverPoint, prev, current, dist); + fpVector3_t maxFo = { .x = flyOverPoint.x, .y = flyOverPoint.y, .z = zMax }; + if (checkPathPointOrSetAlt(&maxFo)) { + if (!initPathPoint(pathPoints, maxFo, &pathPointCount)) { + return -1; + } + } + dist += MAX_DISTANCE_FLY_OVER_POINTS; + } + } + } + } + + if (zMin > 0) { + fpVector3_t min = { .x = current->x, .y = current->y, .z = zMin }; + if (checkPathPointOrSetAlt(&min)) { + if (!initPathPoint(pathPoints, min, &pathPointCount)) { + return -1; + } + } + + } + prev = current; + } + } + + if (!initPathPoint(pathPoints, *target, &pathPointCount)) { + return -1; + } + + // Dijkstra + pathPoint_t *current = pathPoints; + while (!pathPoints[pathPointCount - 1].visited) { + pathPoint_t *next = current; + float min = FLT_MAX; + for (uint16_t i = 1; i < pathPointCount; i++) { + + float currentDist = FLT_MAX; + if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { + float dist2D = calculateDistance2((fpVector2_t*)¤t->point, (fpVector2_t*)&pathPoints[i].point); + float distAlt = ABS(current->point.z - pathPoints[i].point.z); + currentDist = current->distance + dist2D + 2 * distAlt; + } + + if (currentDist < pathPoints[i].distance && !pathPoints[i].visited) { + pathPoints[i].distance = currentDist; + pathPoints[i].prev = current; + } + if (min > pathPoints[i].distance && !pathPoints[i].visited) { + min = pathPoints[i].distance; + next = &pathPoints[i]; + } + } + + if (min == FLT_MAX) { + return -1; + } + + current = next; + current->visited = true; + } + + waypointCount = 0; + current = &pathPoints[pathPointCount - 1]; + while (current != pathPoints) { + waypointCount++; + current = current->prev; + } + // Don't set home to the WP list + current = pathPoints[pathPointCount - 1].prev; + uint8_t i = waypointCount - 2; + while (current != pathPoints) { + waypoints[i] = current->point; + current = current->prev; + i--; + } + return waypointCount - 1; +} + +static void updateCurrentZones(void) +{ + currentZoneCount = getCurrentZones(currentZones, false); + geozone.insideNfz = false; + geozone.insideFz = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE) { + geozone.insideNfz = true; + } + if (currentZones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + geozone.insideFz = true; + } + } +} + +static void updateZoneInfos(void) +{ + float nearestDistanceToBorder = FLT_MAX; + fpVector3_t nearestBorderPoint; + aboveOrUnderZone = false; + + geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; + uint8_t currentZoneCount = getCurrentZones(currentZones, true); + int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; + + if (currentZoneCount == 1) { + currentMaxAltitude = currentZones[0]->config.maxAltitude; + currentMinAltitude = currentZones[0]->config.minAltitude; + + if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { + nearestHorInclZone = currentZones[0]; + } + + } else if (currentZoneCount >= 2) { + + geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; + float distAbove = FLT_MAX, distBelow = FLT_MAX; + geoZoneRuntimeConfig_t *current = NULL; + for (uint8_t i = 0; i < currentZoneCount; i++) { + current = currentZones[i]; + + if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { + currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); + currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + } + + if (current->config.minAltitude > getEstimatedActualPosition(Z)) { + float dist = current->config.maxAltitude - getEstimatedActualPosition(Z); + if (dist < distAbove) { + aboveZone = current; + distAbove = dist; + } + } + + if (current->config.maxAltitude < getEstimatedActualPosition(Z)) { + float dist = getEstimatedActualPosition(Z) - current->config.maxAltitude; + if (dist < distBelow) { + belowZone = current; + distBelow = dist; + } + } + } + + if (aboveZone) { + if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); + nearestHorInclZone = aboveZone; + } else { + currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); + } + } + + if (belowZone) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); + nearestHorInclZone = belowZone; + } else { + currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); + } + } + } + + if (currentMinAltitude == INT32_MAX) { + currentMinAltitude = 0; + } + + if (currentMaxAltitude == INT32_MIN) { + currentMaxAltitude = 0; + } + + if (currentMaxAltitude == INT32_MAX && currentMinAltitude != 0) { + geozone.distanceVertToNearestZone = ABS(currentMinAltitude - getEstimatedActualPosition(Z)); + } else if (currentMinAltitude == 0 && currentMaxAltitude != 0) { + geozone.distanceVertToNearestZone = currentMaxAltitude - getEstimatedActualPosition(Z); + } else if (currentMinAltitude != 0 && currentMaxAltitude > 0) { + int32_t distToMin = currentMinAltitude - getEstimatedActualPosition(Z); + int32_t distToMax = currentMaxAltitude - getEstimatedActualPosition(Z); + if (getEstimatedActualPosition(Z) > currentMinAltitude && getEstimatedActualPosition(Z) < currentMaxAltitude) { + if (ABS(distToMin) < ABS(currentMaxAltitude - currentMinAltitude) / 2 ) { + geozone.distanceVertToNearestZone = distToMin; + } else { + geozone.distanceVertToNearestZone = distToMax; + } + } else { + geozone.distanceVertToNearestZone = MIN(ABS(distToMin), distToMax); + } + } else { + geozone.distanceVertToNearestZone = 0; + } + + if (currentZoneCount == 0) { + geozone.currentzoneMaxAltitude = 0; + geozone.currentzoneMinAltitude = 0; + } else { + + if (getEstimatedActualPosition(Z) < currentMinAltitude || getEstimatedActualPosition(Z) > currentMaxAltitude) { + aboveOrUnderZone = true; + } + + if (currentMaxAltitude > 0) { + geozone.currentzoneMaxAltitude = currentMaxAltitude - geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMaxAltitude = 0; + } + + if (currentMinAltitude > 0) { + geozone.currentzoneMinAltitude = currentMinAltitude + geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMinAltitude = 0; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) + { + // Ignore NFZ for now, we want back fo the FZ, we will check for NFZ later at RTH + if (currentZoneCount == 0 && isAtLeastOneInclusiveZoneActive && activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + continue; + } + + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t* prev = &activeGeoZones[i].verticesLocal[activeGeoZones[i].config.vertexCount - 1]; + fpVector2_t* current = NULL; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + current = &activeGeoZones[i].verticesLocal[j]; + fpVector2_t pos = calcNearestPointOnLine(prev, current, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos); + float dist = calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &pos); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + nearestBorderPoint.x = pos.x; + nearestBorderPoint.y = pos.y; + nearestBorderPoint.z = getEstimatedActualPosition(Z); + geozone.directionToNearestZone = calculateBearingToDestination(&nearestBorderPoint); + geozone.distanceHorToNearestZone = roundf(nearestDistanceToBorder); + nearestInclusiveZone = &activeGeoZones[i]; + } + prev = current; + } + } else if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + float dist = fabsf(calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0]) - activeGeoZones[i].radius); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + int32_t directionToBorder = calculateBearingToDestination((fpVector3_t*)&activeGeoZones[i].verticesLocal[0]); + + if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { + directionToBorder = wrap_36000(directionToBorder + 18000); + } + + geozone.directionToNearestZone = directionToBorder; + geozone.distanceHorToNearestZone = roundf(dist); + nearestInclusiveZone = &activeGeoZones[i]; + } + } + + if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorInclZone; + geozone.distanceHorToNearestZone = 0; + } + } +} + +void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) +{ + // Actions only for assisted modes now + if (isNonGeozoneModeFromBoxInput() || geozone.avoidInRTHInProgress || (calculateDistanceToDestination(intersection) > geozoneGetDetectionDistance())) { + return; + } + + int32_t alt = roundf(intersection->z); + if (alt == zone->config.maxAltitude || alt == zone->config.minAltitude) { + return; + } + + fpVector3_t prevPoint; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + + if (zone->config.fenceAction == GEOFENCE_ACTION_AVOID) { + bool avoidFzStep = false; + float fzStepAlt = 0; + if (zone->config.type == GEOZONE_TYPE_INCLUSIVE) { + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES_IN_CONFIG]; + uint8_t zoneCount = getZonesForPos(zones, intersection, true); + + float maxAlt = FLT_MAX; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE && zones[i]->config.minAltitude > intersection->z) { + float alt = zones[i]->config.minAltitude; + if (alt < maxAlt) { + maxAlt = alt; + } + } + } + + if (maxAlt < FLT_MAX) { + fzStepAlt = maxAlt + geoZoneConfig()->safeAltitudeDistance * 2; + avoidFzStep = true; + } + } + // We can move upwards + if ((zone->config.type == GEOZONE_TYPE_EXCLUSIVE && geozone.zoneInfo > 0 && (geozone.zoneInfo < geoZoneConfig()->avoidAltitudeRange)) || avoidFzStep) { + + calculateFarAwayTarget(&posControl.sendTo.targetPos, posControl.actualState.cog, geoZoneConfig()->fenceDetectionDistance * 2); + if (avoidFzStep) { + posControl.sendTo.targetPos.z = fzStepAlt; + } else { + posControl.sendTo.targetPos.z = zone->config.maxAltitude + geoZoneConfig()->safeAltitudeDistance * 2; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING_UPWARD; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } else { + // Calc new course + avoidCourse = calcBounceCourseForZone(zone, &prevPoint, intersection); + + if (avoidCourse == -1) { + return; + } + + calculateFarAwayTarget(&posControl.sendTo.targetPos, avoidCourse, geoZoneConfig()->fenceDetectionDistance * 2); // Its too far, mode will be abort if we are on the right course + + // Check for min/max altitude + if (geozone.currentzoneMaxAltitude > 0 && getEstimatedActualPosition(Z) > geozone.currentzoneMaxAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 0.25; + } else if (geozone.currentzoneMinAltitude != 0 && getEstimatedActualPosition(Z) < geozone.currentzoneMinAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 0.25; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = AVOID_TIMEOUT; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } + } else if (zone->config.fenceAction == GEOFENCE_ACTION_POS_HOLD) { + actionState = GEOZONE_ACTION_STATE_POSHOLD; + + if (STATE(AIRPLANE)) { + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector3_t refPoint; + int32_t course = calcBounceCourseForZone(zone, &prevPoint, intersection); + calculateFarAwayTarget(&refPoint, course, geoZoneConfig()->fenceDetectionDistance * 2); + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, (fpVector2_t*)&refPoint)) { + geozone.loiterDir = 1; // Right + } else { + geozone.loiterDir = -1; // Left + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, &zone->verticesLocal[0])) { + geozone.loiterDir = -1; // Left + } else { + geozone.loiterDir = 1; // Right + } + } + } + + geozone.sticksLocked = true; + activateForcedPosHold(); + actionStartTime = millis(); + } else if (zone->config.fenceAction == GEOFENCE_ACTION_RTH) { + actionState = GEOZONE_ACTION_STATE_RTH; + geozone.sticksLocked = true; + activateForcedRTH(); + actionStartTime = millis(); + } +} + +static void endFenceAction(void) +{ + posControl.sendTo.lockSticks = false; + geozone.sticksLocked = false; + geozone.sticksLocked = 0; + + switch (actionState) { + case GEOZONE_ACTION_STATE_AVOIDING: + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + abortSendTo(); + break; + case GEOZONE_ACTION_STATE_POSHOLD: + abortForcedPosHold(); + break; + case GEOZONE_ACTION_STATE_RTH: + abortForcedRTH(); + break; + default: + break; + } + + actionState = GEOZONE_ACTION_STATE_NONE; + + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)){ + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_Z); + } + + abortSendTo(); +} + +static void geoZoneInit(void) +{ + activeGeoZonesCount = 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[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; + } + + activeGeoZones[activeGeoZonesCount].enable = true; + activeGeoZonesCount++; + } + } + + if (activeGeoZonesCount > 0) { + // Covert geozone vertices to local + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + 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) { + 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; + 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]; + } + } + } + } + + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) + { + safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; + safeHomeGeozoneConfig.type = GEOZONE_TYPE_INCLUSIVE; + safeHomeGeozoneConfig.fenceAction = geoZoneConfig()->safeHomeFenceAction; + safeHomeGeozoneConfig.maxAltitude = 0; + safeHomeGeozoneConfig.minAltitude = 0; + safeHomeGeozoneConfig.vertexCount = 1; + + activeGeoZones[activeGeoZonesCount].config = safeHomeGeozoneConfig; + activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; + activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; + activeGeoZonesCount++; + } + + updateCurrentZones(); + uint8_t newActiveZoneCount = activeGeoZonesCount; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (!geozone.insideFz) { + // Deactivate all inclusive geozones with distance > GEOZONE_INCLUSE_IGNORE_DISTANCE + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && !isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + fpVector2_t pos2 = { .x = getEstimatedActualPosition(X), .y = getEstimatedActualPosition(Y) }; + + uint32_t minDistanceToZone = INT32_MAX; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + float dist = calculateDistance2(&pos2, &activeGeoZones[i].verticesLocal[j]); + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + minDistanceToZone = dist - activeGeoZones[i].radius; + break; + } + + if (dist < minDistanceToZone) { + minDistanceToZone = dist; + } + } + if (minDistanceToZone > GEOZONE_INCLUSE_IGNORE_DISTANCE) { + activeGeoZones[i].enable = false; + newActiveZoneCount--; + } + } + } + } + + activeGeoZonesCount = newActiveZoneCount; + if (activeGeoZonesCount == 0) { + setTaskEnabled(TASK_GEOZONE, false); + geozoneIsEnabled = false; + return; + } + geozoneIsEnabled = true; + + qsort(activeGeoZones, MAX_GEOZONES, sizeof(geoZoneRuntimeConfig_t), geoZoneRTComp); + + for (int i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + isAtLeastOneInclusiveZoneActive = true; + break; + } + } +} + +// Called by Scheduler +void geozoneUpdate(timeUs_t curentTimeUs) +{ + UNUSED(curentTimeUs); + + geozone.messageState = GEOZONE_MESSAGE_STATE_NONE; + if (!isInitalised && navigationPositionEstimateIsHealthy()) { + geoZoneInit(); + isInitalised = true; + } + + if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + noZoneRTH = false; + return; + } + + // Zone RTH Override: Toggle RTH switch short + if (geozone.avoidInRTHInProgress) { + if (rthHomeSwitchLastState != IS_RC_MODE_ACTIVE(BOXNAVRTH)) { + if (millis() - rthOverrideStartTime < RTH_OVERRIDE_TIMEOUT) { + geozoneResetRTH(); + noZoneRTH = true; + } + rthOverrideStartTime = millis(); + } + rthHomeSwitchLastState = IS_RC_MODE_ACTIVE(BOXNAVRTH); + } + + updateCurrentZones(); + updateZoneInfos(); + + // User has switched to non geofence mode, end all actions and switch to mode from box input + if (isNonGeozoneModeFromBoxInput()) { + if (actionState != GEOZONE_ACTION_STATE_NONE) { + endFenceAction(); + } + lockRTZ = false; + return; + } + + switch (actionState) + { + case GEOZONE_ACTION_STATE_AVOIDING: + if (calculateDistanceToDestination(&avoidingPoint) > geozoneGetDetectionDistance()) { + posControl.sendTo.lockSticks = false; + } + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(avoidCourse, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), 500) || millis() - actionStartTime > AVOID_TIMEOUT || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE; + if ((geozone.insideFz && ABS(geozone.distanceVertToNearestZone ) > geoZoneConfig()->safeAltitudeDistance) || !posControl.flags.sendToActive) { + lockRTZ = true; + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_FLYOUT_NFZ; + if (!geozone.insideNfz || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_UPWARD: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RTH: + case GEOZONE_ACTION_STATE_POSHOLD: + geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { + geozone.sticksLocked = false; + } + if (!geozone.sticksLocked && areSticksDeflectdFromChannelValue()) { + endFenceAction(); + } + return; + default: + break; + } + + bool currentZoneHasAction = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + currentZoneHasAction = true; + break; + } + } + + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + + float targetAltitide = 0; + if (!geozone.insideFz) { + if (geozone.distanceVertToNearestZone < 0) { + targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + } else { + targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + } else { + targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + + fpVector3_t targetPos; + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitide; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + actionState = GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE; + actionStartTime = millis(); + activateSendTo(); + return; + } + + // RTH active: Further checks are done in RTH logic + if ((navGetCurrentStateFlags() & NAV_AUTO_RTH) || IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated || FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return; + } else if (geozone.avoidInRTHInProgress) { + geozoneResetRTH(); + } + + if (lockRTZ && (geozone.insideFz || geozone.insideNfz)) { + lockRTZ = false; + } + + if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { + + if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_OUTSIDE_FZ; + } + + if (geozone.insideNfz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_NFZ; + } + + if (!isNonGeozoneModeFromBoxInput()) { + bool flyOutNfz = false; + geoZoneRuntimeConfig_t *targetZone = nearestInclusiveZone; + + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + flyOutNfz = true; + targetZone = currentZones[i]; + break; + } + } + + if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { + int32_t targetAltitude = 0; + if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5; + } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z); + } + + fpVector3_t targetPos; + if (aboveOrUnderZone) { + if (ABS(geozone.distanceVertToNearestZone) < 2000) { + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + if(geozone.distanceVertToNearestZone > 0) { + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5; + } + + } else { + targetPos = navGetCurrentActualPositionAndVelocity()->pos; + } + } else { + calculateFarAwayTarget(&targetPos, geozone.directionToNearestZone, geozone.distanceHorToNearestZone + geoZoneConfig()->fenceDetectionDistance / 2); + } + + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitude; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + + if (flyOutNfz) { + actionState = GEOZONE_ACTION_STATE_FLYOUT_NFZ; + } else { + actionState = GEOZONE_ACTION_STATE_RETURN_TO_FZ; + } + + activateSendTo(); + } + } + } + + + geoZoneRuntimeConfig_t *intersectZone = NULL; + fpVector3_t intersection, prevPoint; + float distanceToZone = 0; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + if (findNearestIntersectionZone(&intersectZone, &intersection, &distanceToZone, geoZoneConfig()->fenceDetectionDistance, &navGetCurrentActualPositionAndVelocity()->pos, &prevPoint)) { + if (geozone.insideFz) { + if (!isPointInAnyOtherZone(intersectZone, GEOZONE_TYPE_INCLUSIVE, &intersection)) { + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + geozone.messageState = GEOZONE_MESSAGE_STATE_LEAVING_FZ; + performeFenceAction(intersectZone, &intersection); + } + } + + if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { + + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + + int32_t minAltitude = intersectZone->config.minAltitude; + int32_t maxAltitude = intersectZone->config.maxAltitude; + if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { + geozone.zoneInfo = INT32_MAX; + } else if (maxAltitude == INT32_MAX) { + geozone.zoneInfo = minAltitude - getEstimatedActualPosition(Z); + } else if (minAltitude == 0) { + geozone.zoneInfo = maxAltitude - getEstimatedActualPosition(Z); + } else { + int32_t distToMax = maxAltitude - getEstimatedActualPosition(Z); + int32_t distToMin = minAltitude - getEstimatedActualPosition(Z); + if (ABS(distToMin) < ABS(distToMax)) { + geozone.zoneInfo = distToMin; + } else { + geozone.zoneInfo = (distToMax); + } + } + + geozone.messageState = GEOZONE_MESSAGE_STATE_ENTERING_NFZ; + performeFenceAction(intersectZone, &intersection); + } + } +} + +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void) +{ + return &rthWaypoints[rthWaypointIndex]; +} + +void geozoneAdvanceRthAvoidWaypoint(void) +{ + if (rthWaypointIndex < rthWaypointCount) { + rthWaypointIndex++; + } +} + +bool geoZoneIsLastRthWaypoint(void) +{ + return rthWaypointIndex == rthWaypointCount - 1; +} + +void geozoneResetRTH(void) +{ + geozone.avoidInRTHInProgress = false; + rthWaypointIndex = 0; + rthWaypointCount = 0; +} + +void geozoneSetupRTH() { + if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { + noZoneRTH = true; + } else { + noZoneRTH = false; + } +} + +int8_t geozoneCheckForNFZAtCourse(bool isRTH) +{ + UNUSED(isRTH); + + if (geozone.avoidInRTHInProgress || noZoneRTH || !geozoneIsEnabled || !isInitalised) { + return 0; + } + + updateCurrentZones(); + + // Never mind, lets fly out of the zone on current course + if (geozone.insideNfz || (isAtLeastOneInclusiveZoneActive && !geozone.insideFz)) { + return 0; + } + + int8_t waypointCount = calcRthCourse(rthWaypoints, &navGetCurrentActualPositionAndVelocity()->pos, &posControl.rthState.homePosition.pos); + if (waypointCount > 0) { + rthWaypointCount = waypointCount; + rthWaypointIndex = 0; + geozone.avoidInRTHInProgress = true; + return 1; + } + + return waypointCount; +} + +bool isGeozoneActive(void) +{ + return activeGeoZonesCount > 0; +} + +void geozoneUpdateMaxHomeAltitude(void) { + int32_t altitude = INT32_MIN; + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, &posControl.rthState.homePosition.pos, false); + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + altitude = MAX(zones[i]->config.maxAltitude, altitude); + } + } + + if (altitude > INT32_MIN) { + geozone.maxHomeAltitude = altitude - geoZoneConfig()->safeAltitudeDistance; + geozone.homeHasMaxAltitue = true; + } else { + geozone.homeHasMaxAltitue = false; + } +} + +// Avoid arming in NFZ +bool geozoneIsInsideNFZ(void) +{ + // Do not generate arming flags unless we are sure about them + if (!isInitalised || activeGeoZonesCount == 0) { + return false; + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return true; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return false; + } + } + + // We aren't in any zone, is an inclusive one still active? + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + } + + return false; +} + +#endif diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h new file mode 100755 index 00000000000..38849719af0 --- /dev/null +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -0,0 +1,480 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include +#include + +#include "common/vector.h" +#include "navigation/navigation_private.h" + +#define K_EPSILON 1e-8 + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = line1End->x - line1Start->x; + double t1 = -(line2End->x - line2Start->x); + double r1 = line2Start->x - line1Start->x; + + double s2 = line1End->y - line1Start->y; + double t2 = -(line2End->y - line2Start->y); + double r2 = line2Start->y - line1Start->y; + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } + else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6d7b386cbde..fe277a00617 100644 --- a/src/main/navigation/navigation_private.h +++ b/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); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 176812d8f3f..5d38a6dba59 100755 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/target/common.h b/src/main/target/common.h index f7cc3f747ce..3e035710fca 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -221,3 +221,4 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER +#define USE_GEOZONE From e72354e389b2b0c9f6b4354099b6afd27bb8467c Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:27:08 -0300 Subject: [PATCH 02/17] Bugfix --- src/main/io/osd.c | 116 +++++++++--------- src/main/navigation/navigation.c | 36 ++++-- src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_geozone.c | 81 ++++++------ .../navigation_geozone_calculations.h | 3 +- 5 files changed, 129 insertions(+), 110 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index efcfa394f65..444978dc8c1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5867,6 +5867,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); + 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); + 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 */ @@ -5905,64 +5963,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } -#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); - 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); - 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_LOITER: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_NONE: - break; - } -#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bbf8e7910f0..fcde4980f44 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -648,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_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, - [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_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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [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_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_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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE, } }, @@ -3606,14 +3607,15 @@ bool isProbablyStillFlying(void) *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { + + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + #ifdef USE_GEOZONE - if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || - (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) { + 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 - - const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { @@ -4253,6 +4255,14 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); + if (geozone.sticksLocked) { + posControl.flags.isAdjustingAltitude = false; + posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; + } + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 6e2ca8fbcbc..1f545b7353f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -131,7 +131,7 @@ typedef enum { GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, - GEOZONE_MESSAGE_STATE_LOITER + GEOZONE_MESSAGE_STATE_POS_HOLD } geozoneMessageState_e; enum fenceAction_e { @@ -197,6 +197,7 @@ typedef struct geozone_s { int32_t zoneInfo; int32_t currentzoneMaxAltitude; int32_t currentzoneMinAltitude; + bool nearestHorZoneHasAction; bool sticksLocked; int8_t loiterDir; bool avoidInRTHInProgress; diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 8a2505a0ff9..80b9cf4def0 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -58,7 +58,7 @@ #define MAX_DISTANCE_FLY_OVER_POINTS 50000 #define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) #define POS_DETECTION_DISTANCE 7500 -#define STICK_LOCK_MIN_TIME 1500 +#define STICK_LOCK_MIN_TIME 2500 #define AVOID_TIMEOUT 30000 #define MAX_LOCAL_VERTICES 128 #define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m @@ -109,6 +109,7 @@ static geoZoneConfig_t safeHomeGeozoneConfig; static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; static timeMs_t actionStartTime = 0; static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestHorZone = NULL; static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; static fpVector3_t avoidingPoint; static bool geozoneIsEnabled = false; @@ -650,6 +651,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp return 0; } + // Set starting point slightly away from our current position float offset = geozoneGetDetectionDistance(); if (geozone.distanceVertToNearestZone <= offset) { int bearing = wrap_36000(geozone.directionToNearestZone + 18000); @@ -660,7 +662,11 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp pathPoints[pathPointCount].visited = true; pathPoints[pathPointCount].distance = 0; pathPoints[pathPointCount++].point = start; - + + // Calculate possible waypoints + // Vertices of the zones are possible waypoints, + // inclusive zones are “reduced”, exclusive zones are “enlarged” to keep distance, + // round zones are converted into hexagons and long sides get additional points to be able to fly over zones. for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { fpVector2_t *verticesZone; fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; @@ -748,7 +754,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp while (!pathPoints[pathPointCount - 1].visited) { pathPoint_t *next = current; float min = FLT_MAX; - for (uint16_t i = 1; i < pathPointCount; i++) { + for (uint8_t i = 1; i < pathPointCount; i++) { float currentDist = FLT_MAX; if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { @@ -813,7 +819,7 @@ static void updateZoneInfos(void) fpVector3_t nearestBorderPoint; aboveOrUnderZone = false; - geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + nearestHorZone = NULL; geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; uint8_t currentZoneCount = getCurrentZones(currentZones, true); int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; @@ -821,11 +827,8 @@ static void updateZoneInfos(void) if (currentZoneCount == 1) { currentMaxAltitude = currentZones[0]->config.maxAltitude; currentMinAltitude = currentZones[0]->config.minAltitude; - - if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { - nearestHorInclZone = currentZones[0]; - } - + nearestHorZone = currentZones[0]; + } else if (currentZoneCount >= 2) { geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; @@ -837,6 +840,7 @@ static void updateZoneInfos(void) if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + nearestHorZone = current; } if (current->config.minAltitude > getEstimatedActualPosition(Z)) { @@ -859,16 +863,16 @@ static void updateZoneInfos(void) if (aboveZone) { if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); - nearestHorInclZone = aboveZone; + nearestHorZone = aboveZone; } else { currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); } } if (belowZone) { - if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); - nearestHorInclZone = belowZone; + nearestHorZone = belowZone; } else { currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); } @@ -959,18 +963,19 @@ static void updateZoneInfos(void) if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { directionToBorder = wrap_36000(directionToBorder + 18000); } - geozone.directionToNearestZone = directionToBorder; geozone.distanceHorToNearestZone = roundf(dist); nearestInclusiveZone = &activeGeoZones[i]; } } - if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { - nearestInclusiveZone = nearestHorInclZone; + if (aboveOrUnderZone && nearestHorZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorZone; geozone.distanceHorToNearestZone = 0; } } + + geozone.nearestHorZoneHasAction = nearestHorZone && nearestHorZone->config.fenceAction != GEOFENCE_ACTION_NONE; } void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) @@ -1248,7 +1253,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) isInitalised = true; } - if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + if (!ARMING_FLAG(ARMED) || !isInitalised || activeGeoZonesCount == 0) { noZoneRTH = false; return; } @@ -1267,6 +1272,10 @@ void geozoneUpdate(timeUs_t curentTimeUs) updateCurrentZones(); updateZoneInfos(); + + if (STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + return; + } // User has switched to non geofence mode, end all actions and switch to mode from box input if (isNonGeozoneModeFromBoxInput()) { @@ -1315,7 +1324,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) return; case GEOZONE_ACTION_STATE_RTH: case GEOZONE_ACTION_STATE_POSHOLD: - geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + geozone.messageState = GEOZONE_MESSAGE_STATE_POS_HOLD; if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { geozone.sticksLocked = false; } @@ -1327,25 +1336,26 @@ void geozoneUpdate(timeUs_t curentTimeUs) break; } - bool currentZoneHasAction = false; - for (uint8_t i = 0; i < currentZoneCount; i++) { - if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { - currentZoneHasAction = true; - break; - } - } - - if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && + actionState == GEOZONE_ACTION_STATE_NONE && + geozone.nearestHorZoneHasAction && + ABS(geozone.distanceVertToNearestZone) > 0 && + ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { float targetAltitide = 0; - if (!geozone.insideFz) { - if (geozone.distanceVertToNearestZone < 0) { - targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + uint32_t extraSafteyAlt = geoZoneConfig()->safeAltitudeDistance * 0.25; + if (nearestHorZone->config.type == GEOZONE_TYPE_INCLUSIVE && geozone.insideFz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMaxAltitude - extraSafteyAlt; } else { - targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + targetAltitide = geozone.currentzoneMinAltitude + extraSafteyAlt; + } + } else if (nearestHorZone->config.type == GEOZONE_TYPE_EXCLUSIVE && !geozone.insideNfz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMinAltitude - extraSafteyAlt; + } else { + targetAltitide = geozone.currentzoneMaxAltitude + extraSafteyAlt; } - } else { - targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; } fpVector3_t targetPos; @@ -1374,6 +1384,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) lockRTZ = false; } + // RTZ: Return to zone: if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { @@ -1457,9 +1468,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) } if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { - - geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); - + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); int32_t minAltitude = intersectZone->config.minAltitude; int32_t maxAltitude = intersectZone->config.maxAltitude; if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { @@ -1474,7 +1483,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (ABS(distToMin) < ABS(distToMax)) { geozone.zoneInfo = distToMin; } else { - geozone.zoneInfo = (distToMax); + geozone.zoneInfo = distToMax; } } diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index 38849719af0..d29d7deb35f 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -348,8 +348,7 @@ static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* dista if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { intersect.x = i1.x; intersect.y = i1.y; - } - else { + } else { intersect.x = i2.x; intersect.y = i2.y; } From 0971756190340c94ab48585e7a454e73287b387d Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sat, 2 Nov 2024 10:51:48 -0300 Subject: [PATCH 03/17] Inital commit --- src/main/CMakeLists.txt | 1 + src/main/common/vector.h | 83 + src/main/config/parameter_group_ids.h | 5 +- src/main/fc/cli.c | 258 ++- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 8 + src/main/fc/fc_msp.c | 98 + src/main/fc/fc_tasks.c | 23 + src/main/fc/runtime_config.h | 9 +- src/main/fc/settings.yaml | 52 + src/main/io/osd.c | 117 ++ src/main/io/osd.h | 22 +- src/main/io/osd_dji_hd.c | 2 + src/main/msp/msp_protocol_v2_inav.h | 7 +- src/main/navigation/navigation.c | 261 ++- src/main/navigation/navigation.h | 113 ++ src/main/navigation/navigation_fixedwing.c | 6 + src/main/navigation/navigation_geozone.c | 1598 +++++++++++++++++ .../navigation_geozone_calculations.h | 480 +++++ src/main/navigation/navigation_private.h | 31 + src/main/scheduler/scheduler.h | 4 + src/main/target/common.h | 1 + 22 files changed, 3171 insertions(+), 10 deletions(-) create mode 100755 src/main/navigation/navigation_geozone.c create mode 100755 src/main/navigation/navigation_geozone_calculations.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e10985..7f188974cd9 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 449a0973b3d..737c29d56a7 100644 --- a/src/main/common/vector.h +++ b/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; +} diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index b4062201c72..26715ab3cec 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cbe85496f1a..d5c115c66b1 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -152,7 +152,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", - "", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", @@ -1561,6 +1561,251 @@ 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"; + 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; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction); + } +} + +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; + geoZonesConfigMutable(zoneId)->vertexCount++; + + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + + } else { + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 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++; + 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++; + } + + if (argumentCount != 6) { + 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)->fenceAction = fenceAction; + } +} +#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 +4423,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 +4814,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), diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ddb6a826b83..17c8ded8c1e 100644 --- a/src/main/fc/config.h +++ b/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, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f7603eee692..ba29a03d5e0 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,6 +278,14 @@ 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); + } +#endif + /* CHECK: */ if ( sensors(SENSOR_ACC) && diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f87b7f079c7..c875587ae5e 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1771,6 +1771,53 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) +static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, geoZonesConfig(idx)->type); + sbufWriteU8(dst, geoZonesConfig(idx)->shape); + sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); + sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); + sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); + sbufWriteU8(dst, idx); + 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) { @@ -3318,6 +3365,49 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; +#if defined(USE_GEOZONE) && defined (USE_GPS) + case MSP2_INAV_SET_GEOZONE: + if (dataSize == 13) { + 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)->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: @@ -3896,6 +3986,14 @@ 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) + 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 diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db526..df335ccb428 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -339,6 +339,15 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } +#if defined(USE_GEOZONE) && defined (USE_GPS) +void geozoneUpdateTask(timeUs_t currentTimeUs) +{ + if (feature(FEATURE_GEOZONE)) { + geozoneUpdate(currentTimeUs); + } +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -453,6 +462,11 @@ void fcTasksInit(void) #if defined(SITL_BUILD) serialProxyStart(); #endif + +#if defined(USE_GEOZONE) && defined (USE_GPS) + setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); +#endif + } cfTask_t cfTasks[TASK_COUNT] = { @@ -740,4 +754,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + [TASK_GEOZONE] = { + .taskName = "GEOZONE", + .taskFunc = geozoneUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(5), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 974f90f8c4d..637ea867e64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -24,6 +24,7 @@ typedef enum { 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 +50,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_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | + 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 +66,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 +108,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; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fd6073fa4bd..490fab4b914 100644 --- a/src/main/fc/settings.yaml +++ b/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 @@ -3824,6 +3829,7 @@ groups: condition: USE_OSD || USE_DJI_HD_OSD members: - name: osd_speed_source + condition: USE_GEOZONE description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" default_value: "GROUND" field: speedSource @@ -4362,3 +4368,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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 41c20dda928..0307f63c1ab 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,6 +866,8 @@ 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: + return OSD_MESSAGE_STR(OSD_MSG_NFZ); // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; @@ -2377,6 +2379,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!"; @@ -3837,6 +3844,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; @@ -5890,6 +5943,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()); @@ -5952,6 +6011,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } +#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_LOITER: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b0423d40eff..69d5433e0a5 100644 --- a/src/main/io/osd.h +++ b/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, @@ -291,7 +304,10 @@ typedef enum { OSD_CUSTOM_ELEMENT_5, OSD_CUSTOM_ELEMENT_6, OSD_CUSTOM_ELEMENT_7, - OSD_CUSTOM_ELEMENT_8, // 158 + OSD_CUSTOM_ELEMENT_8, + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -474,6 +490,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); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b018d8f9068..c0120c7faa1 100644 --- a/src/main/io/osd_dji_hd.c +++ b/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: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 465f5098536..dcbdeb5e71f 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/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 \ No newline at end of file +#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 \ No newline at end of file diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4ae4cf228db..bbf8e7910f0 100644 --- a/src/main/navigation/navigation.c +++ b/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, } }, @@ -1178,6 +1189,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 +1512,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,6 +1682,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { +#ifdef USE_GEOZONE + // Check for NFZ in our way + int8_t wpCount = geozoneCheckForNFZAtCourse(true); + if (wpCount > 0) { + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + 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)) { @@ -1626,6 +1728,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio 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 +1902,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 +2561,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 +2706,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 +2724,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 +2934,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 +3132,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 +3347,9 @@ void updateHomePosition(void) setHome = true; break; } +#if defined(USE_GEOZONE) && defined (USE_GPS) + geozoneUpdateMaxHomeAltitude(); +#endif } } else { @@ -3409,7 +3605,14 @@ bool isProbablyStillFlying(void) * Z-position controller *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) -{ +{ +#ifdef USE_GEOZONE + if ((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 + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; @@ -4095,6 +4298,10 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; +#endif return; } @@ -4145,7 +4352,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 +4503,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } + if (posControl.flags.sendToActive) { + return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; + } + + if (posControl.flags.forcedPosholdActive) { + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + } + /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission * auto restarting after interruption by Manual or RTH modes. @@ -4769,6 +4984,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 +5074,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 *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 24b8daa0402..ad6d60ee06f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -116,6 +116,119 @@ void resetFwAutolandApproach(int8_t idx); #endif +#if defined(USE_GEOZONE) + +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 + +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_LOITER +} 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; + 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 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 geozoneResetVertices(int8_t zoneId, int16_t idx); +void geozoneUpdate(timeUs_t curentTimeUs); +bool geozoneIsInsideNFZ(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 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ff38b2e17e5..87165cd7d6f 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/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; } diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c new file mode 100755 index 00000000000..8a2505a0ff9 --- /dev/null +++ b/src/main/navigation/navigation_geozone.c @@ -0,0 +1,1598 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#include "common/utils.h" +#include "common/vector.h" +#include "common/printf.h" + +#include "config/config_reset.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" + +#include "flight/imu.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#if defined(USE_GEOZONE) && defined (USE_GPS) + +#include "navigation_geozone_calculations.h" + +#define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) +#define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home + +#define MAX_DISTANCE_FLY_OVER_POINTS 50000 +#define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) +#define POS_DETECTION_DISTANCE 7500 +#define STICK_LOCK_MIN_TIME 1500 +#define AVOID_TIMEOUT 30000 +#define MAX_LOCAL_VERTICES 128 +#define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m +#define STICK_MOVE_THRESHOULD 40 +#define MAX_RTH_WAYPOINTS (MAX_VERTICES / 2) +#define GEOZONE_INACTIVE INT8_MAX +#define RTH_OVERRIDE_TIMEOUT 1000 + +#define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) + +typedef enum { + GEOZONE_ACTION_STATE_NONE, + GEOZONE_ACTION_STATE_AVOIDING, + GEOZONE_ACTION_STATE_AVOIDING_UPWARD, + GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE, + GEOZONE_ACTION_STATE_RETURN_TO_FZ, + GEOZONE_ACTION_STATE_FLYOUT_NFZ, + GEOZONE_ACTION_STATE_POSHOLD, + GEOZONE_ACTION_STATE_RTH +} geozoneActionState_e; + +typedef struct geoZoneRuntimeConfig_s +{ + geoZoneConfig_t config; + bool enable; + bool isInfZone; + uint32_t radius; + fpVector2_t *verticesLocal; +} geoZoneRuntimeConfig_t; + +typedef struct pathPoint_s pathPoint_t; +struct pathPoint_s { + fpVector3_t point; + pathPoint_t* prev; + float distance; + bool visited; +}; + +static bool isInitalised = false; +static geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; +static fpVector2_t verticesLocal[MAX_VERTICES]; +static uint8_t currentZoneCount = 0; + +static bool isAtLeastOneInclusiveZoneActive = false; +static geoZoneRuntimeConfig_t activeGeoZones[MAX_GEOZONES]; +static uint8_t activeGeoZonesCount = 0; +static geoZoneConfig_t safeHomeGeozoneConfig; +static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; +static timeMs_t actionStartTime = 0; +static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; +static fpVector3_t avoidingPoint; +static bool geozoneIsEnabled = false; +static fpVector3_t rthWaypoints[MAX_RTH_WAYPOINTS]; +static uint8_t rthWaypointIndex = 0; +static int8_t rthWaypointCount = 0; +static bool aboveOrUnderZone = false; +static timeMs_t rthOverrideStartTime; +static bool noZoneRTH = false; +static bool rthHomeSwitchLastState = false; +static bool lockRTZ = false; + +geozone_t geozone; + +PG_REGISTER_WITH_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, PG_GEOZONE_CONFIG, 0); + +PG_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, + .fenceDetectionDistance = SETTING_GEOZONE_DETECTION_DISTANCE_DEFAULT, + .avoidAltitudeRange = SETTING_GEOZONE_AVOID_ALTITUDE_RANGE_DEFAULT, + .safeAltitudeDistance = SETTING_GEOZONE_SAFE_ALTITUDE_DISTANCE_DEFAULT, + .nearestSafeHomeAsInclusivZone = SETTING_GEOZONE_SAFEHOME_AS_INCLUSIVE_DEFAULT, + .copterFenceStopDistance = SETTING_GEOZONE_MR_STOP_DISTANCE_DEFAULT, + .noWayHomeAction = SETTING_GEOZONE_NO_WAY_HOME_ACTION_DEFAULT +); + +PG_REGISTER_ARRAY_WITH_RESET_FN(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig, PG_GEOZONES, 1); + +void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) +{ + for (int i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + RESET_CONFIG(geoZoneConfig_t, &instance[i], + .shape = GEOZONE_TYPE_EXCLUSIVE, + .type = GEOZONE_SHAPE_CIRCULAR, + .minAltitude = 0, + .maxAltitude = 0, + .fenceAction = GEOFENCE_ACTION_NONE, + .vertexCount = 0 + ); + } +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices, PG_GEOZONE_VERTICES, 0); + +void pgResetFn_geoZoneVertices(vertexConfig_t *instance) +{ + for (int i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + RESET_CONFIG(vertexConfig_t, &instance[i], + .zoneId = -1, + .idx = 0, + .lat = 0, + .lon = 0 + ); + } +} + +uint8_t geozoneGetUsedVerticesCount(void) +{ + uint8_t count = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + count += geoZonesConfig(i)->vertexCount; + } + return count; +} + +void geozoneResetVertices(const int8_t zoneId, const int16_t idx) +{ + if (zoneId < 0 && idx < 0) { + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + } + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx < 0) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx >= 0 && idx < MAX_VERTICES_IN_CONFIG) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == idx) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + break; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount--; + } + } +} + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon) +{ + if (vertexId < MAX_VERTICES_IN_CONFIG) + { + int16_t vertexConfigIdx = -1; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexConfigIdx = i; + break; + } + } + if (vertexConfigIdx >= 0) { + geoZoneVerticesMutable(vertexConfigIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexConfigIdx)->idx = vertexId; + geoZoneVerticesMutable(vertexConfigIdx)->lat = lat; + geoZoneVerticesMutable(vertexConfigIdx)->lon = lon; + return true; + } + } + return false; +} + +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) +{ + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexId) { + return i; + } + } + return -1; +} + +static bool areSticksDeflectdFromChannelValue(void) +{ + return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; +} + +static bool isNonGeozoneModeFromBoxInput(void) +{ + return !(IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)); +} + +static bool isPointOnBorder(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos) +{ + fpVector2_t *prev = &zone->verticesLocal[zone->config.vertexCount -1]; + fpVector2_t *current; + bool isOnBorder = false; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + current = &zone->verticesLocal[i]; + if (isPointOnLine2(prev, current, (fpVector2_t*)pos)) { + isOnBorder = true; + break; + } + prev = current; + } + + if (isOnBorder) { + return (pos->z >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos->z <= zone->config.maxAltitude; + } + + return isOnBorder; +} + +static bool isInZoneAltitudeRange(geoZoneRuntimeConfig_t *zone, const float pos) +{ + return (pos >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos <= zone->config.maxAltitude; +} + +static bool isInGeozone(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos, bool ignoreAltitude) +{ + if (activeGeoZonesCount == 0) { + return false; + } + + bool isIn2D = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + isIn2D = isPointInPloygon((fpVector2_t*)pos, zone->verticesLocal, zone->config.vertexCount) || isPointOnBorder(zone, pos); + } else { // cylindric + isIn2D = isPointInCircle((fpVector2_t*)pos, &zone->verticesLocal[0], zone->radius); + } + + if (isIn2D && !ignoreAltitude) { + return isInZoneAltitudeRange(zone, pos->z); + } + + return isIn2D; +} + +static bool isPointInAnyOtherZone(const geoZoneRuntimeConfig_t *zone, uint8_t type, const fpVector3_t *pos) +{ + bool isInZone = false; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (zone != &activeGeoZones[i] && activeGeoZones[i].config.type == type && isInGeozone(&activeGeoZones[i], pos, false)) { + isInZone = true; + break; + } + } + return isInZone; +} + +static uint8_t getZonesForPos(geoZoneRuntimeConfig_t *zones[], const fpVector3_t *pos, const bool ignoreAltitude) +{ + uint8_t count = 0; + for (int i = 0; i < activeGeoZonesCount; i++) { + if (isInGeozone(&activeGeoZones[i], pos, ignoreAltitude)) { + zones[count++] = &activeGeoZones[i]; + } + } + return count; +} + +static uint8_t getCurrentZones(geoZoneRuntimeConfig_t *zones[], const bool ignoreAltitude) +{ + return getZonesForPos(zones, &navGetCurrentActualPositionAndVelocity()->pos, ignoreAltitude); +} + +static int geoZoneRTComp(const void *a, const void *b) +{ + geoZoneRuntimeConfig_t *zoneA = (geoZoneRuntimeConfig_t*)a; + geoZoneRuntimeConfig_t *zoneB = (geoZoneRuntimeConfig_t*)b; + + if (zoneA->enable == zoneB->enable) { + return 0; + } else if (zoneA->enable) { + return -1; + } else { + return 1; + } +} + +// in cm and cms/s +static uint32_t calcTime(const int32_t distance, const int32_t speed) +{ + if (speed <= 0) { + return 0; + } + + return distance / speed; +} + +static void calcPreviewPoint(fpVector3_t *target, const int32_t distance) +{ + calculateFarAwayTarget(target, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), distance); + target->z = getEstimatedActualPosition(Z) + calcTime(geoZoneConfig()->fenceDetectionDistance, gpsSol.groundSpeed) * getEstimatedActualVelocity(Z); +} + +static bool calcIntersectionForZone(fpVector3_t *intersection, float *distance, geoZoneRuntimeConfig_t *zone, const fpVector3_t *start, const fpVector3_t *end) +{ + bool hasIntersection = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + if (calcLine3dPolygonIntersection( + intersection, + distance, + start, + end, + zone->verticesLocal, + zone->config.vertexCount, + zone->config.minAltitude, + zone->config.maxAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + fpVector3_t circleCenter = { .x = zone->verticesLocal[0].x, .y = zone->verticesLocal[0].y, .z = zone->config.minAltitude }; + if (calcLineCylinderIntersection( + intersection, + distance, + start, + end, + &circleCenter, + zone->radius, + zone->config.maxAltitude - zone->config.minAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } + + if (hasIntersection && isPointOnLine3(start, end, intersection)){ + return true; + } + *distance = -1; + return false; +} + +static int32_t calcBouceCoursePolygon(const int32_t course, const fpVector2_t* pos, const fpVector2_t *intersect, const fpVector2_t* p1, const fpVector2_t* p2) +{ + int32_t newCourse = 0; + int32_t angelp1 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p1, pos, intersect)); + int32_t angelp2 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p2, pos, intersect)); + if (angelp1 < angelp2) { + if (isPointRightFromLine(pos, intersect, p1)) { + newCourse = course - 2 * angelp1; + } + else { + newCourse = course + 2 * angelp1; + } + } + else { + if (isPointRightFromLine(pos, intersect, p2)) { + newCourse = course - 2 * angelp2; + } + else { + newCourse = course + 2 * angelp2; + } + } + return wrap_36000(newCourse); +} + +static int32_t calcBouceCourseCircle(const int32_t course, const fpVector2_t* pos, const fpVector2_t* intersect, const fpVector2_t* mid) +{ + int32_t newCourse = 0; + int32_t angel = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(mid, pos, intersect)); + if (isPointRightFromLine(pos, mid, intersect)) { + newCourse = course + 2 * (angel - 9000); + } + else { + newCourse = course - 2 * (angel - 9000); + } + return wrap_36000(newCourse); +} + +static bool findNearestIntersectionZone(geoZoneRuntimeConfig_t **intersectZone, fpVector3_t *intersection, float *distance, const float detectionDistance, const fpVector3_t *start, const fpVector3_t *end) +{ + geoZoneRuntimeConfig_t *zone = NULL; + fpVector3_t intersect; + float distanceToZone = FLT_MAX; + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + float currentDistance = FLT_MAX; + if (!calcIntersectionForZone( + ¤tIntersect, + ¤tDistance, + &activeGeoZones[i], + start, + end)) { + continue; + } + + if (currentDistance < distanceToZone) { + distanceToZone = currentDistance; + zone = &activeGeoZones[i]; + intersect = currentIntersect; + } + } + + if (zone && distanceToZone < detectionDistance) { + *intersectZone = zone; + *distance = distanceToZone; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + + return false; +} + +static bool isPointDirectReachable(const fpVector3_t* start, const fpVector3_t *point) +{ + float currentDistance = 0; + bool pointIsInOverlappingZone = false, pointIsInExclusiveZone = false, hasIntersection = false; + + /* + if (start->x == point->x && start->y == point->y) { + return false; + } + */ + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + + if (!calcIntersectionForZone(¤tIntersect, ¤tDistance, &activeGeoZones[i], start, point)) { + continue; + } + hasIntersection = true; + + // Intersct a exclusive Zone? + geoZoneRuntimeConfig_t *intersectZones[MAX_GEOZONES]; + uint8_t intersectZonesCount = getZonesForPos(intersectZones, ¤tIntersect, false); + for (uint8_t j = 0; j < intersectZonesCount; j++) { + if (intersectZones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE ) { + pointIsInExclusiveZone = true; + break; + } + } + + if (pointIsInExclusiveZone) { + break; + } + + // We targeting a exit point (in min two zones) + if (intersectZonesCount < 2) { + break; + } + + geoZoneRuntimeConfig_t *startZones[MAX_GEOZONES]; + uint8_t startZonesCount = getZonesForPos(startZones, start, false); + geoZoneRuntimeConfig_t *endZones[MAX_GEOZONES]; + uint8_t endZonesCount = getZonesForPos(endZones, point, false); + + for (uint8_t j = 0; j < intersectZonesCount; j++) { + for (uint8_t k = 0; k < startZonesCount; k++) { + for (uint8_t l = 0; l < endZonesCount; l++) { + if (intersectZones[j] == startZones[k] && intersectZones[j] == endZones[l]) { + pointIsInOverlappingZone = true; + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + } + + return !pointIsInExclusiveZone && (!hasIntersection || pointIsInOverlappingZone); +} + +uint32_t geozoneGetDetectionDistance(void) +{ + uint32_t detctionDistance = 0; + if (STATE(AIRPLANE)) { + detctionDistance = navConfig()->fw.loiter_radius * 1.5; + } else { + detctionDistance = geoZoneConfig()->copterFenceStopDistance; + } + return detctionDistance; +} + +static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) +{ + int32_t course; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t intersect; + bool found = false; + fpVector2_t *p1 = &zone->verticesLocal[zone->config.vertexCount - 1]; + fpVector2_t *p2; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + p2 = &zone->verticesLocal[i]; + if (calcLineLineIntersection(&intersect, p1, p2, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)prevPoint, true)) { + found = true; + break; + } + p1 = p2; + } + + if (!found) { + return -1; + } + course = calcBouceCoursePolygon(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &intersect, p1, p2); + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + course = calcBouceCourseCircle(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)intersection, &zone->verticesLocal[0]); + } + return course; +} + +static bool initPathPoint(pathPoint_t *pathPoints, const fpVector3_t pos, uint8_t *idx) +{ + if (*idx + 1 >= MAX_PATH_PONITS) { + return false; + } + + pathPoints[*idx].distance = FLT_MAX; + pathPoints[*idx].visited = false; + pathPoints[(*idx)++].point = pos; + + return true; +} + +static bool isPosInGreenAlt(geoZoneRuntimeConfig_t *zones[], const uint8_t zoneCount, const float alt) +{ + bool isInNfz = false, isInFz = false; + for (uint8_t j = 0; j < zoneCount; j++) { + if(isInZoneAltitudeRange(zones[j], alt)){ + isInFz = zones[j]->config.type == GEOZONE_TYPE_INCLUSIVE; + isInNfz = zones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE; + } + } + return !isInNfz && (!isAtLeastOneInclusiveZoneActive || isInFz); +} + +static bool checkPathPointOrSetAlt(fpVector3_t *pos) +{ + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, pos, true); + + if (zoneCount == 0) { + return !isAtLeastOneInclusiveZoneActive; + } + + if (zoneCount == 1 && zones[0]->config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + + bool isInExclusice = false; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(zones[i], pos, false)) { + isInExclusice = true; + if (zones[i]->config.minAltitude != 0) { + float min = zones[i]->config.minAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + if (isPosInGreenAlt(zones, zoneCount, min)) { + pos->z = min; + return true; + } + } + + if (!zones[i]->isInfZone || zones[i]->config.maxAltitude < INT32_MAX) { + float max = zones[i]->config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + if(isPosInGreenAlt(zones, zoneCount, max)) { + pos->z = max; + return true; + } + } + } + } + + return !isInExclusice; +} + +// Return value: 0 - Target direct reachable; -1 No way; >= 1 Waypoints to target +#define CIRCLE_POLY_SIDES 6 +static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fpVector3_t* target) +{ + pathPoint_t pathPoints[MAX_PATH_PONITS]; + uint8_t pathPointCount = 0, waypointCount = 0; + fpVector3_t start = *point; + + if (isPointDirectReachable(&start, target)) { + return 0; + } + + float offset = geozoneGetDetectionDistance(); + if (geozone.distanceVertToNearestZone <= offset) { + int bearing = wrap_36000(geozone.directionToNearestZone + 18000); + start.x += offset * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); + start.y += offset * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); + } + + pathPoints[pathPointCount].visited = true; + pathPoints[pathPointCount].distance = 0; + pathPoints[pathPointCount++].point = start; + + for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { + fpVector2_t *verticesZone; + fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; + uint8_t verticesZoneCount; + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + verticesZone = activeGeoZones[i].verticesLocal; + verticesZoneCount = activeGeoZones[i].config.vertexCount; + } else { + generatePolygonFromCircle(verticesCirclePoly, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius, CIRCLE_POLY_SIDES); + verticesZone = verticesCirclePoly; + verticesZoneCount = CIRCLE_POLY_SIDES; + } + + fpVector2_t safeZone[MAX_VERTICES]; + offset = geozoneGetDetectionDistance() * 2 / 3; + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + offset *= -1; + } + + float zMin = start.z, zMax = 0; + if (!isInZoneAltitudeRange(&activeGeoZones[i], start.z) && activeGeoZones[i].config.minAltitude > 0) { + zMin = activeGeoZones[i].config.minAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + } else if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + generateOffsetPolygon(safeZone, verticesZone, verticesZoneCount, offset); + fpVector2_t *prev = &safeZone[verticesZoneCount - 1]; + fpVector2_t *current; + for (uint8_t j = 0; j < verticesZoneCount; j++) { + current = &safeZone[j]; + + if (zMax > 0 ) { + fpVector3_t max = { .x = current->x, .y = current->y, .z = zMax }; + if (checkPathPointOrSetAlt(&max)) { + if (!initPathPoint(pathPoints, max, &pathPointCount)) { + return -1; + } + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + // Set some "fly over points" + float dist = calculateDistance2(prev, current); + if (dist > MAX_DISTANCE_FLY_OVER_POINTS) { + uint8_t sectionCount = (uint8_t)(dist / MAX_DISTANCE_FLY_OVER_POINTS); + float dist = MAX_DISTANCE_FLY_OVER_POINTS; + for (uint8_t k = 0; k < sectionCount; k++) { + fpVector3_t flyOverPoint; + calcPointOnLine((fpVector2_t*)&flyOverPoint, prev, current, dist); + fpVector3_t maxFo = { .x = flyOverPoint.x, .y = flyOverPoint.y, .z = zMax }; + if (checkPathPointOrSetAlt(&maxFo)) { + if (!initPathPoint(pathPoints, maxFo, &pathPointCount)) { + return -1; + } + } + dist += MAX_DISTANCE_FLY_OVER_POINTS; + } + } + } + } + + if (zMin > 0) { + fpVector3_t min = { .x = current->x, .y = current->y, .z = zMin }; + if (checkPathPointOrSetAlt(&min)) { + if (!initPathPoint(pathPoints, min, &pathPointCount)) { + return -1; + } + } + + } + prev = current; + } + } + + if (!initPathPoint(pathPoints, *target, &pathPointCount)) { + return -1; + } + + // Dijkstra + pathPoint_t *current = pathPoints; + while (!pathPoints[pathPointCount - 1].visited) { + pathPoint_t *next = current; + float min = FLT_MAX; + for (uint16_t i = 1; i < pathPointCount; i++) { + + float currentDist = FLT_MAX; + if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { + float dist2D = calculateDistance2((fpVector2_t*)¤t->point, (fpVector2_t*)&pathPoints[i].point); + float distAlt = ABS(current->point.z - pathPoints[i].point.z); + currentDist = current->distance + dist2D + 2 * distAlt; + } + + if (currentDist < pathPoints[i].distance && !pathPoints[i].visited) { + pathPoints[i].distance = currentDist; + pathPoints[i].prev = current; + } + if (min > pathPoints[i].distance && !pathPoints[i].visited) { + min = pathPoints[i].distance; + next = &pathPoints[i]; + } + } + + if (min == FLT_MAX) { + return -1; + } + + current = next; + current->visited = true; + } + + waypointCount = 0; + current = &pathPoints[pathPointCount - 1]; + while (current != pathPoints) { + waypointCount++; + current = current->prev; + } + // Don't set home to the WP list + current = pathPoints[pathPointCount - 1].prev; + uint8_t i = waypointCount - 2; + while (current != pathPoints) { + waypoints[i] = current->point; + current = current->prev; + i--; + } + return waypointCount - 1; +} + +static void updateCurrentZones(void) +{ + currentZoneCount = getCurrentZones(currentZones, false); + geozone.insideNfz = false; + geozone.insideFz = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE) { + geozone.insideNfz = true; + } + if (currentZones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + geozone.insideFz = true; + } + } +} + +static void updateZoneInfos(void) +{ + float nearestDistanceToBorder = FLT_MAX; + fpVector3_t nearestBorderPoint; + aboveOrUnderZone = false; + + geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; + uint8_t currentZoneCount = getCurrentZones(currentZones, true); + int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; + + if (currentZoneCount == 1) { + currentMaxAltitude = currentZones[0]->config.maxAltitude; + currentMinAltitude = currentZones[0]->config.minAltitude; + + if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { + nearestHorInclZone = currentZones[0]; + } + + } else if (currentZoneCount >= 2) { + + geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; + float distAbove = FLT_MAX, distBelow = FLT_MAX; + geoZoneRuntimeConfig_t *current = NULL; + for (uint8_t i = 0; i < currentZoneCount; i++) { + current = currentZones[i]; + + if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { + currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); + currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + } + + if (current->config.minAltitude > getEstimatedActualPosition(Z)) { + float dist = current->config.maxAltitude - getEstimatedActualPosition(Z); + if (dist < distAbove) { + aboveZone = current; + distAbove = dist; + } + } + + if (current->config.maxAltitude < getEstimatedActualPosition(Z)) { + float dist = getEstimatedActualPosition(Z) - current->config.maxAltitude; + if (dist < distBelow) { + belowZone = current; + distBelow = dist; + } + } + } + + if (aboveZone) { + if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); + nearestHorInclZone = aboveZone; + } else { + currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); + } + } + + if (belowZone) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); + nearestHorInclZone = belowZone; + } else { + currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); + } + } + } + + if (currentMinAltitude == INT32_MAX) { + currentMinAltitude = 0; + } + + if (currentMaxAltitude == INT32_MIN) { + currentMaxAltitude = 0; + } + + if (currentMaxAltitude == INT32_MAX && currentMinAltitude != 0) { + geozone.distanceVertToNearestZone = ABS(currentMinAltitude - getEstimatedActualPosition(Z)); + } else if (currentMinAltitude == 0 && currentMaxAltitude != 0) { + geozone.distanceVertToNearestZone = currentMaxAltitude - getEstimatedActualPosition(Z); + } else if (currentMinAltitude != 0 && currentMaxAltitude > 0) { + int32_t distToMin = currentMinAltitude - getEstimatedActualPosition(Z); + int32_t distToMax = currentMaxAltitude - getEstimatedActualPosition(Z); + if (getEstimatedActualPosition(Z) > currentMinAltitude && getEstimatedActualPosition(Z) < currentMaxAltitude) { + if (ABS(distToMin) < ABS(currentMaxAltitude - currentMinAltitude) / 2 ) { + geozone.distanceVertToNearestZone = distToMin; + } else { + geozone.distanceVertToNearestZone = distToMax; + } + } else { + geozone.distanceVertToNearestZone = MIN(ABS(distToMin), distToMax); + } + } else { + geozone.distanceVertToNearestZone = 0; + } + + if (currentZoneCount == 0) { + geozone.currentzoneMaxAltitude = 0; + geozone.currentzoneMinAltitude = 0; + } else { + + if (getEstimatedActualPosition(Z) < currentMinAltitude || getEstimatedActualPosition(Z) > currentMaxAltitude) { + aboveOrUnderZone = true; + } + + if (currentMaxAltitude > 0) { + geozone.currentzoneMaxAltitude = currentMaxAltitude - geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMaxAltitude = 0; + } + + if (currentMinAltitude > 0) { + geozone.currentzoneMinAltitude = currentMinAltitude + geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMinAltitude = 0; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) + { + // Ignore NFZ for now, we want back fo the FZ, we will check for NFZ later at RTH + if (currentZoneCount == 0 && isAtLeastOneInclusiveZoneActive && activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + continue; + } + + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t* prev = &activeGeoZones[i].verticesLocal[activeGeoZones[i].config.vertexCount - 1]; + fpVector2_t* current = NULL; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + current = &activeGeoZones[i].verticesLocal[j]; + fpVector2_t pos = calcNearestPointOnLine(prev, current, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos); + float dist = calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &pos); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + nearestBorderPoint.x = pos.x; + nearestBorderPoint.y = pos.y; + nearestBorderPoint.z = getEstimatedActualPosition(Z); + geozone.directionToNearestZone = calculateBearingToDestination(&nearestBorderPoint); + geozone.distanceHorToNearestZone = roundf(nearestDistanceToBorder); + nearestInclusiveZone = &activeGeoZones[i]; + } + prev = current; + } + } else if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + float dist = fabsf(calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0]) - activeGeoZones[i].radius); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + int32_t directionToBorder = calculateBearingToDestination((fpVector3_t*)&activeGeoZones[i].verticesLocal[0]); + + if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { + directionToBorder = wrap_36000(directionToBorder + 18000); + } + + geozone.directionToNearestZone = directionToBorder; + geozone.distanceHorToNearestZone = roundf(dist); + nearestInclusiveZone = &activeGeoZones[i]; + } + } + + if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorInclZone; + geozone.distanceHorToNearestZone = 0; + } + } +} + +void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) +{ + // Actions only for assisted modes now + if (isNonGeozoneModeFromBoxInput() || geozone.avoidInRTHInProgress || (calculateDistanceToDestination(intersection) > geozoneGetDetectionDistance())) { + return; + } + + int32_t alt = roundf(intersection->z); + if (alt == zone->config.maxAltitude || alt == zone->config.minAltitude) { + return; + } + + fpVector3_t prevPoint; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + + if (zone->config.fenceAction == GEOFENCE_ACTION_AVOID) { + bool avoidFzStep = false; + float fzStepAlt = 0; + if (zone->config.type == GEOZONE_TYPE_INCLUSIVE) { + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES_IN_CONFIG]; + uint8_t zoneCount = getZonesForPos(zones, intersection, true); + + float maxAlt = FLT_MAX; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE && zones[i]->config.minAltitude > intersection->z) { + float alt = zones[i]->config.minAltitude; + if (alt < maxAlt) { + maxAlt = alt; + } + } + } + + if (maxAlt < FLT_MAX) { + fzStepAlt = maxAlt + geoZoneConfig()->safeAltitudeDistance * 2; + avoidFzStep = true; + } + } + // We can move upwards + if ((zone->config.type == GEOZONE_TYPE_EXCLUSIVE && geozone.zoneInfo > 0 && (geozone.zoneInfo < geoZoneConfig()->avoidAltitudeRange)) || avoidFzStep) { + + calculateFarAwayTarget(&posControl.sendTo.targetPos, posControl.actualState.cog, geoZoneConfig()->fenceDetectionDistance * 2); + if (avoidFzStep) { + posControl.sendTo.targetPos.z = fzStepAlt; + } else { + posControl.sendTo.targetPos.z = zone->config.maxAltitude + geoZoneConfig()->safeAltitudeDistance * 2; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING_UPWARD; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } else { + // Calc new course + avoidCourse = calcBounceCourseForZone(zone, &prevPoint, intersection); + + if (avoidCourse == -1) { + return; + } + + calculateFarAwayTarget(&posControl.sendTo.targetPos, avoidCourse, geoZoneConfig()->fenceDetectionDistance * 2); // Its too far, mode will be abort if we are on the right course + + // Check for min/max altitude + if (geozone.currentzoneMaxAltitude > 0 && getEstimatedActualPosition(Z) > geozone.currentzoneMaxAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 0.25; + } else if (geozone.currentzoneMinAltitude != 0 && getEstimatedActualPosition(Z) < geozone.currentzoneMinAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 0.25; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = AVOID_TIMEOUT; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } + } else if (zone->config.fenceAction == GEOFENCE_ACTION_POS_HOLD) { + actionState = GEOZONE_ACTION_STATE_POSHOLD; + + if (STATE(AIRPLANE)) { + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector3_t refPoint; + int32_t course = calcBounceCourseForZone(zone, &prevPoint, intersection); + calculateFarAwayTarget(&refPoint, course, geoZoneConfig()->fenceDetectionDistance * 2); + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, (fpVector2_t*)&refPoint)) { + geozone.loiterDir = 1; // Right + } else { + geozone.loiterDir = -1; // Left + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, &zone->verticesLocal[0])) { + geozone.loiterDir = -1; // Left + } else { + geozone.loiterDir = 1; // Right + } + } + } + + geozone.sticksLocked = true; + activateForcedPosHold(); + actionStartTime = millis(); + } else if (zone->config.fenceAction == GEOFENCE_ACTION_RTH) { + actionState = GEOZONE_ACTION_STATE_RTH; + geozone.sticksLocked = true; + activateForcedRTH(); + actionStartTime = millis(); + } +} + +static void endFenceAction(void) +{ + posControl.sendTo.lockSticks = false; + geozone.sticksLocked = false; + geozone.sticksLocked = 0; + + switch (actionState) { + case GEOZONE_ACTION_STATE_AVOIDING: + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + abortSendTo(); + break; + case GEOZONE_ACTION_STATE_POSHOLD: + abortForcedPosHold(); + break; + case GEOZONE_ACTION_STATE_RTH: + abortForcedRTH(); + break; + default: + break; + } + + actionState = GEOZONE_ACTION_STATE_NONE; + + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)){ + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_Z); + } + + abortSendTo(); +} + +static void geoZoneInit(void) +{ + activeGeoZonesCount = 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[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; + } + + activeGeoZones[activeGeoZonesCount].enable = true; + activeGeoZonesCount++; + } + } + + if (activeGeoZonesCount > 0) { + // Covert geozone vertices to local + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + 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) { + 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; + 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]; + } + } + } + } + + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) + { + safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; + safeHomeGeozoneConfig.type = GEOZONE_TYPE_INCLUSIVE; + safeHomeGeozoneConfig.fenceAction = geoZoneConfig()->safeHomeFenceAction; + safeHomeGeozoneConfig.maxAltitude = 0; + safeHomeGeozoneConfig.minAltitude = 0; + safeHomeGeozoneConfig.vertexCount = 1; + + activeGeoZones[activeGeoZonesCount].config = safeHomeGeozoneConfig; + activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; + activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; + activeGeoZonesCount++; + } + + updateCurrentZones(); + uint8_t newActiveZoneCount = activeGeoZonesCount; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (!geozone.insideFz) { + // Deactivate all inclusive geozones with distance > GEOZONE_INCLUSE_IGNORE_DISTANCE + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && !isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + fpVector2_t pos2 = { .x = getEstimatedActualPosition(X), .y = getEstimatedActualPosition(Y) }; + + uint32_t minDistanceToZone = INT32_MAX; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + float dist = calculateDistance2(&pos2, &activeGeoZones[i].verticesLocal[j]); + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + minDistanceToZone = dist - activeGeoZones[i].radius; + break; + } + + if (dist < minDistanceToZone) { + minDistanceToZone = dist; + } + } + if (minDistanceToZone > GEOZONE_INCLUSE_IGNORE_DISTANCE) { + activeGeoZones[i].enable = false; + newActiveZoneCount--; + } + } + } + } + + activeGeoZonesCount = newActiveZoneCount; + if (activeGeoZonesCount == 0) { + setTaskEnabled(TASK_GEOZONE, false); + geozoneIsEnabled = false; + return; + } + geozoneIsEnabled = true; + + qsort(activeGeoZones, MAX_GEOZONES, sizeof(geoZoneRuntimeConfig_t), geoZoneRTComp); + + for (int i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + isAtLeastOneInclusiveZoneActive = true; + break; + } + } +} + +// Called by Scheduler +void geozoneUpdate(timeUs_t curentTimeUs) +{ + UNUSED(curentTimeUs); + + geozone.messageState = GEOZONE_MESSAGE_STATE_NONE; + if (!isInitalised && navigationPositionEstimateIsHealthy()) { + geoZoneInit(); + isInitalised = true; + } + + if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + noZoneRTH = false; + return; + } + + // Zone RTH Override: Toggle RTH switch short + if (geozone.avoidInRTHInProgress) { + if (rthHomeSwitchLastState != IS_RC_MODE_ACTIVE(BOXNAVRTH)) { + if (millis() - rthOverrideStartTime < RTH_OVERRIDE_TIMEOUT) { + geozoneResetRTH(); + noZoneRTH = true; + } + rthOverrideStartTime = millis(); + } + rthHomeSwitchLastState = IS_RC_MODE_ACTIVE(BOXNAVRTH); + } + + updateCurrentZones(); + updateZoneInfos(); + + // User has switched to non geofence mode, end all actions and switch to mode from box input + if (isNonGeozoneModeFromBoxInput()) { + if (actionState != GEOZONE_ACTION_STATE_NONE) { + endFenceAction(); + } + lockRTZ = false; + return; + } + + switch (actionState) + { + case GEOZONE_ACTION_STATE_AVOIDING: + if (calculateDistanceToDestination(&avoidingPoint) > geozoneGetDetectionDistance()) { + posControl.sendTo.lockSticks = false; + } + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(avoidCourse, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), 500) || millis() - actionStartTime > AVOID_TIMEOUT || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE; + if ((geozone.insideFz && ABS(geozone.distanceVertToNearestZone ) > geoZoneConfig()->safeAltitudeDistance) || !posControl.flags.sendToActive) { + lockRTZ = true; + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_FLYOUT_NFZ; + if (!geozone.insideNfz || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_UPWARD: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RTH: + case GEOZONE_ACTION_STATE_POSHOLD: + geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { + geozone.sticksLocked = false; + } + if (!geozone.sticksLocked && areSticksDeflectdFromChannelValue()) { + endFenceAction(); + } + return; + default: + break; + } + + bool currentZoneHasAction = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + currentZoneHasAction = true; + break; + } + } + + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + + float targetAltitide = 0; + if (!geozone.insideFz) { + if (geozone.distanceVertToNearestZone < 0) { + targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + } else { + targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + } else { + targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + + fpVector3_t targetPos; + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitide; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + actionState = GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE; + actionStartTime = millis(); + activateSendTo(); + return; + } + + // RTH active: Further checks are done in RTH logic + if ((navGetCurrentStateFlags() & NAV_AUTO_RTH) || IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated || FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return; + } else if (geozone.avoidInRTHInProgress) { + geozoneResetRTH(); + } + + if (lockRTZ && (geozone.insideFz || geozone.insideNfz)) { + lockRTZ = false; + } + + if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { + + if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_OUTSIDE_FZ; + } + + if (geozone.insideNfz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_NFZ; + } + + if (!isNonGeozoneModeFromBoxInput()) { + bool flyOutNfz = false; + geoZoneRuntimeConfig_t *targetZone = nearestInclusiveZone; + + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + flyOutNfz = true; + targetZone = currentZones[i]; + break; + } + } + + if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { + int32_t targetAltitude = 0; + if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5; + } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z); + } + + fpVector3_t targetPos; + if (aboveOrUnderZone) { + if (ABS(geozone.distanceVertToNearestZone) < 2000) { + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + if(geozone.distanceVertToNearestZone > 0) { + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5; + } + + } else { + targetPos = navGetCurrentActualPositionAndVelocity()->pos; + } + } else { + calculateFarAwayTarget(&targetPos, geozone.directionToNearestZone, geozone.distanceHorToNearestZone + geoZoneConfig()->fenceDetectionDistance / 2); + } + + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitude; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + + if (flyOutNfz) { + actionState = GEOZONE_ACTION_STATE_FLYOUT_NFZ; + } else { + actionState = GEOZONE_ACTION_STATE_RETURN_TO_FZ; + } + + activateSendTo(); + } + } + } + + + geoZoneRuntimeConfig_t *intersectZone = NULL; + fpVector3_t intersection, prevPoint; + float distanceToZone = 0; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + if (findNearestIntersectionZone(&intersectZone, &intersection, &distanceToZone, geoZoneConfig()->fenceDetectionDistance, &navGetCurrentActualPositionAndVelocity()->pos, &prevPoint)) { + if (geozone.insideFz) { + if (!isPointInAnyOtherZone(intersectZone, GEOZONE_TYPE_INCLUSIVE, &intersection)) { + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + geozone.messageState = GEOZONE_MESSAGE_STATE_LEAVING_FZ; + performeFenceAction(intersectZone, &intersection); + } + } + + if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { + + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + + int32_t minAltitude = intersectZone->config.minAltitude; + int32_t maxAltitude = intersectZone->config.maxAltitude; + if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { + geozone.zoneInfo = INT32_MAX; + } else if (maxAltitude == INT32_MAX) { + geozone.zoneInfo = minAltitude - getEstimatedActualPosition(Z); + } else if (minAltitude == 0) { + geozone.zoneInfo = maxAltitude - getEstimatedActualPosition(Z); + } else { + int32_t distToMax = maxAltitude - getEstimatedActualPosition(Z); + int32_t distToMin = minAltitude - getEstimatedActualPosition(Z); + if (ABS(distToMin) < ABS(distToMax)) { + geozone.zoneInfo = distToMin; + } else { + geozone.zoneInfo = (distToMax); + } + } + + geozone.messageState = GEOZONE_MESSAGE_STATE_ENTERING_NFZ; + performeFenceAction(intersectZone, &intersection); + } + } +} + +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void) +{ + return &rthWaypoints[rthWaypointIndex]; +} + +void geozoneAdvanceRthAvoidWaypoint(void) +{ + if (rthWaypointIndex < rthWaypointCount) { + rthWaypointIndex++; + } +} + +bool geoZoneIsLastRthWaypoint(void) +{ + return rthWaypointIndex == rthWaypointCount - 1; +} + +void geozoneResetRTH(void) +{ + geozone.avoidInRTHInProgress = false; + rthWaypointIndex = 0; + rthWaypointCount = 0; +} + +void geozoneSetupRTH() { + if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { + noZoneRTH = true; + } else { + noZoneRTH = false; + } +} + +int8_t geozoneCheckForNFZAtCourse(bool isRTH) +{ + UNUSED(isRTH); + + if (geozone.avoidInRTHInProgress || noZoneRTH || !geozoneIsEnabled || !isInitalised) { + return 0; + } + + updateCurrentZones(); + + // Never mind, lets fly out of the zone on current course + if (geozone.insideNfz || (isAtLeastOneInclusiveZoneActive && !geozone.insideFz)) { + return 0; + } + + int8_t waypointCount = calcRthCourse(rthWaypoints, &navGetCurrentActualPositionAndVelocity()->pos, &posControl.rthState.homePosition.pos); + if (waypointCount > 0) { + rthWaypointCount = waypointCount; + rthWaypointIndex = 0; + geozone.avoidInRTHInProgress = true; + return 1; + } + + return waypointCount; +} + +bool isGeozoneActive(void) +{ + return activeGeoZonesCount > 0; +} + +void geozoneUpdateMaxHomeAltitude(void) { + int32_t altitude = INT32_MIN; + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, &posControl.rthState.homePosition.pos, false); + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + altitude = MAX(zones[i]->config.maxAltitude, altitude); + } + } + + if (altitude > INT32_MIN) { + geozone.maxHomeAltitude = altitude - geoZoneConfig()->safeAltitudeDistance; + geozone.homeHasMaxAltitue = true; + } else { + geozone.homeHasMaxAltitue = false; + } +} + +// Avoid arming in NFZ +bool geozoneIsInsideNFZ(void) +{ + // Do not generate arming flags unless we are sure about them + if (!isInitalised || activeGeoZonesCount == 0) { + return false; + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return true; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return false; + } + } + + // We aren't in any zone, is an inclusive one still active? + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + } + + return false; +} + +#endif diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h new file mode 100755 index 00000000000..38849719af0 --- /dev/null +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -0,0 +1,480 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include +#include + +#include "common/vector.h" +#include "navigation/navigation_private.h" + +#define K_EPSILON 1e-8 + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = line1End->x - line1Start->x; + double t1 = -(line2End->x - line2Start->x); + double r1 = line2Start->x - line1Start->x; + + double s2 = line1End->y - line1Start->y; + double t2 = -(line2End->y - line2Start->y); + double r2 = line2Start->y - line1Start->y; + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } + else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6d7b386cbde..fe277a00617 100644 --- a/src/main/navigation/navigation_private.h +++ b/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); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 176812d8f3f..5d38a6dba59 100755 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/target/common.h b/src/main/target/common.h index f7cc3f747ce..3e035710fca 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -221,3 +221,4 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER +#define USE_GEOZONE From 314ab9009dd501d0c3313fd399a200fb965dc15d Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:27:08 -0300 Subject: [PATCH 04/17] Bugfix --- src/main/io/osd.c | 116 +++++++++--------- src/main/navigation/navigation.c | 36 ++++-- src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_geozone.c | 81 ++++++------ .../navigation_geozone_calculations.h | 3 +- 5 files changed, 129 insertions(+), 110 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0307f63c1ab..dedbf641c10 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5973,6 +5973,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 */ @@ -6011,64 +6069,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } -#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_LOITER: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_NONE: - break; - } -#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bbf8e7910f0..fcde4980f44 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -648,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_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, - [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_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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [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_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_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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE, } }, @@ -3606,14 +3607,15 @@ bool isProbablyStillFlying(void) *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { + + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + #ifdef USE_GEOZONE - if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || - (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) { + 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 - - const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { @@ -4253,6 +4255,14 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); + if (geozone.sticksLocked) { + posControl.flags.isAdjustingAltitude = false; + posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; + } + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ad6d60ee06f..2a1dd8484d3 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -131,7 +131,7 @@ typedef enum { GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, - GEOZONE_MESSAGE_STATE_LOITER + GEOZONE_MESSAGE_STATE_POS_HOLD } geozoneMessageState_e; enum fenceAction_e { @@ -197,6 +197,7 @@ typedef struct geozone_s { int32_t zoneInfo; int32_t currentzoneMaxAltitude; int32_t currentzoneMinAltitude; + bool nearestHorZoneHasAction; bool sticksLocked; int8_t loiterDir; bool avoidInRTHInProgress; diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 8a2505a0ff9..80b9cf4def0 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -58,7 +58,7 @@ #define MAX_DISTANCE_FLY_OVER_POINTS 50000 #define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) #define POS_DETECTION_DISTANCE 7500 -#define STICK_LOCK_MIN_TIME 1500 +#define STICK_LOCK_MIN_TIME 2500 #define AVOID_TIMEOUT 30000 #define MAX_LOCAL_VERTICES 128 #define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m @@ -109,6 +109,7 @@ static geoZoneConfig_t safeHomeGeozoneConfig; static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; static timeMs_t actionStartTime = 0; static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestHorZone = NULL; static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; static fpVector3_t avoidingPoint; static bool geozoneIsEnabled = false; @@ -650,6 +651,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp return 0; } + // Set starting point slightly away from our current position float offset = geozoneGetDetectionDistance(); if (geozone.distanceVertToNearestZone <= offset) { int bearing = wrap_36000(geozone.directionToNearestZone + 18000); @@ -660,7 +662,11 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp pathPoints[pathPointCount].visited = true; pathPoints[pathPointCount].distance = 0; pathPoints[pathPointCount++].point = start; - + + // Calculate possible waypoints + // Vertices of the zones are possible waypoints, + // inclusive zones are “reduced”, exclusive zones are “enlarged” to keep distance, + // round zones are converted into hexagons and long sides get additional points to be able to fly over zones. for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { fpVector2_t *verticesZone; fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; @@ -748,7 +754,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp while (!pathPoints[pathPointCount - 1].visited) { pathPoint_t *next = current; float min = FLT_MAX; - for (uint16_t i = 1; i < pathPointCount; i++) { + for (uint8_t i = 1; i < pathPointCount; i++) { float currentDist = FLT_MAX; if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { @@ -813,7 +819,7 @@ static void updateZoneInfos(void) fpVector3_t nearestBorderPoint; aboveOrUnderZone = false; - geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + nearestHorZone = NULL; geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; uint8_t currentZoneCount = getCurrentZones(currentZones, true); int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; @@ -821,11 +827,8 @@ static void updateZoneInfos(void) if (currentZoneCount == 1) { currentMaxAltitude = currentZones[0]->config.maxAltitude; currentMinAltitude = currentZones[0]->config.minAltitude; - - if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { - nearestHorInclZone = currentZones[0]; - } - + nearestHorZone = currentZones[0]; + } else if (currentZoneCount >= 2) { geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; @@ -837,6 +840,7 @@ static void updateZoneInfos(void) if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + nearestHorZone = current; } if (current->config.minAltitude > getEstimatedActualPosition(Z)) { @@ -859,16 +863,16 @@ static void updateZoneInfos(void) if (aboveZone) { if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); - nearestHorInclZone = aboveZone; + nearestHorZone = aboveZone; } else { currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); } } if (belowZone) { - if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); - nearestHorInclZone = belowZone; + nearestHorZone = belowZone; } else { currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); } @@ -959,18 +963,19 @@ static void updateZoneInfos(void) if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { directionToBorder = wrap_36000(directionToBorder + 18000); } - geozone.directionToNearestZone = directionToBorder; geozone.distanceHorToNearestZone = roundf(dist); nearestInclusiveZone = &activeGeoZones[i]; } } - if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { - nearestInclusiveZone = nearestHorInclZone; + if (aboveOrUnderZone && nearestHorZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorZone; geozone.distanceHorToNearestZone = 0; } } + + geozone.nearestHorZoneHasAction = nearestHorZone && nearestHorZone->config.fenceAction != GEOFENCE_ACTION_NONE; } void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) @@ -1248,7 +1253,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) isInitalised = true; } - if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + if (!ARMING_FLAG(ARMED) || !isInitalised || activeGeoZonesCount == 0) { noZoneRTH = false; return; } @@ -1267,6 +1272,10 @@ void geozoneUpdate(timeUs_t curentTimeUs) updateCurrentZones(); updateZoneInfos(); + + if (STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + return; + } // User has switched to non geofence mode, end all actions and switch to mode from box input if (isNonGeozoneModeFromBoxInput()) { @@ -1315,7 +1324,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) return; case GEOZONE_ACTION_STATE_RTH: case GEOZONE_ACTION_STATE_POSHOLD: - geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + geozone.messageState = GEOZONE_MESSAGE_STATE_POS_HOLD; if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { geozone.sticksLocked = false; } @@ -1327,25 +1336,26 @@ void geozoneUpdate(timeUs_t curentTimeUs) break; } - bool currentZoneHasAction = false; - for (uint8_t i = 0; i < currentZoneCount; i++) { - if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { - currentZoneHasAction = true; - break; - } - } - - if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && + actionState == GEOZONE_ACTION_STATE_NONE && + geozone.nearestHorZoneHasAction && + ABS(geozone.distanceVertToNearestZone) > 0 && + ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { float targetAltitide = 0; - if (!geozone.insideFz) { - if (geozone.distanceVertToNearestZone < 0) { - targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + uint32_t extraSafteyAlt = geoZoneConfig()->safeAltitudeDistance * 0.25; + if (nearestHorZone->config.type == GEOZONE_TYPE_INCLUSIVE && geozone.insideFz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMaxAltitude - extraSafteyAlt; } else { - targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + targetAltitide = geozone.currentzoneMinAltitude + extraSafteyAlt; + } + } else if (nearestHorZone->config.type == GEOZONE_TYPE_EXCLUSIVE && !geozone.insideNfz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMinAltitude - extraSafteyAlt; + } else { + targetAltitide = geozone.currentzoneMaxAltitude + extraSafteyAlt; } - } else { - targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; } fpVector3_t targetPos; @@ -1374,6 +1384,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) lockRTZ = false; } + // RTZ: Return to zone: if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { @@ -1457,9 +1468,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) } if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { - - geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); - + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); int32_t minAltitude = intersectZone->config.minAltitude; int32_t maxAltitude = intersectZone->config.maxAltitude; if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { @@ -1474,7 +1483,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (ABS(distToMin) < ABS(distToMax)) { geozone.zoneInfo = distToMin; } else { - geozone.zoneInfo = (distToMax); + geozone.zoneInfo = distToMax; } } diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index 38849719af0..d29d7deb35f 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -348,8 +348,7 @@ static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* dista if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { intersect.x = i1.x; intersect.y = i1.y; - } - else { + } else { intersect.x = i2.x; intersect.y = i2.y; } From 30a0ca9c642c9db089fe842a2b0d32e6ba716bd0 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:34:49 -0300 Subject: [PATCH 05/17] Revert VScode files --- .vscode/settings.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index d1eb2357c02..5de759a0db4 100644 --- a/.vscode/settings.json +++ b/.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 }" -} +} \ No newline at end of file From fc7365fd1280a9089c22f9f64d05bf608f54a803 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:34:49 -0300 Subject: [PATCH 06/17] Revert VScode files --- .vscode/c_cpp_properties.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3a8d8d1c8ca..3c84282e518 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -41,4 +41,4 @@ } ], "version": 4 -} \ No newline at end of file +} From 7d540a89cf7d70af04f9aa4794f6234530f02402 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 08:29:29 -0300 Subject: [PATCH 07/17] Update Settings.md --- docs/Settings.md | 70 ++++++++++++++++++++++++++++++++ src/main/navigation/navigation.c | 6 ++- 2 files changed, 74 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ce4ff9c1ce4..e35e8168cc1 100644 --- a/docs/Settings.md +++ b/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. diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fcde4980f44..900cc8f3ba6 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3611,8 +3611,8 @@ 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 ))) { + 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 @@ -4255,6 +4255,7 @@ 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; @@ -4262,6 +4263,7 @@ static void processNavigationRCAdjustments(void) return; } +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { From 58d2efddc8993b1395b46160f17935255971d368 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:03:44 -0300 Subject: [PATCH 08/17] Fix MacOS SITL and F722 --- cmake/sitl.cmake | 1 + src/main/navigation/navigation.h | 2 -- src/main/navigation/navigation_geozone.c | 4 ++-- src/main/target/common.h | 4 ++++ 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 39e6456830a..66e9844e449 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -62,6 +62,7 @@ if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr -Wno-error=maybe-uninitialized + -Wno-double-promotion -fsingle-precision-constant ) if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 2a1dd8484d3..cc944eb9fc9 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -118,8 +118,6 @@ void resetFwAutolandApproach(int8_t idx); #if defined(USE_GEOZONE) -#define MAX_GEOZONES_IN_CONFIG 63 -#define MAX_VERTICES_IN_CONFIG 126 typedef enum { GEOZONE_MESSAGE_STATE_NONE, diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 80b9cf4def0..64714594c44 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -551,7 +551,7 @@ uint32_t geozoneGetDetectionDistance(void) static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) { - int32_t course; + int32_t course = 0; if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { fpVector2_t intersect; bool found = false; @@ -1517,7 +1517,7 @@ void geozoneResetRTH(void) rthWaypointCount = 0; } -void geozoneSetupRTH() { +void geozoneSetupRTH(void) { if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { noZoneRTH = true; } else { diff --git a/src/main/target/common.h b/src/main/target/common.h index 3e035710fca..bcfb3bbbf60 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,6 +209,8 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 #elif !defined(STM32F7) #define MAX_MIXER_PROFILE_COUNT 1 #endif @@ -217,6 +219,8 @@ #define SKIP_CLI_COMMAND_HELP #undef USE_SERIALRX_SPEKTRUM #undef USE_TELEMETRY_SRXL + #define MAX_GEOZONES_IN_CONFIG 32 + #define MAX_VERTICES_IN_CONFIG 64 #endif #define USE_EZ_TUNE From 27535ebf167b83b2d4bc07e9fed4b4bd1a49af6b Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:06:23 -0300 Subject: [PATCH 09/17] Oops --- cmake/sitl.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 66e9844e449..9bbf9a5c2c9 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -62,7 +62,6 @@ if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr -Wno-error=maybe-uninitialized - -Wno-double-promotion -fsingle-precision-constant ) if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0) @@ -70,6 +69,7 @@ if(NOT MACOSX) endif() else() set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} + -Wno-double-promotion ) endif() From a523ad4758746098e77f6c2e7c02794409413d81 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:10:58 -0300 Subject: [PATCH 10/17] MacOs? --- cmake/sitl.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 9bbf9a5c2c9..1f377103246 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -69,7 +69,7 @@ if(NOT MACOSX) endif() else() set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} - -Wno-double-promotion + -Wno-error=double-promotion ) endif() From 39b10121b858f17791e00574238779987de24f76 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:38:34 -0300 Subject: [PATCH 11/17] Exclude F722 --- src/main/fc/runtime_config.h | 1 - src/main/fc/settings.yaml | 1 - src/main/io/osd.c | 6 ++++++ src/main/navigation/navigation.c | 3 +++ src/main/target/common.h | 5 ++--- 5 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 637ea867e64..452256a6b64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -23,7 +23,6 @@ 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), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 490fab4b914..c64f56a9602 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3829,7 +3829,6 @@ groups: condition: USE_OSD || USE_DJI_HD_OSD members: - name: osd_speed_source - condition: USE_GEOZONE description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" default_value: "GROUND" field: speedSource diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a00f8f97b40..fc3278d8110 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,8 +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; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 900cc8f3ba6..a72cd027966 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4515,13 +4515,16 @@ 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 diff --git a/src/main/target/common.h b/src/main/target/common.h index bcfb3bbbf60..f4963636715 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,6 +209,7 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#define USE_GEOZONE #define MAX_GEOZONES_IN_CONFIG 63 #define MAX_VERTICES_IN_CONFIG 126 #elif !defined(STM32F7) @@ -219,10 +220,8 @@ #define SKIP_CLI_COMMAND_HELP #undef USE_SERIALRX_SPEKTRUM #undef USE_TELEMETRY_SRXL - #define MAX_GEOZONES_IN_CONFIG 32 - #define MAX_VERTICES_IN_CONFIG 64 #endif #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER -#define USE_GEOZONE + From 06abdc5e77a9f9b28916c4d7b9f1684d453f1047 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 11:50:30 -0300 Subject: [PATCH 12/17] Enaable for SITL, Fix feature in cli --- src/main/fc/cli.c | 4 ++-- src/main/target/SITL/target.h | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d5c115c66b1..cb2be8edb38 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -151,8 +151,8 @@ 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", - "GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "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", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index f6d80f74971..f7963dfdd95 100644 --- a/src/main/target/SITL/target.h +++ b/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 From d2a2a03105e37808cb5447ae63b9ad8793159c06 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 10 Nov 2024 17:33:58 -0300 Subject: [PATCH 13/17] Improvements from PR --- src/main/fc/cli.c | 30 ++++++++--- src/main/fc/fc_core.c | 12 ++--- src/main/fc/fc_msp.c | 12 +++-- src/main/fc/fc_tasks.c | 6 +-- src/main/io/osd.c | 2 +- src/main/io/osd.h | 2 +- src/main/navigation/navigation.c | 26 ++++----- src/main/navigation/navigation.h | 7 ++- src/main/navigation/navigation_geozone.c | 54 +++++++++++++++++-- .../navigation_geozone_calculations.h | 12 ++--- src/main/target/common.h | 2 + 11 files changed, 118 insertions(+), 47 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cb2be8edb38..f6506ef110a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1565,7 +1565,7 @@ 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) { @@ -1573,11 +1573,12 @@ static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, cons && 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; } } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ba29a03d5e0..b2a6c1c8026 100644 --- a/src/main/fc/fc_core.c +++ b/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: */ diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c875587ae5e..cad74fa306e 100644 --- a/src/main/fc/fc_msp.c +++ b/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; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index df335ccb428..475a69fdc4a 100755 --- a/src/main/fc/fc_tasks.c +++ b/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, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fc3278d8110..74e26018ba2 100644 --- a/src/main/io/osd.c +++ b/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) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 69d5433e0a5..0952da1fe7f 100644 --- a/src/main/io/osd.h +++ b/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" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a72cd027966..a3de782b2bc 100644 --- a/src/main/navigation/navigation.c +++ b/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 } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cc944eb9fc9..9132974e480 100644 --- a/src/main/navigation/navigation.h +++ b/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); diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 64714594c44..70d9dac0115 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -48,7 +48,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE #include "navigation_geozone_calculations.h" @@ -145,6 +145,7 @@ void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) .type = GEOZONE_SHAPE_CIRCULAR, .minAltitude = 0, .maxAltitude = 0, + .isSealevelRef = false, .fenceAction = GEOFENCE_ACTION_NONE, .vertexCount = 0 ); @@ -174,6 +175,29 @@ uint8_t geozoneGetUsedVerticesCount(void) return count; } +void geozoneReset(const int8_t idx) +{ + if (idx < 0) { + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->shape = GEOZONE_SHAPE_CIRCULAR; + geoZonesConfigMutable(i)->type = GEOZONE_TYPE_EXCLUSIVE; + geoZonesConfigMutable(i)->maxAltitude = 0; + geoZonesConfigMutable(i)->minAltitude = 0; + geoZonesConfigMutable(i)->isSealevelRef = false; + geoZonesConfigMutable(i)->fenceAction = GEOFENCE_ACTION_NONE; + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (idx < MAX_GEOZONES_IN_CONFIG) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + geoZonesConfigMutable(idx)->maxAltitude = 0; + geoZonesConfigMutable(idx)->minAltitude = 0; + geoZonesConfigMutable(idx)->isSealevelRef = false; + geoZonesConfigMutable(idx)->fenceAction = GEOFENCE_ACTION_NONE; + geoZonesConfigMutable(idx)->vertexCount = 0; + } +} + void geozoneResetVertices(const int8_t zoneId, const int16_t idx) { if (zoneId < 0 && idx < 0) { @@ -1129,6 +1153,7 @@ static void endFenceAction(void) static void geoZoneInit(void) { activeGeoZonesCount = 0; + uint8_t expectedVertices = 0, configuredVertices = 0; for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { if (geoZonesConfig(i)->vertexCount > 0) { @@ -1136,6 +1161,17 @@ static void geoZoneInit(void) if (activeGeoZones[i].config.maxAltitude == 0) { activeGeoZones[i].config.maxAltitude = INT32_MAX; } + + if (activeGeoZones[i].config.isSealevelRef) { + + if (activeGeoZones[i].config.maxAltitude != 0) { + activeGeoZones[i].config.maxAltitude -= GPS_home.alt; + } + + if (activeGeoZones[i].config.minAltitude != 0) { + activeGeoZones[i].config.minAltitude -= GPS_home.alt; + } + } activeGeoZones[i].isInfZone = activeGeoZones[i].config.maxAltitude == INT32_MAX && activeGeoZones[i].config.minAltitude == 0; @@ -1146,6 +1182,7 @@ static void geoZoneInit(void) activeGeoZones[activeGeoZonesCount].enable = true; activeGeoZonesCount++; } + expectedVertices += geoZonesConfig(i)->vertexCount; } if (activeGeoZonesCount > 0) { @@ -1155,6 +1192,7 @@ static void geoZoneInit(void) fpVector3_t posLocal3; if (geoZoneVertices(i)->zoneId >= 0 && geoZoneVertices(i)->zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx <= MAX_VERTICES_IN_CONFIG) { + 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; @@ -1180,7 +1218,7 @@ static void geoZoneInit(void) } } } - + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) { safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; @@ -1194,6 +1232,8 @@ static void geoZoneInit(void) activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; activeGeoZonesCount++; + expectedVertices++; + configuredVertices++; } updateCurrentZones(); @@ -1225,7 +1265,7 @@ static void geoZoneInit(void) } activeGeoZonesCount = newActiveZoneCount; - if (activeGeoZonesCount == 0) { + if (activeGeoZonesCount == 0 || expectedVertices != configuredVertices) { setTaskEnabled(TASK_GEOZONE, false); geozoneIsEnabled = false; return; @@ -1525,6 +1565,10 @@ void geozoneSetupRTH(void) { } } +// Return value +// -1: Unable to calculate a course home +// 0: No NFZ in the way +// >0: Number of waypoints int8_t geozoneCheckForNFZAtCourse(bool isRTH) { UNUSED(isRTH); @@ -1575,10 +1619,10 @@ void geozoneUpdateMaxHomeAltitude(void) { } // Avoid arming in NFZ -bool geozoneIsInsideNFZ(void) +bool geozoneIsBlockingArming(void) { // Do not generate arming flags unless we are sure about them - if (!isInitalised || activeGeoZonesCount == 0) { + if (!isInitalised || !geozoneIsEnabled || activeGeoZonesCount == 0) { return false; } diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index d29d7deb35f..789da2b36a4 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -93,13 +93,13 @@ bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line intersection->x = intersection->y = 0; // Double precision is needed here - double s1 = line1End->x - line1Start->x; - double t1 = -(line2End->x - line2Start->x); - double r1 = line2Start->x - line1Start->x; + double s1 = (double)(line1End->x - line1Start->x); + double t1 = (double)(-(line2End->x - line2Start->x)); + double r1 = (double)(line2Start->x - line1Start->x); - double s2 = line1End->y - line1Start->y; - double t2 = -(line2End->y - line2Start->y); - double r2 = line2Start->y - line1Start->y; + double s2 = (double)(line1End->y - line1Start->y); + double t2 = (double)(-(line2End->y - line2Start->y)); + double r2 = (double)(line2Start->y - line1Start->y); // Use Cramer's rule for the solution of the system of linear equations double determ = s1 * t2 - t1 * s2; diff --git a/src/main/target/common.h b/src/main/target/common.h index f4963636715..af0de4bf30f 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,9 +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 From ec604486b66a22c51785edb36d9e5333686bcaa0 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 10 Nov 2024 17:38:57 -0300 Subject: [PATCH 14/17] MacOs (again) --- cmake/sitl.cmake | 1 - src/main/navigation/navigation_geozone.c | 10 +++++----- src/main/navigation/navigation_geozone_calculations.h | 2 +- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 1f377103246..39e6456830a 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -69,7 +69,6 @@ if(NOT MACOSX) endif() else() set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} - -Wno-error=double-promotion ) endif() diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 70d9dac0115..31a60b4084d 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -566,7 +566,7 @@ uint32_t geozoneGetDetectionDistance(void) { uint32_t detctionDistance = 0; if (STATE(AIRPLANE)) { - detctionDistance = navConfig()->fw.loiter_radius * 1.5; + detctionDistance = navConfig()->fw.loiter_radius * 1.5f; } else { detctionDistance = geoZoneConfig()->copterFenceStopDistance; } @@ -1450,9 +1450,9 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { int32_t targetAltitude = 0; if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { - targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5f; } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { - targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5f; } else { targetAltitude = getEstimatedActualPosition(Z); } @@ -1462,9 +1462,9 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (ABS(geozone.distanceVertToNearestZone) < 2000) { calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); if(geozone.distanceVertToNearestZone > 0) { - targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5f; } else { - targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5f; } } else { diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index 789da2b36a4..6d3cd7ad98f 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -24,7 +24,7 @@ #include "common/vector.h" #include "navigation/navigation_private.h" -#define K_EPSILON 1e-8 +#define K_EPSILON 1e-8f static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) { From 535dbc5514217394dd266f0a9a0fd7cc81100435 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 10 Nov 2024 17:57:53 -0300 Subject: [PATCH 15/17] Rebase --- src/main/fc/fc_msp.c | 2 +- src/main/io/osd.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b957a561707..0d379a39a09 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1777,6 +1777,7 @@ 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); @@ -1784,7 +1785,6 @@ static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef); sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); - sbufWriteU8(dst, idx); return MSP_RESULT_ACK; } else { return MSP_RESULT_ERROR; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2f6e9adb847..ef718c9cf01 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -305,13 +305,13 @@ typedef enum { OSD_CUSTOM_ELEMENT_6, OSD_CUSTOM_ELEMENT_7, OSD_CUSTOM_ELEMENT_8, - OSD_COURSE_TO_FENCE, - OSD_H_DIST_TO_FENCE, - OSD_V_DIST_TO_FENCE, OSD_LQ_DOWNLINK, 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; From 0eaf99b3b48a8d0fc6733699150ffc5340d12a6b Mon Sep 17 00:00:00 2001 From: Scavanger Date: Tue, 12 Nov 2024 11:39:27 -0300 Subject: [PATCH 16/17] CLI --- src/main/fc/cli.c | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index f6506ef110a..eaca5eb2608 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1565,7 +1565,7 @@ 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 %d %u"; + 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) { @@ -1574,11 +1574,13 @@ static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, cons && geoZone[i].type == defaultGeoZone->type && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude && geoZone[i].minAltitude == defaultGeoZone->minAltitude - && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef; + && 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); + 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); + 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); } } @@ -1718,7 +1720,6 @@ static void cliGeozone(char* cmdLine) geoZoneVerticesMutable(vertexIdx)->lon = lon; geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId; geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx; - geoZonesConfigMutable(zoneId)->vertexCount++; uint8_t totalVertices = geozoneGetUsedVerticesCount(); cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); @@ -1728,11 +1729,13 @@ static void cliGeozone(char* cmdLine) 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; + 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; @@ -1795,11 +1798,23 @@ static void cliGeozone(char* cmdLine) 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 != 7) { + if (argumentCount != 8) { cliShowParseError(); return; } @@ -1820,6 +1835,7 @@ static void cliGeozone(char* cmdLine) geoZonesConfigMutable(idx)->minAltitude = minAltitude; geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef; geoZonesConfigMutable(idx)->fenceAction = fenceAction; + geoZonesConfigMutable(idx)->vertexCount = vertexCount; } } #endif From 3d6b23518e56190f54290f6d4a5d7e37880fc4f1 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 15 Nov 2024 09:21:13 -0300 Subject: [PATCH 17/17] Move calculations to c file --- src/main/navigation/navigation_geozone.c | 457 ++++++++++++++++- .../navigation_geozone_calculations.h | 479 ------------------ 2 files changed, 455 insertions(+), 481 deletions(-) delete mode 100755 src/main/navigation/navigation_geozone_calculations.h diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 31a60b4084d..dfc7539859b 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -50,8 +50,6 @@ #ifdef USE_GEOZONE -#include "navigation_geozone_calculations.h" - #define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) #define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home @@ -67,6 +65,8 @@ #define GEOZONE_INACTIVE INT8_MAX #define RTH_OVERRIDE_TIMEOUT 1000 +#define K_EPSILON 1e-8f + #define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) typedef enum { @@ -274,6 +274,459 @@ int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) return -1; } + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = (double)(line1End->x - line1Start->x); + double t1 = (double)(-(line2End->x - line2Start->x)); + double r1 = (double)(line2Start->x - line1Start->x); + + double s2 = (double)(line1End->y - line1Start->y); + double t2 = (double)(-(line2End->y - line2Start->y)); + double r2 = (double)(line2Start->y - line1Start->y); + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return a + b == c; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return a + b == c; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + static bool areSticksDeflectdFromChannelValue(void) { return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h deleted file mode 100755 index 6d3cd7ad98f..00000000000 --- a/src/main/navigation/navigation_geozone_calculations.h +++ /dev/null @@ -1,479 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -#pragma once - -#include -#include -#include - -#include "common/vector.h" -#include "navigation/navigation_private.h" - -#define K_EPSILON 1e-8f - -static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) -{ - return calculateDistance2(point, center) < radius; -} - -static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) -{ - bool result = false; - fpVector2_t *p1, *p2; - fpVector2_t* prev = &vertices[verticesNum - 1]; - fpVector2_t *current; - for (uint8_t i = 0; i < verticesNum; i++) - { - current = &vertices[i]; - - if (current->x > prev->x) { - p1 = prev; - p2 = current; - } else { - p1 = current; - p2 = prev; - } - - if ((current->x < point->x) == (point->x <= prev->x) - && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) - { - result = !result; - } - prev = current; - } - return result; -} - -static float angelFromSideLength(const float a, const float b, const float c) -{ - return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); -} - -static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { - return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; -} - -static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) -{ - float ab = calculateDistance2(a, b); - float ac = calculateDistance2(a, c); - float bc = calculateDistance2(b, c); - - return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); -} - -static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) -{ - fpVector2_t dir, a; - float fac; - vector2Sub(&dir, end, start); - fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); - vector2Scale(&a, &dir, fac); - vector2Add(result, start, &a); -} - -// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection -bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) -{ - intersection->x = intersection->y = 0; - - // Double precision is needed here - double s1 = (double)(line1End->x - line1Start->x); - double t1 = (double)(-(line2End->x - line2Start->x)); - double r1 = (double)(line2Start->x - line1Start->x); - - double s2 = (double)(line1End->y - line1Start->y); - double t2 = (double)(-(line2End->y - line2Start->y)); - double r2 = (double)(line2Start->y - line1Start->y); - - // Use Cramer's rule for the solution of the system of linear equations - double determ = s1 * t2 - t1 * s2; - if (determ == 0) { // No solution - return false; - } - - double s0 = (r1 * t2 - t1 * r2) / determ; - double t0 = (s1 * r2 - r1 * s2) / determ; - - if (s0 == 0 && t0 == 0) { - return false; - } - - // No intersection - if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { - return false; - } - - intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); - intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); - - return true; -} - -float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) -{ - return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); -} - -static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) -{ - fpVector3_t result, a, b; - vectorSub(&a, p2, p1); - vectorSub(&b, p3, p1); - vectorCrossProduct(&result, &a, &b); - return result; -} - -static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) -{ - fpVector3_t result; - vectorSub(&result, p1, p2); - return result; -} - -static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) -{ - for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { - polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; - polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; - } -} - -// TRUE if point is in the same direction from pos as ref -static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) -{ - fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); - fpVector3_t dir = calcDirVectorFromPoints(point, pos); - return vectorDotProduct(&refDir, &dir) < 0.0f; -} - -static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) -{ - fpVector2_t ap, ab, prod2, result; - float distance, magAb, prod; - vector2Sub(&ap, point, lineStart); - vector2Sub(&ab, lineEnd, lineStart); - magAb = vector2NormSquared(&ab); - prod = vector2DotProduct(&ap, &ab); - distance = prod / magAb; - if (distance < 0) { - return *lineStart; - } else if (distance > 1) { - return *lineEnd; - } - vector2Scale(&prod2, &ab, distance); - vector2Add(&result, lineStart, &prod2); - return result; -} - -static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) -{ - float a = roundf(calculateDistance2(linepoint, lineStart)); - float b = roundf(calculateDistance2(linepoint, lineEnd)); - float c = roundf(calculateDistance2(lineStart, lineEnd)); - return ABS(a + b - c) <= 1; -} - -static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) -{ - float a = roundf(calculateDistance3(linepoint, lineStart)); - float b = roundf(calculateDistance3(linepoint, lineEnd)); - float c = roundf(calculateDistance3(lineStart, lineEnd)); - return ABS(a + b - c) <= 1; -} - -static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) -{ - if (vectorDotProduct(linePoint, planeNormal) == 0) { - return false; - } - fpVector3_t diff, p1, p4; - float p2 = 0, p3 = 0; - vectorSub(&diff, linePoint, planePoint); - vectorAdd(&p1, &diff, planePoint); - p2 = vectorDotProduct(&diff, planeNormal); - p3 = vectorDotProduct(lineVector, planeNormal); - vectorScale(&p4, lineVector, -p2 / p3); - vectorAdd(result, &p1, &p4); - return true; -} - - -// Möller–Trumbore intersection algorithm -static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) -{ - fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; - float det, invDet, t, u, v; - - vectorSub(&v0v1, v1, v0); - vectorSub(&v0v2, v2, v0); - vectorCrossProduct(&pvec, dir, &v0v2); - det = vectorDotProduct(&v0v1, &pvec); - if (fabsf(det) < K_EPSILON) { - return false; - } - invDet = 1.f / det; - vectorSub(&tvec, org, v0); - u = vectorDotProduct(&tvec, &pvec) * invDet; - if (u < 0 || u > 1) { - return false; - } - vectorCrossProduct(&quvec, &tvec, &v0v1); - v = vectorDotProduct(dir, &quvec) * invDet; - if (v < 0 || u + v > 1) { - return false; - } - t = vectorDotProduct(&v0v2, &quvec) * invDet; - vectorScale(&prod, dir, t); - vectorAdd(intersection, &prod, org); - return true; -} - - -static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) -{ - if (verticesNum < 3) { - return false; - } - - fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; - fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; - fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; - fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); - fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); - - fpVector3_t tmp; - if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { - if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { - memcpy(intersect, &tmp, sizeof(fpVector3_t)); - return true; - } - } - return false; -} - -static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) -{ - // Unfortunately, we need double precision here - double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); - double yIntercept = (double)startPos->y - slope * (double)startPos->x; - - double a = (double)1.0 + sq(slope); - double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); - double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); - - double discr = sq(b) - (double)4.0 * a * c; - if (discr > 0) - { - double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); - double y1 = slope * x1 + yIntercept; - double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); - double y2 = slope * x2 + yIntercept; - - intersection1->x = (float)x1; - intersection1->y = (float)y1; - intersection2->x = (float)x2; - intersection2->y = (float)y2; - return true; - } - return false; -} - -static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) -{ - fpVector2_t* prev = &verticesOld[numVertices - 1]; - fpVector2_t* current; - fpVector2_t* next; - for (uint8_t i = 0; i < numVertices; i++) { - current = &verticesOld[i]; - if (i + 1 < numVertices) { - next = &verticesOld[i + 1]; - } - else { - next = &verticesOld[0]; - } - - fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; - vector2Sub(&v, current, prev); - vector2Normalize(&vn, &v); - vector2Scale(&vs, &vn, offset); - pcp1.x = prev->x - vs.y; - pcp1.y = prev->y + vs.x; - pcp2.x = current->x - vs.y; - pcp2.y = current->y + vs.x; - - vector2Sub(&v, next, current); - vector2Normalize(&vn, &v); - vector2Scale(&vs, &vn, offset); - pcn1.x = current->x - vs.y; - pcn1.y = current->y + vs.x; - pcn2.x = next->x - vs.y; - pcn2.y = next->y + vs.x; - - if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { - verticesNew[i].x = intersect.x; - verticesNew[i].y = intersect.y; - } - prev = current; - } -} - -// Calculates the nearest intersection point -// Inspired by raytracing algortyhms -static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) -{ - float distToIntersection = FLT_MAX; - fpVector3_t intersect; - - fpVector2_t i1, i2; - if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { - return false; - } - - if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { - intersect.x = i1.x; - intersect.y = i1.y; - } else { - intersect.x = i2.x; - intersect.y = i2.y; - } - - float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); - float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); - fpVector2_t p4, p5, p6, p7; - p4.x = 0; - p4.y = endPos->z; - p5.x = dist2; - p5.y = startPos->z; - p6.x = dist1; - p6.y = circleCenter->z; - p7.x = dist1; - p7.y = circleCenter->z + height; - - fpVector2_t heightIntersection; - if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { - intersect.z = heightIntersection.y; - if (isInFront(startPos, &intersect, endPos)) { - distToIntersection = calculateDistance3(startPos, &intersect); - } - } - - fpVector3_t intersectCap; - fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); - if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { - fpVector3_t p1 = *circleCenter; - p1.x = circleCenter->x + radius; - fpVector3_t p2 = *circleCenter; - p2.y = circleCenter->y + radius; - fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); - - if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) - && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) - && isInFront(startPos, &intersectCap, endPos)) { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (startPos->z > circleCenter->z + height || inside) { - fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; - fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; - fpVector3_t p3 = *circleCenter; - p3.z = circleCenter->z + height; - fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); - - if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) - && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) - && isInFront(startPos, &intersectCap, endPos)) { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (distToIntersection < FLT_MAX) { - *distance = distToIntersection; - memcpy(intersection, &intersect, sizeof(fpVector3_t)); - return true; - } - return false; -} - -static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) -{ - float distToIntersection = FLT_MAX; - fpVector3_t intersect; - - fpVector2_t* prev = &vertices[numVertices - 1]; - fpVector2_t* current; - for (uint8_t i = 0; i < numVertices; i++) { - current = &vertices[i]; - - fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; - fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; - fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; - fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; - - fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); - fpVector3_t intersectCurrent; - if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) - || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { - float distWall = calculateDistance3(startPos, &intersectCurrent); - if (distWall < distToIntersection) { - distToIntersection = distWall; - intersect = intersectCurrent; - } - } - prev = current; - } - - fpVector3_t intersectCap; - if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { - if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) - { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (startPos->z > maxHeight || isInclusiveZone) { - if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) - { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (distToIntersection < FLT_MAX) { - *distance = distToIntersection; - memcpy(intersection, &intersect, sizeof(fpVector3_t)); - return true; - } - return false; -}