Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 11:29:56 +03:00
Code Issues Wiki Activity

Merge pull request #10459 from Scavanger/Geozones

Browse source
This commit is contained in:
Marcelo Bezerra 2024-11-15 15:24:48 +01:00 • committed by GitHub
commit abab92ab3b
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
25 changed files with 3361 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.vscode/c_cpp_properties.json vendored
View file

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

4
.vscode/settings.json vendored
View file

@ -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 }"
}
}

70
docs/Settings.md
View file

@ -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.

1
src/main/CMakeLists.txt
View file

@ -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

83
src/main/common/vector.h
View file

@ -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;
}

5
src/main/config/parameter_group_ids.h
View file

@ -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

292
src/main/fc/cli.c
View file

@ -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),

2
src/main/fc/config.h
View file

@ -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,

8
src/main/fc/fc_core.c
View file

@ -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) &&

100
src/main/fc/fc_msp.c
View file

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

23
src/main/fc/fc_tasks.c
View file

@ -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
};

10
src/main/fc/runtime_config.h
View file

@ -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;

51
src/main/fc/settings.yaml
View file

@ -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

124
src/main/io/osd.c
View file

@ -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) {

20
src/main/io/osd.h
View file

@ -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);

2
src/main/io/osd_dji_hd.c
View file

@ -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:

7
src/main/msp/msp_protocol_v2_inav.h
View file

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

322
src/main/navigation/navigation.c
View file

@ -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);
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
// Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
posControl.landingDelay = 0;
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
#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);
posControl.landingDelay = 0;
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_NONE;
}
#ifdef USE_GEOZONE
}
#endif
}
/* Position sensor failure timeout - land */
else if (checkForPositionSensorTimeout()) {
@ -1797,6 +1903,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME
}
#ifdef USE_GEOZONE
geozoneResetRTH();
#endif
// Prevent I-terms growing when already landed
pidResetErrorAccumulators();
return NAV_FSM_EVENT_NONE;
@ -2452,6 +2562,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
}
#endif
#ifdef USE_GEOZONE
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
resetPositionController();
resetAltitudeController(false);
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} else {
setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
posControl.sendTo.startTime = millis();
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState)
{
UNUSED(previousState);
// "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately
if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) {
posControl.sendTo.lockSticks = false;
}
if (!posControl.sendTo.lockSticks && areSticksDeflected()) {
abortSendTo();
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) {
if (posControl.sendTo.altitudeTargetRange > 0) {
if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z + posControl.sendTo.altitudeTargetRange)) {
return NAV_FSM_EVENT_SUCCESS;
} else {
return NAV_FSM_EVENT_NONE;
}
}
return NAV_FSM_EVENT_SUCCESS;
}
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState)
{
UNUSED(previousState);
posControl.sendTo.lockSticks = false;
posControl.flags.sendToActive = false;
return NAV_FSM_EVENT_NONE;
}
#endif
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
{
navigationFSMState_t previousState;
@ -2539,6 +2707,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
switch (mode) {
case RTH_HOME_ENROUTE_INITIAL:
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
#ifdef USE_GEOZONE
if (geozone.currentzoneMaxAltitude > 0) {
posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z);
}
#endif
break;
case RTH_HOME_ENROUTE_PROPORTIONAL:
@ -2552,6 +2725,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
}
}
#ifdef USE_GEOZONE
if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) {
posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance;
}
#endif
break;
case RTH_HOME_ENROUTE_FINAL:
@ -2757,6 +2935,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
}
/*-----------------------------------------------------------
* Calculates 2D distance between two points
*-----------------------------------------------------------*/
float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos)
{
const float deltaX = destinationPos->x - startPos->x;
const float deltaY = destinationPos->y - startPos->y;
return calc_length_pythagorean_2D(deltaX, deltaY);
}
/*-----------------------------------------------------------
* Calculates distance and bearing to destination point
*-----------------------------------------------------------*/
@ -2944,6 +3133,11 @@ static void updateDesiredRTHAltitude(void)
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
}
#if defined(USE_GEOZONE)
if (geozone.homeHasMaxAltitue) {
posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude);
}
#endif
}
/*-----------------------------------------------------------
@ -3154,6 +3348,9 @@ void updateHomePosition(void)
setHome = true;
break;
}
#ifdef USE_GEOZONE
geozoneUpdateMaxHomeAltitude();
#endif
}
}
else {
@ -3409,8 +3606,16 @@ bool isProbablyStillFlying(void)
* Z-position controller
*-----------------------------------------------------------*/
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
{
{
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
#ifdef USE_GEOZONE
if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
(geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) {
return 0.0f;
}
#endif
float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
@ -4050,6 +4255,16 @@ static void processNavigationRCAdjustments(void)
/* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
#ifdef USE_GEOZONE
if (geozone.sticksLocked) {
posControl.flags.isAdjustingAltitude = false;
posControl.flags.isAdjustingPosition = false;
posControl.flags.isAdjustingHeading = false;
return;
}
#endif
if (FLIGHT_MODE(FAILSAFE_MODE)) {
if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
resetMulticopterBrakingMode();
@ -4095,6 +4310,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
resetRthTrackBack();
#ifdef USE_GEOZONE
posControl.flags.sendToActive = false;
#endif
return;
}
@ -4145,7 +4364,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
void switchNavigationFlightModes(void)
{
const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes);
DISABLE_FLIGHT_MODE(disabledFlightModes);
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
}
@ -4296,6 +4515,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
#ifdef USE_GEOZONE
if (posControl.flags.sendToActive) {
return NAV_FSM_EVENT_SWITCH_TO_SEND_TO;
}
if (posControl.flags.forcedPosholdActive) {
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
}
#endif
/* WP mission activation control:
* canActivateWaypoint & waypointWasActivated are used to prevent WP mission
* auto restarting after interruption by Manual or RTH modes.
@ -4769,6 +4999,14 @@ void navigationInit(void)
#ifdef USE_MULTI_MISSION
posControl.multiMissionCount = 0;
#endif
#ifdef USE_GEOZONE
posControl.flags.sendToActive = false;
posControl.sendTo.lockSticks = false;
posControl.sendTo.lockStickTime = 0;
posControl.sendTo.startTime = 0;
posControl.sendTo.targetRange = 0;
#endif
/* Set initial surface invalid */
posControl.actualState.surfaceMin = -1.0f;
@ -4851,6 +5089,40 @@ rthState_e getStateOfForcedRTH(void)
}
}
#ifdef USE_GEOZONE
// "Send to" is not to intended to be activated by user, only by external event
void activateSendTo(void)
{
if (!geozone.avoidInRTHInProgress) {
abortFixedWingLaunch();
posControl.flags.sendToActive = true;
navProcessFSMEvents(selectNavEventFromBoxModeInput());
}
}
void abortSendTo(void)
{
posControl.flags.sendToActive = false;
navProcessFSMEvents(selectNavEventFromBoxModeInput());
}
void activateForcedPosHold(void)
{
if (!geozone.avoidInRTHInProgress) {
abortFixedWingLaunch();
posControl.flags.forcedPosholdActive = true;
navProcessFSMEvents(selectNavEventFromBoxModeInput());
}
}
void abortForcedPosHold(void)
{
posControl.flags.forcedPosholdActive = false;
navProcessFSMEvents(selectNavEventFromBoxModeInput());
}
#endif
/*-----------------------------------------------------------
* Ability to execute Emergency Landing on external event
*-----------------------------------------------------------*/

117
src/main/navigation/navigation.h
View file

@ -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

6
src/main/navigation/navigation_fixedwing.c
View file

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

2104
src/main/navigation/navigation_geozone.c Executable file
View file

File diff suppressed because it is too large Load diff

31
src/main/navigation/navigation_private.h
View file

@ -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);

4
src/main/scheduler/scheduler.h
View file

@ -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,

3
src/main/target/SITL/target.h
View file

@ -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

6
src/main/target/common.h
View file

@ -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