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 +} 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 diff --git a/docs/Settings.md b/docs/Settings.md index f52da9c2c4b..b6ff1448a7a 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/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..eaca5eb2608 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -151,7 +151,7 @@ static uint8_t commandBatchErrorCount = 0; // sync this with features_e static const char * const featureNames[] = { - "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", + "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE", "", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", @@ -1561,6 +1561,285 @@ static void cliSafeHomes(char *cmdline) } #endif + +#if defined(USE_GEOZONE) +static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone) +{ + const char *format = "geozone %u %u %u %d %d %u %u %u"; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultGeoZone) { + equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].shape == defaultGeoZone->shape + && geoZone[i].type == defaultGeoZone->type + && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude + && geoZone[i].minAltitude == defaultGeoZone->minAltitude + && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef + && geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].vertexCount == defaultGeoZone->vertexCount; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction, defaultGeoZone[i].vertexCount); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction, geoZone[i].vertexCount); + } +} + +static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices) +{ + const char *format = "geozone vertex %d %u %d %d"; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultVertices) { + equalsDefault = vertices[i].idx == defaultVertices->idx + && vertices[i].lat == defaultVertices->lat + && vertices[i].lon == defaultVertices->lon + && vertices[i].zoneId == defaultVertices->zoneId; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon); + } + + if (!defaultVertices) { + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + } +} + +static void cliGeozone(char* cmdLine) +{ + if (isEmpty(cmdLine)) { + printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL); + } else if (sl_strcasecmp(cmdLine, "vertex") == 0) { + printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL); + } else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) { + const char* ptr = &cmdLine[12]; + uint8_t zoneId = 0, idx = 0; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + } else { + geozoneResetVertices(-1, -1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + idx = fastA2I(ptr); + } else { + geozoneResetVertices(zoneId, -1); + return; + } + + if (argumentCount != 2) { + cliShowParseError(); + return; + } + + geozoneResetVertices(zoneId, idx); + + } else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) { + int32_t lat = 0, lon = 0; + int8_t zoneId = 0; + int16_t vertexIdx = -1; + uint8_t vertexZoneIdx = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + if (zoneId < 0) { + return; + } + + if (zoneId >= MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + vertexZoneIdx = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lat = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + } + + if (argumentCount != 4) { + cliShowParseError(); + return; + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx) { + geoZoneVerticesMutable(i)->lat = lat; + geoZoneVerticesMutable(i)->lon = lon; + return; + } + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexIdx = i; + break; + } + } + + if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) { + cliPrintError("Maximum number of vertices reached."); + return; + } + + geoZoneVerticesMutable(vertexIdx)->lat = lat; + geoZoneVerticesMutable(vertexIdx)->lon = lon; + geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx; + + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + + } else if (sl_strncasecmp(cmdLine, "reset", 5) == 0) { + const char* ptr = &cmdLine[5]; + if ((ptr = nextArg(ptr))) { + int idx = fastA2I(ptr); + geozoneReset(idx); + geozoneResetVertices(idx, -1); + } else { + geozoneReset(-1); + geozoneResetVertices(-1, -1); + } + } else { + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0, vertexCount = 0; + int32_t minAltitude = 0, maxAltitude = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + idx = fastA2I(ptr); + if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + isPolygon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + isInclusive = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + minAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + maxAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + seaLevelRef = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + fenceAction = fastA2I(ptr); + if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) { + cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + vertexCount = fastA2I(ptr); + if (vertexCount < 1 || vertexCount > MAX_VERTICES_IN_CONFIG) { + cliShowArgumentRangeError("vertex count", 1, MAX_VERTICES_IN_CONFIG); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + } + + if (argumentCount != 8) { + cliShowParseError(); + return; + } + + if (isPolygon) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON; + } else { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + } + + if (isInclusive) { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE; + } else { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + } + + geoZonesConfigMutable(idx)->maxAltitude = maxAltitude; + geoZonesConfigMutable(idx)->minAltitude = minAltitude; + geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef; + geoZonesConfigMutable(idx)->fenceAction = fenceAction; + geoZonesConfigMutable(idx)->vertexCount = vertexCount; + } +} +#endif + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) { @@ -4178,6 +4457,14 @@ static void printConfig(const char *cmdline, bool doDiff) printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0)); #endif +#if defined(USE_GEOZONE) + cliPrintHashLine("geozone"); + printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0)); + + cliPrintHashLine("geozone vertices"); + printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0)); +#endif + cliPrintHashLine("features"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); @@ -4561,6 +4848,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), +#ifdef USE_GEOZONE + CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone), +#endif #ifdef USE_GPS CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites), 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..b2a6c1c8026 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,6 +278,14 @@ static void updateArmingStatus(void) } #endif +#ifdef USE_GEOZONE + if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } +#endif + /* CHECK: */ if ( sensors(SENSOR_ACC) && diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7a0ed98bdf9..0d379a39a09 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1772,6 +1772,54 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif +#ifdef USE_GEOZONE +static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, idx); + sbufWriteU8(dst, geoZonesConfig(idx)->type); + sbufWriteU8(dst, geoZonesConfig(idx)->shape); + sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); + sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef); + sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); + sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + +static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t zoneId = sbufReadU8(src); + const uint8_t vertexId = sbufReadU8(src); + if (zoneId < MAX_GEOZONES_IN_CONFIG) { + int8_t vertexIdx = geozoneGetVertexIdx(zoneId, vertexId); + if (vertexIdx >= 0) { + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId); + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon); + if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1); + if (vertexRadiusIdx >= 0) { + sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat); + } else { + return MSP_RESULT_ERROR; + } + } + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } +} +#endif + static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t idx = sbufReadU8(src); if (idx < MAX_LOGIC_CONDITIONS) { @@ -3371,6 +3419,50 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; +#ifdef USE_GEOZONE + case MSP2_INAV_SET_GEOZONE: + if (dataSize == 14) { + uint8_t geozoneId; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + + geozoneResetVertices(geozoneId, -1); + geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src); + } else { + return MSP_RESULT_ERROR; + } + break; + case MSP2_INAV_SET_GEOZONE_VERTEX: + if (dataSize == 10 || dataSize == 14) { + uint8_t geozoneId = 0; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + uint8_t vertexId = sbufReadU8(src); + int32_t lat = sbufReadU32(src); + int32_t lon = sbufReadU32(src); + if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) { + return MSP_RESULT_ERROR; + } + + if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) { + return MSP_RESULT_ERROR; + } + } + } else { + return MSP_RESULT_ERROR; + } + break; +#endif + #ifdef USE_EZ_TUNE case MSP2_INAV_EZ_TUNE_SET: @@ -3949,6 +4041,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspFwApproachOutCommand(dst, src); break; #endif +#ifdef USE_GEOZONE + case MSP2_INAV_GEOZONE: + *ret = mspFcGeozoneOutCommand(dst, src); + break; + case MSP2_INAV_GEOZONE_VERTEX: + *ret = mspFcGeozoneVerteciesOutCommand(dst, src); + break; +#endif #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 97cf005a5b6..4ca125d5c6a 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -336,6 +336,15 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } +#ifdef USE_GEOZONE +void geozoneUpdateTask(timeUs_t currentTimeUs) +{ + if (feature(FEATURE_GEOZONE)) { + geozoneUpdate(currentTimeUs); + } +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -450,6 +459,11 @@ void fcTasksInit(void) #if defined(SITL_BUILD) serialProxyStart(); #endif + +#ifdef USE_GEOZONE + setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); +#endif + } cfTask_t cfTasks[TASK_COUNT] = { @@ -737,4 +751,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#ifdef USE_GEOZONE + [TASK_GEOZONE] = { + .taskName = "GEOZONE", + .taskFunc = geozoneUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(5), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 974f90f8c4d..452256a6b64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -23,7 +23,7 @@ typedef enum { WAS_EVER_ARMED = (1 << 3), SIMULATOR_MODE_HITL = (1 << 4), SIMULATOR_MODE_SITL = (1 << 5), - + ARMING_DISABLED_GEOZONE = (1 << 6), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), @@ -49,8 +49,8 @@ typedef enum { ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_LANDING_DETECTED = (1 << 30), - ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | - ARMING_DISABLED_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 +65,8 @@ typedef enum { // situations where we might just need the motors to spin so the // aircraft can move (even unpredictably) and get unstuck (e.g. // crashed into a high tree). -#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_NOT_LEVEL \ +#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_GEOZONE \ + | ARMING_DISABLED_NOT_LEVEL \ | ARMING_DISABLED_NAVIGATION_UNSAFE \ | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \ | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \ @@ -106,6 +107,7 @@ typedef enum { SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), NAV_FW_AUTOLAND = (1 << 18), + NAV_SEND_TO = (1 << 19), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1960e5dac67..507a90eb7ed 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 @@ -4362,3 +4367,49 @@ groups: field: roll_ratio min: 0 max: 5 + - name: PG_GEOZONE_CONFIG + type: geozone_config_t + headers: ["navigation/navigation.h"] + condition: USE_GEOZONE && USE_GPS + members: + - name: geozone_detection_distance + description: "Distance from which a geozone is detected" + default_value: 50000 + field: fenceDetectionDistance + min: 0 + max: 1000000 + - name: geozone_avoid_altitude_range + description: "Altitude range in which an attempt is made to avoid a geozone upwards" + default_value: 5000 + field: avoidAltitudeRange + min: 0 + max: 1000000 + - name: geozone_safe_altitude_distance + description: "Vertical distance that must be maintained to the upper and lower limits of the zone." + default_value: 1000 + field: safeAltitudeDistance + min: 0 + max: 10000 + - name: geozone_safehome_as_inclusive + description: "Treat nearest safehome as inclusive geozone" + type: bool + field: nearestSafeHomeAsInclusivZone + default_value: OFF + - name: geozone_safehome_zone_action + description: "Fence action for safehome zone" + default_value: "NONE" + field: safeHomeFenceAction + table: fence_action + type: uint8_t + - name: geozone_mr_stop_distance + description: "Distance in which multirotors stops before the border" + default_value: 15000 + min: 0 + max: 100000 + field: copterFenceStopDistance + - name: geozone_no_way_home_action + description: "Action if RTH with active geozones is unable to calculate a course to home" + default_value: RTH + field: noWayHomeAction + table: geozone_rth_no_way_home + type: uint8_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4ab6f7df7f..953588e6f4d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,6 +866,14 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + + case ARMING_DISABLED_GEOZONE: +#ifdef USE_GEOZONE + return OSD_MESSAGE_STR(OSD_MSG_NFZ); +#else + FALLTHROUGH; +#endif + // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; @@ -2381,6 +2389,11 @@ static bool osdDrawSingleElement(uint8_t item) if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else +#endif +#ifdef USE_GEOZONE + if (FLIGHT_MODE(NAV_SEND_TO)) + p = "AUTO"; + else #endif if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; @@ -3889,6 +3902,52 @@ static bool osdDrawSingleElement(uint8_t item) clearMultiFunction = true; break; } +#if defined(USE_GEOZONE) + case OSD_COURSE_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset; + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction); + } else { + if (isGeozoneActive()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr); + } + break; + } + + case OSD_H_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0, 3); + tfp_sprintf(buff, "FD %s", buff2 ); + } else { + strcpy(buff, "FD ---"); + } + } + break; + + case OSD_V_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone)); + tfp_sprintf(buff, "FD%s", buff2); + displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr); + } else { + strcpy(buff, "FD ---"); + } + + break; + } +#endif default: return false; @@ -5946,6 +6005,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = safehomeMessage; } #endif + +#ifdef USE_GEOZONE + if (geozone.avoidInRTHInProgress) { + messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH; + } +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); @@ -5970,6 +6035,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } else { +#ifdef USE_GEOZONE + char buf[12], buf1[12]; + switch (geozone.messageState) { + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_POS_HOLD: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ @@ -6008,6 +6131,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } + } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c57bd62f640..ef718c9cf01 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, @@ -296,6 +309,9 @@ typedef enum { OSD_RX_POWER_DOWNLINK, // 160 OSD_RX_BAND, OSD_RX_MODE, + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -478,6 +494,10 @@ typedef struct osdConfig_s { uint16_t adsb_ignore_plane_above_me_limit; // in metres #endif uint8_t radar_peers_display_time; // in seconds +#ifdef USE_GEOZONE + uint8_t geozoneDistanceWarning; // Distance to fence or action + bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action +#endif } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); 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..a3de782b2bc 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, } }, @@ -637,15 +648,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_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, } }, @@ -1178,6 +1190,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, #endif + +#ifdef USE_GEOZONE + [NAV_STATE_SEND_TO_INITALIZE] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_SEND_TO_IN_PROGESS] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, + [NAV_STATE_SEND_TO_FINISHED] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_FINISHED, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, +#endif }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1444,6 +1513,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // If we have valid position sensor or configured to ignore it's loss at initial stage - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) { // Prepare controllers +#ifdef USE_GEOZONE + geozoneResetRTH(); + geozoneSetupRTH(); +#endif resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL setupAltitudeController(); @@ -1610,22 +1683,55 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); +#ifdef USE_GEOZONE + // Check for NFZ in our way + 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)) { - // 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 } /* Position sensor failure timeout - land */ else if (checkForPositionSensorTimeout()) { @@ -1797,6 +1903,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } +#ifdef USE_GEOZONE + geozoneResetRTH(); +#endif + // Prevent I-terms growing when already landed pidResetErrorAccumulators(); return NAV_FSM_EVENT_NONE; @@ -2452,6 +2562,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga } #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + resetPositionController(); + resetAltitudeController(false); + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } else { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + posControl.sendTo.startTime = millis(); + return NAV_FSM_EVENT_SUCCESS; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + // "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately + if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) { + posControl.sendTo.lockSticks = false; + } + + if (!posControl.sendTo.lockSticks && areSticksDeflected()) { + abortSendTo(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) { + if (posControl.sendTo.altitudeTargetRange > 0) { + if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z + posControl.sendTo.altitudeTargetRange)) { + return NAV_FSM_EVENT_SUCCESS; + } else { + return NAV_FSM_EVENT_NONE; + } + } + return NAV_FSM_EVENT_SUCCESS; + } + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState) +{ + UNUSED(previousState); + posControl.sendTo.lockSticks = false; + posControl.flags.sendToActive = false; + return NAV_FSM_EVENT_NONE; +} +#endif + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -2539,6 +2707,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) switch (mode) { case RTH_HOME_ENROUTE_INITIAL: posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude; +#ifdef USE_GEOZONE + if (geozone.currentzoneMaxAltitude > 0) { + posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z); + } +#endif break; case RTH_HOME_ENROUTE_PROPORTIONAL: @@ -2552,6 +2725,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; } } +#ifdef USE_GEOZONE + if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance; + } +#endif break; case RTH_HOME_ENROUTE_FINAL: @@ -2757,6 +2935,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void) return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs; } +/*----------------------------------------------------------- + * Calculates 2D distance between two points + *-----------------------------------------------------------*/ +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos) +{ + const float deltaX = destinationPos->x - startPos->x; + const float deltaY = destinationPos->y - startPos->y; + + return calc_length_pythagorean_2D(deltaX, deltaY); +} + /*----------------------------------------------------------- * Calculates distance and bearing to destination point *-----------------------------------------------------------*/ @@ -2944,6 +3133,11 @@ static void updateDesiredRTHAltitude(void) posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; } +#if defined(USE_GEOZONE) + if (geozone.homeHasMaxAltitue) { + posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude); + } +#endif } /*----------------------------------------------------------- @@ -3154,6 +3348,9 @@ void updateHomePosition(void) setHome = true; break; } +#ifdef USE_GEOZONE + geozoneUpdateMaxHomeAltitude(); +#endif } } else { @@ -3409,8 +3606,16 @@ bool isProbablyStillFlying(void) * Z-position controller *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) -{ +{ + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + +#ifdef USE_GEOZONE + if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) { + return 0.0f; + } +#endif float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { @@ -4050,6 +4255,16 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); +#ifdef USE_GEOZONE + if (geozone.sticksLocked) { + posControl.flags.isAdjustingAltitude = false; + posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; + } +#endif + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); @@ -4095,6 +4310,10 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; +#endif return; } @@ -4145,7 +4364,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4296,6 +4515,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } +#ifdef USE_GEOZONE + if (posControl.flags.sendToActive) { + return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; + } + + + if (posControl.flags.forcedPosholdActive) { + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + } +#endif + /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission * auto restarting after interruption by Manual or RTH modes. @@ -4769,6 +4999,14 @@ void navigationInit(void) #ifdef USE_MULTI_MISSION posControl.multiMissionCount = 0; #endif + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; + posControl.sendTo.lockSticks = false; + posControl.sendTo.lockStickTime = 0; + posControl.sendTo.startTime = 0; + posControl.sendTo.targetRange = 0; +#endif /* Set initial surface invalid */ posControl.actualState.surfaceMin = -1.0f; @@ -4851,6 +5089,40 @@ rthState_e getStateOfForcedRTH(void) } } + +#ifdef USE_GEOZONE +// "Send to" is not to intended to be activated by user, only by external event +void activateSendTo(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.sendToActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortSendTo(void) +{ + posControl.flags.sendToActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void activateForcedPosHold(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.forcedPosholdActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortForcedPosHold(void) +{ + posControl.flags.forcedPosholdActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} +#endif + /*----------------------------------------------------------- * Ability to execute Emergency Landing on external event *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 24b8daa0402..9132974e480 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -116,6 +116,123 @@ void resetFwAutolandApproach(int8_t idx); #endif +#if defined(USE_GEOZONE) + +#ifndef USE_GPS + #error "Geozone needs GPS support" +#endif + +typedef enum { + GEOZONE_MESSAGE_STATE_NONE, + GEOZONE_MESSAGE_STATE_NFZ, + GEOZONE_MESSAGE_STATE_LEAVING_FZ, + GEOZONE_MESSAGE_STATE_OUTSIDE_FZ, + GEOZONE_MESSAGE_STATE_ENTERING_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_FB, + GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, + GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, + GEOZONE_MESSAGE_STATE_POS_HOLD +} geozoneMessageState_e; + +enum fenceAction_e { + GEOFENCE_ACTION_NONE, + GEOFENCE_ACTION_AVOID, + GEOFENCE_ACTION_POS_HOLD, + GEOFENCE_ACTION_RTH, +}; + +enum noWayHomeAction { + NO_WAY_HOME_ACTION_RTH, + NO_WAY_HOME_ACTION_EMRG_LAND, +}; + +#define GEOZONE_SHAPE_CIRCULAR 0 +#define GEOZONE_SHAPE_POLYGON 1 + +#define GEOZONE_TYPE_EXCLUSIVE 0 +#define GEOZONE_TYPE_INCLUSIVE 1 + +typedef struct geoZoneConfig_s +{ + uint8_t shape; + uint8_t type; + int32_t minAltitude; + int32_t maxAltitude; + bool isSealevelRef; + uint8_t fenceAction; + uint8_t vertexCount; +} geoZoneConfig_t; + +typedef struct geozone_config_s +{ + uint32_t fenceDetectionDistance; + uint16_t avoidAltitudeRange; + uint16_t safeAltitudeDistance; + bool nearestSafeHomeAsInclusivZone; + uint8_t safeHomeFenceAction; + uint32_t copterFenceStopDistance; + uint8_t noWayHomeAction; +} geozone_config_t; + +typedef struct vertexConfig_s +{ + int8_t zoneId; + uint8_t idx; + int32_t lat; + int32_t lon; +} vertexConfig_t; + +PG_DECLARE(geozone_config_t, geoZoneConfig); +PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig); +PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices); + +typedef struct geozone_s { + bool insideFz; + bool insideNfz; + uint32_t distanceToZoneBorder3d; + int32_t vertDistanceToZoneBorder; + geozoneMessageState_e messageState; + int32_t directionToNearestZone; + int32_t distanceHorToNearestZone; + int32_t distanceVertToNearestZone; + int32_t zoneInfo; + int32_t currentzoneMaxAltitude; + int32_t currentzoneMinAltitude; + bool nearestHorZoneHasAction; + bool sticksLocked; + int8_t loiterDir; + bool avoidInRTHInProgress; + int32_t maxHomeAltitude; + bool homeHasMaxAltitue; +} geozone_t; + +extern geozone_t geozone; + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon); +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId); +bool isGeozoneActive(void); +uint8_t geozoneGetUsedVerticesCount(void); +void geozoneReset(int8_t idx); +void geozoneResetVertices(int8_t zoneId, int16_t idx); +void geozoneUpdate(timeUs_t curentTimeUs); +bool geozoneIsBlockingArming(void); +void geozoneAdvanceRthAvoidWaypoint(void); +int8_t geozoneCheckForNFZAtCourse(bool isRTH); +bool geoZoneIsLastRthWaypoint(void); +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void); +void geozoneSetupRTH(void); +void geozoneResetRTH(void); +void geozoneUpdateMaxHomeAltitude(void); +uint32_t geozoneGetDetectionDistance(void); + +void activateSendTo(void); +void abortSendTo(void); +void activateForcedPosHold(void); +void abortForcedPosHold(void); + +#endif + #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 #endif 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..dfc7539859b --- /dev/null +++ b/src/main/navigation/navigation_geozone.c @@ -0,0 +1,2104 @@ +/* + * 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" + +#ifdef USE_GEOZONE + +#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 2500 +#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 K_EPSILON 1e-8f + +#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 *nearestHorZone = NULL; +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, + .isSealevelRef = false, + .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 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) { + 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 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; +} + +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.5f; + } else { + detctionDistance = geoZoneConfig()->copterFenceStopDistance; + } + return detctionDistance; +} + +static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) +{ + int32_t course = 0; + 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; + } + + // Set starting point slightly away from our current position + 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; + + // 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]; + 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 (uint8_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; + + nearestHorZone = 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; + nearestHorZone = 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); + nearestHorZone = current; + } + + 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); + nearestHorZone = aboveZone; + } else { + currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); + } + } + + if (belowZone) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); + nearestHorZone = 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 && 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) +{ + // 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; + uint8_t expectedVertices = 0, configuredVertices = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) + { + if (geoZonesConfig(i)->vertexCount > 0) { + memcpy(&activeGeoZones[activeGeoZonesCount].config, geoZonesConfig(i), sizeof(geoZoneRuntimeConfig_t)); + if (activeGeoZones[i].config.maxAltitude == 0) { + activeGeoZones[i].config.maxAltitude = INT32_MAX; + } + + 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; + + if (!STATE(AIRPLANE) && activeGeoZones[i].config.fenceAction == GEOFENCE_ACTION_AVOID) { + activeGeoZones[i].config.fenceAction = GEOFENCE_ACTION_POS_HOLD; + } + + activeGeoZones[activeGeoZonesCount].enable = true; + activeGeoZonesCount++; + } + expectedVertices += geoZonesConfig(i)->vertexCount; + } + + 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) { + 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; + 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++; + expectedVertices++; + configuredVertices++; + } + + 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 || expectedVertices != configuredVertices) { + 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) || !isInitalised || activeGeoZonesCount == 0) { + 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(); + + 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()) { + 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_POS_HOLD; + if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { + geozone.sticksLocked = false; + } + if (!geozone.sticksLocked && areSticksDeflectdFromChannelValue()) { + endFenceAction(); + } + return; + default: + break; + } + + 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; + 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 = 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; + } + } + + 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; + } + + // RTZ: Return to zone: + 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.5f; + } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5f; + } 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.5f; + } else { + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5f; + } + + } 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(void) { + if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { + noZoneRTH = true; + } else { + noZoneRTH = false; + } +} + +// 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); + + 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 geozoneIsBlockingArming(void) +{ + // Do not generate arming flags unless we are sure about them + if (!isInitalised || !geozoneIsEnabled || 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_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/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 diff --git a/src/main/target/common.h b/src/main/target/common.h index f7cc3f747ce..af0de4bf30f 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,6 +209,11 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#ifdef USE_GPS +#define USE_GEOZONE +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 +#endif #elif !defined(STM32F7) #define MAX_MIXER_PROFILE_COUNT 1 #endif @@ -221,3 +226,4 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER +