mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-13 11:29:56 +03:00
Inital commit
This commit is contained in:
Scavanger
parent
f3da487264
commit
ec46d9ee91
25 changed files with 3243 additions and 23 deletions
16
.vscode/c_cpp_properties.json
vendored
16
.vscode/c_cpp_properties.json
vendored
|
|
@ -4,7 +4,8 @@
|
|||
"name": "Linux",
|
||||
"includePath": [
|
||||
"${workspaceRoot}",
|
||||
"${workspaceRoot}/src/main/**"
|
||||
"${workspaceRoot}/src/main/**",
|
||||
"${workspaceRoot}/debug_SITL/src/main/target/SITL/SITL/*"
|
||||
],
|
||||
"browse": {
|
||||
"limitSymbolsToIncludedHeaders": false,
|
||||
|
|
@ -12,10 +13,14 @@
|
|||
"${workspaceRoot}/**"
|
||||
]
|
||||
},
|
||||
"intelliSenseMode": "msvc-x64",
|
||||
"intelliSenseMode": "gcc-arm",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
"uint16_t=unsigned short",
|
||||
"int16_t=short",
|
||||
"uint32_t=unsigned int",
|
||||
"MCU_FLASH_SIZE=1024",
|
||||
"USE_OSD",
|
||||
"USE_GYRO_NOTCH_1",
|
||||
"USE_GYRO_NOTCH_2",
|
||||
|
|
@ -35,9 +40,10 @@
|
|||
"USE_PCF8574",
|
||||
"USE_ESC_SENSOR",
|
||||
"USE_ADAPTIVE_FILTER",
|
||||
"MCU_FLASH_SIZE 1024",
|
||||
],
|
||||
"configurationProvider": "ms-vscode.cmake-tools"
|
||||
"SITL_BUILD",
|
||||
"USE_GEOZONE"
|
||||
]
|
||||
//"configurationProvider": "ms-vscode.cmake-tools"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
|
|
|
|||
8
.vscode/settings.json
vendored
8
.vscode/settings.json
vendored
|
|
@ -4,10 +4,14 @@
|
|||
"cmath": "c",
|
||||
"ranges": "c",
|
||||
"navigation.h": "c",
|
||||
"rth_trackback.h": "c"
|
||||
"rth_trackback.h": "c",
|
||||
"platform.h": "c",
|
||||
"timer.h": "c",
|
||||
"bus.h": "c"
|
||||
"bus.h": "c",
|
||||
"controlrate_profile.h": "c",
|
||||
"settings.h": "c",
|
||||
"settings_generated.h": "c",
|
||||
"parameter_group.h": "c"
|
||||
},
|
||||
"editor.tabSize": 4,
|
||||
"editor.insertSpaces": true,
|
||||
|
|
|
|||
61
.vscode/tasks.json
vendored
61
.vscode/tasks.json
vendored
|
|
@ -4,9 +4,9 @@
|
|||
"version": "2.0.0",
|
||||
"tasks": [
|
||||
{
|
||||
"label": "Build AOCODARCH7DUAL",
|
||||
"label": "Build Matek F722-SE",
|
||||
"type": "shell",
|
||||
"command": "make AOCODARCH7DUAL",
|
||||
"command": "make MATEKF722SE",
|
||||
"group": "build",
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
|
|
@ -14,12 +14,11 @@
|
|||
}
|
||||
},
|
||||
{
|
||||
"label": "Build AOCODARCH7DUAL",
|
||||
"label": "Build Matek F722",
|
||||
"type": "shell",
|
||||
"command": "make AOCODARCH7DUAL",
|
||||
"command": "make MATEKF722",
|
||||
"group": {
|
||||
"kind": "build",
|
||||
"isDefault": true
|
||||
"kind": "build"
|
||||
},
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
|
|
@ -36,6 +35,56 @@
|
|||
"options": {
|
||||
"cwd": "${workspaceFolder}/build"
|
||||
}
|
||||
},
|
||||
{
|
||||
"label": "CMAKE Build SITL",
|
||||
"type": "shell",
|
||||
"command": "mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON ..",
|
||||
"group": "build",
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
"cwd": "${workspaceFolder}"
|
||||
}
|
||||
},
|
||||
{
|
||||
"label": "CMAKE Debug SITL",
|
||||
"type": "shell",
|
||||
"command": "mkdir -p debug_SITL && cd debug_SITL && cmake -DCMAKE_BUILD_TYPE=Debug -DSITL=ON ..",
|
||||
"group": "build",
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
"cwd": "${workspaceFolder}"
|
||||
}
|
||||
},
|
||||
{
|
||||
"label": "CMAKE Release SITL",
|
||||
"type": "shell",
|
||||
"command": "mkdir -p release_SITL && cd release_SITL && cmake -DCMAKE_BUILD_TYPE=Release -DSITL=ON ..",
|
||||
"group": "build",
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
"cwd": "${workspaceFolder}"
|
||||
}
|
||||
},
|
||||
{
|
||||
"label": "Debug SITL",
|
||||
"type": "shell",
|
||||
"command": "make",
|
||||
"group": "build",
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
"cwd": "${workspaceFolder}/debug_SITL"
|
||||
}
|
||||
},
|
||||
{
|
||||
"label": "Release SITL",
|
||||
"type": "shell",
|
||||
"command": "make",
|
||||
"group": "build",
|
||||
"problemMatcher": [],
|
||||
"options": {
|
||||
"cwd": "${workspaceFolder}/release_SITL"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -152,7 +152,7 @@ static uint8_t commandBatchErrorCount = 0;
|
|||
// sync this with features_e
|
||||
static const char * const featureNames[] = {
|
||||
"THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
|
||||
"", "SOFTSERIAL", "GPS", "RPM_FILTERS",
|
||||
"GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS",
|
||||
"", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
|
||||
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
|
||||
"BLACKBOX", "", "TRANSPONDER", "AIRMODE",
|
||||
|
|
@ -1561,6 +1561,251 @@ static void cliSafeHomes(char *cmdline)
|
|||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE)
|
||||
static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone)
|
||||
{
|
||||
const char *format = "geozone %u %u %u %d %d %u";
|
||||
for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) {
|
||||
bool equalsDefault = false;
|
||||
if (defaultGeoZone) {
|
||||
equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction
|
||||
&& geoZone[i].shape == defaultGeoZone->shape
|
||||
&& geoZone[i].type == defaultGeoZone->type
|
||||
&& geoZone[i].maxAltitude == defaultGeoZone->maxAltitude
|
||||
&& geoZone[i].minAltitude == defaultGeoZone->minAltitude;
|
||||
|
||||
cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction);
|
||||
}
|
||||
cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction);
|
||||
}
|
||||
}
|
||||
|
||||
static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices)
|
||||
{
|
||||
const char *format = "geozone vertex %d %u %d %d";
|
||||
for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
|
||||
bool equalsDefault = false;
|
||||
if (defaultVertices) {
|
||||
equalsDefault = vertices[i].idx == defaultVertices->idx
|
||||
&& vertices[i].lat == defaultVertices->lat
|
||||
&& vertices[i].lon == defaultVertices->lon
|
||||
&& vertices[i].zoneId == defaultVertices->zoneId;
|
||||
|
||||
cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon);
|
||||
}
|
||||
|
||||
cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon);
|
||||
}
|
||||
|
||||
if (!defaultVertices) {
|
||||
uint8_t totalVertices = geozoneGetUsedVerticesCount();
|
||||
cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG);
|
||||
}
|
||||
}
|
||||
|
||||
static void cliGeozone(char* cmdLine)
|
||||
{
|
||||
if (isEmpty(cmdLine)) {
|
||||
printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL);
|
||||
} else if (sl_strcasecmp(cmdLine, "vertex") == 0) {
|
||||
printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL);
|
||||
} else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) {
|
||||
const char* ptr = &cmdLine[12];
|
||||
uint8_t zoneId = 0, idx = 0;
|
||||
uint8_t argumentCount = 1;
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
zoneId = fastA2I(ptr);
|
||||
} else {
|
||||
geozoneResetVertices(-1, -1);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
argumentCount++;
|
||||
idx = fastA2I(ptr);
|
||||
} else {
|
||||
geozoneResetVertices(zoneId, -1);
|
||||
return;
|
||||
}
|
||||
|
||||
if (argumentCount != 2) {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
geozoneResetVertices(zoneId, idx);
|
||||
|
||||
} else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) {
|
||||
int32_t lat = 0, lon = 0;
|
||||
int8_t zoneId = 0;
|
||||
int16_t vertexIdx = -1;
|
||||
uint8_t vertexZoneIdx = 0;
|
||||
const char* ptr = cmdLine;
|
||||
uint8_t argumentCount = 1;
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
zoneId = fastA2I(ptr);
|
||||
if (zoneId < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (zoneId >= MAX_GEOZONES_IN_CONFIG) {
|
||||
cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1);
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
argumentCount++;
|
||||
vertexZoneIdx = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
argumentCount++;
|
||||
lat = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
argumentCount++;
|
||||
lon = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
argumentCount++;
|
||||
}
|
||||
|
||||
if (argumentCount != 4) {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
|
||||
if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx) {
|
||||
geoZoneVerticesMutable(i)->lat = lat;
|
||||
geoZoneVerticesMutable(i)->lon = lon;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
|
||||
if (geoZoneVertices(i)->zoneId == -1) {
|
||||
vertexIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) {
|
||||
cliPrintError("Maximum number of vertices reached.");
|
||||
return;
|
||||
}
|
||||
|
||||
geoZoneVerticesMutable(vertexIdx)->lat = lat;
|
||||
geoZoneVerticesMutable(vertexIdx)->lon = lon;
|
||||
geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId;
|
||||
geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx;
|
||||
geoZonesConfigMutable(zoneId)->vertexCount++;
|
||||
|
||||
uint8_t totalVertices = geozoneGetUsedVerticesCount();
|
||||
cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG);
|
||||
|
||||
} else {
|
||||
int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0;
|
||||
int32_t minAltitude = 0, maxAltitude = 0;
|
||||
const char* ptr = cmdLine;
|
||||
uint8_t argumentCount = 1;
|
||||
|
||||
idx = fastA2I(ptr);
|
||||
if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) {
|
||||
cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))) {
|
||||
argumentCount++;
|
||||
isPolygon = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))){
|
||||
argumentCount++;
|
||||
isInclusive = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))){
|
||||
argumentCount++;
|
||||
minAltitude = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))){
|
||||
argumentCount++;
|
||||
maxAltitude = fastA2I(ptr);
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))){
|
||||
argumentCount++;
|
||||
fenceAction = fastA2I(ptr);
|
||||
if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) {
|
||||
cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH);
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((ptr = nextArg(ptr))){
|
||||
argumentCount++;
|
||||
}
|
||||
|
||||
if (argumentCount != 6) {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if (isPolygon) {
|
||||
geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON;
|
||||
} else {
|
||||
geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR;
|
||||
}
|
||||
|
||||
if (isInclusive) {
|
||||
geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE;
|
||||
} else {
|
||||
geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE;
|
||||
}
|
||||
|
||||
geoZonesConfigMutable(idx)->maxAltitude = maxAltitude;
|
||||
geoZonesConfigMutable(idx)->minAltitude = minAltitude;
|
||||
geoZonesConfigMutable(idx)->fenceAction = fenceAction;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
|
||||
static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
|
||||
{
|
||||
|
|
@ -4167,6 +4412,14 @@ static void printConfig(const char *cmdline, bool doDiff)
|
|||
printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE)
|
||||
cliPrintHashLine("geozone");
|
||||
printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0));
|
||||
|
||||
cliPrintHashLine("geozone vertices");
|
||||
printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0));
|
||||
#endif
|
||||
|
||||
cliPrintHashLine("features");
|
||||
printFeature(dumpMask, &featureConfig_Copy, featureConfig());
|
||||
|
||||
|
|
@ -4550,6 +4803,9 @@ const clicmd_t cmdTable[] = {
|
|||
CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
|
||||
#endif
|
||||
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
|
||||
#ifdef USE_GEOZONE
|
||||
CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone),
|
||||
#endif
|
||||
#ifdef USE_GPS
|
||||
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
|
||||
CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites),
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -278,6 +278,14 @@ static void updateArmingStatus(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
if (geozoneIsInsideNFZ()) {
|
||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
|
||||
} else {
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* CHECK: */
|
||||
if (
|
||||
sensors(SENSOR_ACC) &&
|
||||
|
|
|
|||
|
|
@ -1768,6 +1768,53 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src)
|
||||
{
|
||||
const uint8_t idx = sbufReadU8(src);
|
||||
if (idx < MAX_GEOZONES_IN_CONFIG) {
|
||||
sbufWriteU8(dst, geoZonesConfig(idx)->type);
|
||||
sbufWriteU8(dst, geoZonesConfig(idx)->shape);
|
||||
sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude);
|
||||
sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude);
|
||||
sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction);
|
||||
sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount);
|
||||
sbufWriteU8(dst, idx);
|
||||
return MSP_RESULT_ACK;
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src)
|
||||
{
|
||||
const uint8_t zoneId = sbufReadU8(src);
|
||||
const uint8_t vertexId = sbufReadU8(src);
|
||||
if (zoneId < MAX_GEOZONES_IN_CONFIG) {
|
||||
int8_t vertexIdx = geozoneGetVertexIdx(zoneId, vertexId);
|
||||
if (vertexIdx >= 0) {
|
||||
sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId);
|
||||
sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx);
|
||||
sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat);
|
||||
sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon);
|
||||
if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) {
|
||||
int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1);
|
||||
if (vertexRadiusIdx >= 0) {
|
||||
sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat);
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
return MSP_RESULT_ACK;
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
|
||||
const uint8_t idx = sbufReadU8(src);
|
||||
if (idx < MAX_LOGIC_CONDITIONS) {
|
||||
|
|
@ -3301,6 +3348,49 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
gpsUbloxSendCommand(src->ptr, dataSize, 0);
|
||||
break;
|
||||
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
case MSP2_INAV_SET_GEOZONE:
|
||||
if (dataSize == 13) {
|
||||
uint8_t geozoneId;
|
||||
if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
|
||||
geozoneResetVertices(geozoneId, -1);
|
||||
geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src);
|
||||
geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src);
|
||||
geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src);
|
||||
geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src);
|
||||
geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src);
|
||||
geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src);
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
case MSP2_INAV_SET_GEOZONE_VERTEX:
|
||||
if (dataSize == 10 || dataSize == 14) {
|
||||
uint8_t geozoneId = 0;
|
||||
if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
uint8_t vertexId = sbufReadU8(src);
|
||||
int32_t lat = sbufReadU32(src);
|
||||
int32_t lon = sbufReadU32(src);
|
||||
if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
|
||||
if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) {
|
||||
if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
#ifdef USE_EZ_TUNE
|
||||
|
||||
case MSP2_INAV_EZ_TUNE_SET:
|
||||
|
|
@ -3862,6 +3952,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
*ret = mspFwApproachOutCommand(dst, src);
|
||||
break;
|
||||
#endif
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
case MSP2_INAV_GEOZONE:
|
||||
*ret = mspFcGeozoneOutCommand(dst, src);
|
||||
break;
|
||||
case MSP2_INAV_GEOZONE_VERTEX:
|
||||
*ret = mspFcGeozoneVerteciesOutCommand(dst, src);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_SIMULATOR
|
||||
case MSP_SIMULATOR:
|
||||
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
|
||||
|
|
|
|||
|
|
@ -339,6 +339,15 @@ void taskUpdateAux(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
void geozoneUpdateTask(timeUs_t currentTimeUs)
|
||||
{
|
||||
if (feature(FEATURE_GEOZONE)) {
|
||||
geozoneUpdate(currentTimeUs);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void fcTasksInit(void)
|
||||
{
|
||||
schedulerInit();
|
||||
|
|
@ -453,6 +462,11 @@ void fcTasksInit(void)
|
|||
#if defined(SITL_BUILD)
|
||||
serialProxyStart();
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE));
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
cfTask_t cfTasks[TASK_COUNT] = {
|
||||
|
|
@ -740,4 +754,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
},
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
[TASK_GEOZONE] = {
|
||||
.taskName = "GEOZONE",
|
||||
.taskFunc = geozoneUpdateTask,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(5),
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
#endif
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ typedef enum {
|
|||
SIMULATOR_MODE_HITL = (1 << 4),
|
||||
SIMULATOR_MODE_SITL = (1 << 5),
|
||||
|
||||
ARMING_DISABLED_GEOZONE = (1 << 6),
|
||||
ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7),
|
||||
ARMING_DISABLED_NOT_LEVEL = (1 << 8),
|
||||
ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9),
|
||||
|
|
@ -49,8 +50,8 @@ typedef enum {
|
|||
ARMING_DISABLED_DSHOT_BEEPER = (1 << 29),
|
||||
ARMING_DISABLED_LANDING_DETECTED = (1 << 30),
|
||||
|
||||
ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING |
|
||||
ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
|
||||
ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL |
|
||||
ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
|
||||
ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED |
|
||||
ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE |
|
||||
ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
|
||||
|
|
@ -65,7 +66,8 @@ typedef enum {
|
|||
// situations where we might just need the motors to spin so the
|
||||
// aircraft can move (even unpredictably) and get unstuck (e.g.
|
||||
// crashed into a high tree).
|
||||
#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_NOT_LEVEL \
|
||||
#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_GEOZONE \
|
||||
| ARMING_DISABLED_NOT_LEVEL \
|
||||
| ARMING_DISABLED_NAVIGATION_UNSAFE \
|
||||
| ARMING_DISABLED_COMPASS_NOT_CALIBRATED \
|
||||
| ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \
|
||||
|
|
@ -106,6 +108,7 @@ typedef enum {
|
|||
SOARING_MODE = (1 << 16),
|
||||
ANGLEHOLD_MODE = (1 << 17),
|
||||
NAV_FW_AUTOLAND = (1 << 18),
|
||||
NAV_SEND_TO = (1 << 19),
|
||||
} flightModeFlags_e;
|
||||
|
||||
extern uint32_t flightModeFlags;
|
||||
|
|
|
|||
|
|
@ -198,6 +198,11 @@ tables:
|
|||
- name: headtracker_dev_type
|
||||
values: ["NONE", "SERIAL", "MSP"]
|
||||
enum: headTrackerDevType_e
|
||||
- name: fence_action
|
||||
values: ["NONE", "AVOID", "POS_HOLD", "RTH"]
|
||||
enum: fenceAction_e
|
||||
- name: geozone_rth_no_way_home
|
||||
values: [RTH, EMRG_LAND]
|
||||
|
||||
constants:
|
||||
RPYL_PID_MIN: 0
|
||||
|
|
@ -3776,6 +3781,7 @@ groups:
|
|||
condition: USE_OSD || USE_DJI_HD_OSD
|
||||
members:
|
||||
- name: osd_speed_source
|
||||
condition: USE_GEOZONE
|
||||
description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR"
|
||||
default_value: "GROUND"
|
||||
field: speedSource
|
||||
|
|
@ -4314,3 +4320,49 @@ groups:
|
|||
field: roll_ratio
|
||||
min: 0
|
||||
max: 5
|
||||
- name: PG_GEOZONE_CONFIG
|
||||
type: geozone_config_t
|
||||
headers: ["navigation/navigation.h"]
|
||||
condition: USE_GEOZONE && USE_GPS
|
||||
members:
|
||||
- name: geozone_detection_distance
|
||||
description: "Distance from which a geozone is detected"
|
||||
default_value: 50000
|
||||
field: fenceDetectionDistance
|
||||
min: 0
|
||||
max: 1000000
|
||||
- name: geozone_avoid_altitude_range
|
||||
description: "Altitude range in which an attempt is made to avoid a geozone upwards"
|
||||
default_value: 5000
|
||||
field: avoidAltitudeRange
|
||||
min: 0
|
||||
max: 1000000
|
||||
- name: geozone_safe_altitude_distance
|
||||
description: "Vertical distance that must be maintained to the upper and lower limits of the zone."
|
||||
default_value: 1000
|
||||
field: safeAltitudeDistance
|
||||
min: 0
|
||||
max: 10000
|
||||
- name: geozone_safehome_as_inclusive
|
||||
description: "Treat nearest safehome as inclusive geozone"
|
||||
type: bool
|
||||
field: nearestSafeHomeAsInclusivZone
|
||||
default_value: OFF
|
||||
- name: geozone_safehome_zone_action
|
||||
description: "Fence action for safehome zone"
|
||||
default_value: "NONE"
|
||||
field: safeHomeFenceAction
|
||||
table: fence_action
|
||||
type: uint8_t
|
||||
- name: geozone_mr_stop_distance
|
||||
description: "Distance in which multirotors stops before the border"
|
||||
default_value: 15000
|
||||
min: 0
|
||||
max: 100000
|
||||
field: copterFenceStopDistance
|
||||
- name: geozone_no_way_home_action
|
||||
description: "Action if RTH with active geozones is unable to calculate a course to home"
|
||||
default_value: RTH
|
||||
field: noWayHomeAction
|
||||
table: geozone_rth_no_way_home
|
||||
type: uint8_t
|
||||
|
|
|
|||
|
|
@ -866,6 +866,8 @@ static const char * osdArmingDisabledReasonMessage(void)
|
|||
return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM);
|
||||
case ARMING_DISABLED_DSHOT_BEEPER:
|
||||
return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER);
|
||||
case ARMING_DISABLED_GEOZONE:
|
||||
return OSD_MESSAGE_STR(OSD_MSG_NFZ);
|
||||
// Cases without message
|
||||
case ARMING_DISABLED_LANDING_DETECTED:
|
||||
FALLTHROUGH;
|
||||
|
|
@ -2336,6 +2338,11 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
if (FLIGHT_MODE(NAV_FW_AUTOLAND))
|
||||
p = "LAND";
|
||||
else
|
||||
#endif
|
||||
#ifdef USE_GEOZONE
|
||||
if (FLIGHT_MODE(NAV_SEND_TO))
|
||||
p = "AUTO";
|
||||
else
|
||||
#endif
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE))
|
||||
p = "!FS!";
|
||||
|
|
@ -3797,6 +3804,52 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
clearMultiFunction = true;
|
||||
break;
|
||||
}
|
||||
#if defined(USE_GEOZONE)
|
||||
case OSD_COURSE_TO_FENCE:
|
||||
{
|
||||
if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
|
||||
int16_t panHomeDirOffset = 0;
|
||||
if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
|
||||
panHomeDirOffset = osdGetPanServoOffset();
|
||||
}
|
||||
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
|
||||
int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset;
|
||||
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);
|
||||
} else {
|
||||
if (isGeozoneActive()) {
|
||||
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
|
||||
}
|
||||
displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_H_DIST_TO_FENCE:
|
||||
{
|
||||
if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
|
||||
char buff2[12];
|
||||
osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0);
|
||||
tfp_sprintf(buff, "FD %s", buff2 );
|
||||
} else {
|
||||
strcpy(buff, "FD ---");
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case OSD_V_DIST_TO_FENCE:
|
||||
{
|
||||
if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
|
||||
char buff2[12];
|
||||
osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone));
|
||||
tfp_sprintf(buff, "FD%s", buff2);
|
||||
displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr);
|
||||
} else {
|
||||
strcpy(buff, "FD ---");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
default:
|
||||
return false;
|
||||
|
|
@ -4998,7 +5051,7 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX)
|
|||
uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX)
|
||||
{
|
||||
char buff[12];
|
||||
char outBuff[12];
|
||||
char outBuff[20];
|
||||
|
||||
const float max_gforce = accGetMeasuredMaxG();
|
||||
const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
|
||||
|
|
@ -5784,6 +5837,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
messages[messageCount++] = safehomeMessage;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
if (geozone.avoidInRTHInProgress) {
|
||||
messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH;
|
||||
}
|
||||
#endif
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed
|
||||
if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
|
||||
uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
|
||||
|
|
@ -5846,6 +5905,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
|
||||
}
|
||||
}
|
||||
#ifdef USE_GEOZONE
|
||||
char buf[12], buf1[12];
|
||||
switch (geozone.messageState) {
|
||||
case GEOZONE_MESSAGE_STATE_NFZ:
|
||||
messages[messageCount++] = OSD_MSG_NFZ;
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
|
||||
osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0);
|
||||
tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
|
||||
messages[messageCount++] = messageBuf;
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
|
||||
messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
|
||||
osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0);
|
||||
if (geozone.zoneInfo == INT32_MAX) {
|
||||
tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
|
||||
} else {
|
||||
osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
|
||||
}
|
||||
tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
|
||||
messages[messageCount++] = messageBuf;
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
|
||||
messages[messageCount++] = OSD_MSG_AVOIDING_FB;
|
||||
if (!posControl.sendTo.lockSticks) {
|
||||
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
|
||||
}
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
|
||||
messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
|
||||
if (!posControl.sendTo.lockSticks) {
|
||||
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
|
||||
}
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
|
||||
messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
|
||||
if (!posControl.sendTo.lockSticks) {
|
||||
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
|
||||
}
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
|
||||
messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
|
||||
if (!posControl.sendTo.lockSticks) {
|
||||
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
|
||||
}
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_LOITER:
|
||||
messages[messageCount++] = OSD_MSG_AVOIDING_FB;
|
||||
if (!geozone.sticksLocked) {
|
||||
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
|
||||
}
|
||||
break;
|
||||
case GEOZONE_MESSAGE_STATE_NONE:
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
} else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
if (posControl.cruise.multicopterSpeed >= 50.0f) {
|
||||
|
|
|
|||
|
|
@ -132,6 +132,19 @@
|
|||
#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME"
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE)
|
||||
#define OSD_MSG_NFZ "NO FLY ZONE"
|
||||
#define OSD_MSG_LEAVING_FZ "LEAVING FZ IN %s"
|
||||
#define OSD_MSG_OUTSIDE_FZ "OUTSIDE FZ"
|
||||
#define OSD_MSG_ENTERING_NFZ "ENTERING NFZ IN %s %s"
|
||||
#define OSD_MSG_AVOIDING_FB "AVOIDING FENCE BREACH"
|
||||
#define OSD_MSG_RETURN_TO_ZONE "RETURN TO FZ"
|
||||
#define OSD_MSG_FLYOUT_NFZ "FLY OUT NFZ"
|
||||
#define OSD_MSG_AVOIDING_ALT_BREACH "REACHED ZONE ALTITUDE LIMIT"
|
||||
#define OSD_MSG_AVOID_ZONES_RTH "AVOIDING NO FLY ZONES"
|
||||
#define OSD_MSG_GEOZONE_ACTION "PERFORM ACTION IN %s %s"
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
OSD_RSSI_VALUE,
|
||||
OSD_MAIN_BATT_VOLTAGE,
|
||||
|
|
@ -287,6 +300,9 @@ typedef enum {
|
|||
OSD_ADSB_INFO,
|
||||
OSD_BLACKBOX,
|
||||
OSD_FORMATION_FLIGHT, //153
|
||||
OSD_COURSE_TO_FENCE,
|
||||
OSD_H_DIST_TO_FENCE,
|
||||
OSD_V_DIST_TO_FENCE,
|
||||
OSD_ITEM_COUNT // MUST BE LAST
|
||||
} osd_items_e;
|
||||
|
||||
|
|
@ -467,6 +483,10 @@ typedef struct osdConfig_s {
|
|||
uint16_t adsb_ignore_plane_above_me_limit; // in metres
|
||||
#endif
|
||||
uint8_t radar_peers_display_time; // in seconds
|
||||
#ifdef USE_GEOZONE
|
||||
uint8_t geozoneDistanceWarning; // Distance to fence or action
|
||||
bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action
|
||||
#endif
|
||||
} osdConfig_t;
|
||||
|
||||
PG_DECLARE(osdConfig_t, osdConfig);
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -112,4 +112,9 @@
|
|||
#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2101
|
||||
|
||||
#define MSP2_INAV_SERVO_CONFIG 0x2200
|
||||
#define MSP2_INAV_SET_SERVO_CONFIG 0x2201
|
||||
#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
|
||||
|
|
@ -356,6 +356,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
|
||||
#endif
|
||||
#ifdef USE_GEOZONE
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState);
|
||||
#endif
|
||||
|
||||
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
||||
/** Idle state ******************************************************/
|
||||
|
|
@ -377,6 +382,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -449,6 +455,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE,
|
||||
}
|
||||
},
|
||||
/** CRUISE_HOLD mode ************************************************/
|
||||
|
|
@ -485,6 +492,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -507,6 +515,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -544,6 +553,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -566,6 +576,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -1178,6 +1189,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
}
|
||||
},
|
||||
#endif
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
[NAV_STATE_SEND_TO_INITALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_SEND_TO,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_SEND_TO_IN_PROGESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_SEND_TO,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
[NAV_STATE_SEND_TO_FINISHED] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_SEND_TO,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_FINISHED, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
|
||||
|
|
@ -1444,6 +1512,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
// Prepare controllers
|
||||
#ifdef USE_GEOZONE
|
||||
geozoneResetRTH();
|
||||
geozoneSetupRTH();
|
||||
#endif
|
||||
resetPositionController();
|
||||
resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
setupAltitudeController();
|
||||
|
|
@ -1610,6 +1682,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
|
||||
// If we have position sensor - continue home
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
#ifdef USE_GEOZONE
|
||||
// Check for NFZ in our way
|
||||
int8_t wpCount = geozoneCheckForNFZAtCourse(true);
|
||||
if (wpCount > 0) {
|
||||
calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
} else if (geozone.avoidInRTHInProgress) {
|
||||
if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {
|
||||
if (geoZoneIsLastRthWaypoint()) {
|
||||
// Already at Home?
|
||||
fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
|
||||
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
|
||||
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
|
||||
} else {
|
||||
geozoneAdvanceRthAvoidWaypoint();
|
||||
calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
}
|
||||
setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
} else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
} else {
|
||||
#endif
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
|
||||
|
||||
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
|
||||
|
|
@ -1626,6 +1728,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
#ifdef USE_GEOZONE
|
||||
}
|
||||
#endif
|
||||
}
|
||||
/* Position sensor failure timeout - land */
|
||||
else if (checkForPositionSensorTimeout()) {
|
||||
|
|
@ -1797,6 +1902,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
|
|||
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME
|
||||
}
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
geozoneResetRTH();
|
||||
#endif
|
||||
|
||||
// Prevent I-terms growing when already landed
|
||||
pidResetErrorAccumulators();
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
|
|
@ -2452,6 +2561,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (checkForPositionSensorTimeout()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
resetPositionController();
|
||||
resetAltitudeController(false);
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
|
||||
setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
} else {
|
||||
setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
}
|
||||
|
||||
posControl.sendTo.startTime = millis();
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
// "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately
|
||||
if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) {
|
||||
posControl.sendTo.lockSticks = false;
|
||||
}
|
||||
|
||||
if (!posControl.sendTo.lockSticks && areSticksDeflected()) {
|
||||
abortSendTo();
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) {
|
||||
if (posControl.sendTo.altitudeTargetRange > 0) {
|
||||
if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z + posControl.sendTo.altitudeTargetRange)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
} else {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
}
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
posControl.sendTo.lockSticks = false;
|
||||
posControl.flags.sendToActive = false;
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
#endif
|
||||
|
||||
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
|
||||
{
|
||||
navigationFSMState_t previousState;
|
||||
|
|
@ -2539,6 +2706,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
|
|||
switch (mode) {
|
||||
case RTH_HOME_ENROUTE_INITIAL:
|
||||
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
|
||||
#ifdef USE_GEOZONE
|
||||
if (geozone.currentzoneMaxAltitude > 0) {
|
||||
posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z);
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RTH_HOME_ENROUTE_PROPORTIONAL:
|
||||
|
|
@ -2552,6 +2724,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
|
|||
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
|
||||
}
|
||||
}
|
||||
#ifdef USE_GEOZONE
|
||||
if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) {
|
||||
posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RTH_HOME_ENROUTE_FINAL:
|
||||
|
|
@ -2757,6 +2934,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
|
|||
return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Calculates 2D distance between two points
|
||||
*-----------------------------------------------------------*/
|
||||
float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->x - startPos->x;
|
||||
const float deltaY = destinationPos->y - startPos->y;
|
||||
|
||||
return calc_length_pythagorean_2D(deltaX, deltaY);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Calculates distance and bearing to destination point
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2944,6 +3132,11 @@ static void updateDesiredRTHAltitude(void)
|
|||
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
|
||||
posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
|
||||
}
|
||||
#if defined(USE_GEOZONE)
|
||||
if (geozone.homeHasMaxAltitue) {
|
||||
posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3154,6 +3347,9 @@ void updateHomePosition(void)
|
|||
setHome = true;
|
||||
break;
|
||||
}
|
||||
#if defined(USE_GEOZONE) && defined (USE_GPS)
|
||||
geozoneUpdateMaxHomeAltitude();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
|
@ -3409,7 +3605,14 @@ bool isProbablyStillFlying(void)
|
|||
* Z-position controller
|
||||
*-----------------------------------------------------------*/
|
||||
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
|
||||
{
|
||||
{
|
||||
#ifdef USE_GEOZONE
|
||||
if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
|
||||
(geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) {
|
||||
return 0.0f;
|
||||
}
|
||||
#endif
|
||||
|
||||
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
|
||||
float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
|
||||
|
||||
|
|
@ -4095,6 +4298,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
// Reset RTH trackback
|
||||
resetRthTrackBack();
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
posControl.flags.sendToActive = false;
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
|
|
@ -4145,7 +4352,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
void switchNavigationFlightModes(void)
|
||||
{
|
||||
const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes);
|
||||
DISABLE_FLIGHT_MODE(disabledFlightModes);
|
||||
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
|
||||
}
|
||||
|
|
@ -4296,6 +4503,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
return NAV_FSM_EVENT_SWITCH_TO_RTH;
|
||||
}
|
||||
|
||||
if (posControl.flags.sendToActive) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_SEND_TO;
|
||||
}
|
||||
|
||||
if (posControl.flags.forcedPosholdActive) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
|
||||
}
|
||||
|
||||
/* WP mission activation control:
|
||||
* canActivateWaypoint & waypointWasActivated are used to prevent WP mission
|
||||
* auto restarting after interruption by Manual or RTH modes.
|
||||
|
|
@ -4769,6 +4984,14 @@ void navigationInit(void)
|
|||
#ifdef USE_MULTI_MISSION
|
||||
posControl.multiMissionCount = 0;
|
||||
#endif
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
posControl.flags.sendToActive = false;
|
||||
posControl.sendTo.lockSticks = false;
|
||||
posControl.sendTo.lockStickTime = 0;
|
||||
posControl.sendTo.startTime = 0;
|
||||
posControl.sendTo.targetRange = 0;
|
||||
#endif
|
||||
/* Set initial surface invalid */
|
||||
posControl.actualState.surfaceMin = -1.0f;
|
||||
|
||||
|
|
@ -4851,6 +5074,40 @@ rthState_e getStateOfForcedRTH(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
// "Send to" is not to intended to be activated by user, only by external event
|
||||
void activateSendTo(void)
|
||||
{
|
||||
if (!geozone.avoidInRTHInProgress) {
|
||||
abortFixedWingLaunch();
|
||||
posControl.flags.sendToActive = true;
|
||||
navProcessFSMEvents(selectNavEventFromBoxModeInput());
|
||||
}
|
||||
}
|
||||
|
||||
void abortSendTo(void)
|
||||
{
|
||||
posControl.flags.sendToActive = false;
|
||||
navProcessFSMEvents(selectNavEventFromBoxModeInput());
|
||||
}
|
||||
|
||||
void activateForcedPosHold(void)
|
||||
{
|
||||
if (!geozone.avoidInRTHInProgress) {
|
||||
abortFixedWingLaunch();
|
||||
posControl.flags.forcedPosholdActive = true;
|
||||
navProcessFSMEvents(selectNavEventFromBoxModeInput());
|
||||
}
|
||||
}
|
||||
|
||||
void abortForcedPosHold(void)
|
||||
{
|
||||
posControl.flags.forcedPosholdActive = false;
|
||||
navProcessFSMEvents(selectNavEventFromBoxModeInput());
|
||||
}
|
||||
#endif
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Ability to execute Emergency Landing on external event
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
|
|||
|
|
@ -116,6 +116,119 @@ void resetFwAutolandApproach(int8_t idx);
|
|||
|
||||
#endif
|
||||
|
||||
#if defined(USE_GEOZONE)
|
||||
|
||||
#define MAX_GEOZONES_IN_CONFIG 63
|
||||
#define MAX_VERTICES_IN_CONFIG 126
|
||||
|
||||
typedef enum {
|
||||
GEOZONE_MESSAGE_STATE_NONE,
|
||||
GEOZONE_MESSAGE_STATE_NFZ,
|
||||
GEOZONE_MESSAGE_STATE_LEAVING_FZ,
|
||||
GEOZONE_MESSAGE_STATE_OUTSIDE_FZ,
|
||||
GEOZONE_MESSAGE_STATE_ENTERING_NFZ,
|
||||
GEOZONE_MESSAGE_STATE_AVOIDING_FB,
|
||||
GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE,
|
||||
GEOZONE_MESSAGE_STATE_FLYOUT_NFZ,
|
||||
GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH,
|
||||
GEOZONE_MESSAGE_STATE_LOITER
|
||||
} geozoneMessageState_e;
|
||||
|
||||
enum fenceAction_e {
|
||||
GEOFENCE_ACTION_NONE,
|
||||
GEOFENCE_ACTION_AVOID,
|
||||
GEOFENCE_ACTION_POS_HOLD,
|
||||
GEOFENCE_ACTION_RTH,
|
||||
};
|
||||
|
||||
enum noWayHomeAction {
|
||||
NO_WAY_HOME_ACTION_RTH,
|
||||
NO_WAY_HOME_ACTION_EMRG_LAND,
|
||||
};
|
||||
|
||||
#define GEOZONE_SHAPE_CIRCULAR 0
|
||||
#define GEOZONE_SHAPE_POLYGON 1
|
||||
|
||||
#define GEOZONE_TYPE_EXCLUSIVE 0
|
||||
#define GEOZONE_TYPE_INCLUSIVE 1
|
||||
|
||||
typedef struct geoZoneConfig_s
|
||||
{
|
||||
uint8_t shape;
|
||||
uint8_t type;
|
||||
int32_t minAltitude;
|
||||
int32_t maxAltitude;
|
||||
uint8_t fenceAction;
|
||||
uint8_t vertexCount;
|
||||
} geoZoneConfig_t;
|
||||
|
||||
typedef struct geozone_config_s
|
||||
{
|
||||
uint32_t fenceDetectionDistance;
|
||||
uint16_t avoidAltitudeRange;
|
||||
uint16_t safeAltitudeDistance;
|
||||
bool nearestSafeHomeAsInclusivZone;
|
||||
uint8_t safeHomeFenceAction;
|
||||
uint32_t copterFenceStopDistance;
|
||||
uint8_t noWayHomeAction;
|
||||
} geozone_config_t;
|
||||
|
||||
typedef struct vertexConfig_s
|
||||
{
|
||||
int8_t zoneId;
|
||||
uint8_t idx;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
} vertexConfig_t;
|
||||
|
||||
PG_DECLARE(geozone_config_t, geoZoneConfig);
|
||||
PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig);
|
||||
PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices);
|
||||
|
||||
typedef struct geozone_s {
|
||||
bool insideFz;
|
||||
bool insideNfz;
|
||||
uint32_t distanceToZoneBorder3d;
|
||||
int32_t vertDistanceToZoneBorder;
|
||||
geozoneMessageState_e messageState;
|
||||
int32_t directionToNearestZone;
|
||||
int32_t distanceHorToNearestZone;
|
||||
int32_t distanceVertToNearestZone;
|
||||
int32_t zoneInfo;
|
||||
int32_t currentzoneMaxAltitude;
|
||||
int32_t currentzoneMinAltitude;
|
||||
bool sticksLocked;
|
||||
int8_t loiterDir;
|
||||
bool avoidInRTHInProgress;
|
||||
int32_t maxHomeAltitude;
|
||||
bool homeHasMaxAltitue;
|
||||
} geozone_t;
|
||||
|
||||
extern geozone_t geozone;
|
||||
|
||||
bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon);
|
||||
int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId);
|
||||
bool isGeozoneActive(void);
|
||||
uint8_t geozoneGetUsedVerticesCount(void);
|
||||
void geozoneResetVertices(int8_t zoneId, int16_t idx);
|
||||
void geozoneUpdate(timeUs_t curentTimeUs);
|
||||
bool geozoneIsInsideNFZ(void);
|
||||
void geozoneAdvanceRthAvoidWaypoint(void);
|
||||
int8_t geozoneCheckForNFZAtCourse(bool isRTH);
|
||||
bool geoZoneIsLastRthWaypoint(void);
|
||||
fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void);
|
||||
void geozoneSetupRTH(void);
|
||||
void geozoneResetRTH(void);
|
||||
void geozoneUpdateMaxHomeAltitude(void);
|
||||
uint32_t geozoneGetDetectionDistance(void);
|
||||
|
||||
void activateSendTo(void);
|
||||
void abortSendTo(void);
|
||||
void activateForcedPosHold(void);
|
||||
void abortForcedPosHold(void);
|
||||
|
||||
#endif
|
||||
|
||||
#ifndef NAV_MAX_WAYPOINTS
|
||||
#define NAV_MAX_WAYPOINTS 15
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -250,6 +250,12 @@ static int8_t loiterDirection(void) {
|
|||
dir *= -1;
|
||||
}
|
||||
|
||||
#ifdef USE_GEOZONE
|
||||
if (geozone.loiterDir != 0) {
|
||||
dir = geozone.loiterDir;
|
||||
}
|
||||
#endif
|
||||
|
||||
return dir;
|
||||
}
|
||||
|
||||
|
|
|
|||
1598
src/main/navigation/navigation_geozone.c
Executable file
1598
src/main/navigation/navigation_geozone.c
Executable file
File diff suppressed because it is too large
Load diff
480
src/main/navigation/navigation_geozone_calculations.h
Executable file
480
src/main/navigation/navigation_geozone_calculations.h
Executable file
|
|
@ -0,0 +1,480 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* INAV is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* INAV is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "common/vector.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
|
||||
#define K_EPSILON 1e-8
|
||||
|
||||
static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius)
|
||||
{
|
||||
return calculateDistance2(point, center) < radius;
|
||||
}
|
||||
|
||||
static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum)
|
||||
{
|
||||
bool result = false;
|
||||
fpVector2_t *p1, *p2;
|
||||
fpVector2_t* prev = &vertices[verticesNum - 1];
|
||||
fpVector2_t *current;
|
||||
for (uint8_t i = 0; i < verticesNum; i++)
|
||||
{
|
||||
current = &vertices[i];
|
||||
|
||||
if (current->x > prev->x) {
|
||||
p1 = prev;
|
||||
p2 = current;
|
||||
} else {
|
||||
p1 = current;
|
||||
p2 = prev;
|
||||
}
|
||||
|
||||
if ((current->x < point->x) == (point->x <= prev->x)
|
||||
&& (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x))
|
||||
{
|
||||
result = !result;
|
||||
}
|
||||
prev = current;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
static float angelFromSideLength(const float a, const float b, const float c)
|
||||
{
|
||||
return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c));
|
||||
}
|
||||
|
||||
static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) {
|
||||
return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0;
|
||||
}
|
||||
|
||||
static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c)
|
||||
{
|
||||
float ab = calculateDistance2(a, b);
|
||||
float ac = calculateDistance2(a, c);
|
||||
float bc = calculateDistance2(b, c);
|
||||
|
||||
return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc));
|
||||
}
|
||||
|
||||
static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance)
|
||||
{
|
||||
fpVector2_t dir, a;
|
||||
float fac;
|
||||
vector2Sub(&dir, end, start);
|
||||
fac = distance / fast_fsqrtf(vector2NormSquared(&dir));
|
||||
vector2Scale(&a, &dir, fac);
|
||||
vector2Add(result, start, &a);
|
||||
}
|
||||
|
||||
// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection
|
||||
bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment)
|
||||
{
|
||||
intersection->x = intersection->y = 0;
|
||||
|
||||
// Double precision is needed here
|
||||
double s1 = line1End->x - line1Start->x;
|
||||
double t1 = -(line2End->x - line2Start->x);
|
||||
double r1 = line2Start->x - line1Start->x;
|
||||
|
||||
double s2 = line1End->y - line1Start->y;
|
||||
double t2 = -(line2End->y - line2Start->y);
|
||||
double r2 = line2Start->y - line1Start->y;
|
||||
|
||||
// Use Cramer's rule for the solution of the system of linear equations
|
||||
double determ = s1 * t2 - t1 * s2;
|
||||
if (determ == 0) { // No solution
|
||||
return false;
|
||||
}
|
||||
|
||||
double s0 = (r1 * t2 - t1 * r2) / determ;
|
||||
double t0 = (s1 * r2 - r1 * s2) / determ;
|
||||
|
||||
if (s0 == 0 && t0 == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// No intersection
|
||||
if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x));
|
||||
intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos)
|
||||
{
|
||||
return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z));
|
||||
}
|
||||
|
||||
static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3)
|
||||
{
|
||||
fpVector3_t result, a, b;
|
||||
vectorSub(&a, p2, p1);
|
||||
vectorSub(&b, p3, p1);
|
||||
vectorCrossProduct(&result, &a, &b);
|
||||
return result;
|
||||
}
|
||||
|
||||
static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2)
|
||||
{
|
||||
fpVector3_t result;
|
||||
vectorSub(&result, p1, p2);
|
||||
return result;
|
||||
}
|
||||
|
||||
static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides)
|
||||
{
|
||||
for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) {
|
||||
polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x;
|
||||
polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y;
|
||||
}
|
||||
}
|
||||
|
||||
// TRUE if point is in the same direction from pos as ref
|
||||
static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref)
|
||||
{
|
||||
fpVector3_t refDir = calcDirVectorFromPoints(pos, ref);
|
||||
fpVector3_t dir = calcDirVectorFromPoints(point, pos);
|
||||
return vectorDotProduct(&refDir, &dir) < 0.0f;
|
||||
}
|
||||
|
||||
static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point)
|
||||
{
|
||||
fpVector2_t ap, ab, prod2, result;
|
||||
float distance, magAb, prod;
|
||||
vector2Sub(&ap, point, lineStart);
|
||||
vector2Sub(&ab, lineEnd, lineStart);
|
||||
magAb = vector2NormSquared(&ab);
|
||||
prod = vector2DotProduct(&ap, &ab);
|
||||
distance = prod / magAb;
|
||||
if (distance < 0) {
|
||||
return *lineStart;
|
||||
} else if (distance > 1) {
|
||||
return *lineEnd;
|
||||
}
|
||||
vector2Scale(&prod2, &ab, distance);
|
||||
vector2Add(&result, lineStart, &prod2);
|
||||
return result;
|
||||
}
|
||||
|
||||
static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint)
|
||||
{
|
||||
float a = roundf(calculateDistance2(linepoint, lineStart));
|
||||
float b = roundf(calculateDistance2(linepoint, lineEnd));
|
||||
float c = roundf(calculateDistance2(lineStart, lineEnd));
|
||||
return ABS(a + b - c) <= 1;
|
||||
}
|
||||
|
||||
static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint)
|
||||
{
|
||||
float a = roundf(calculateDistance3(linepoint, lineStart));
|
||||
float b = roundf(calculateDistance3(linepoint, lineEnd));
|
||||
float c = roundf(calculateDistance3(lineStart, lineEnd));
|
||||
return ABS(a + b - c) <= 1;
|
||||
}
|
||||
|
||||
static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint)
|
||||
{
|
||||
if (vectorDotProduct(linePoint, planeNormal) == 0) {
|
||||
return false;
|
||||
}
|
||||
fpVector3_t diff, p1, p4;
|
||||
float p2 = 0, p3 = 0;
|
||||
vectorSub(&diff, linePoint, planePoint);
|
||||
vectorAdd(&p1, &diff, planePoint);
|
||||
p2 = vectorDotProduct(&diff, planeNormal);
|
||||
p3 = vectorDotProduct(lineVector, planeNormal);
|
||||
vectorScale(&p4, lineVector, -p2 / p3);
|
||||
vectorAdd(result, &p1, &p4);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// Möller–Trumbore intersection algorithm
|
||||
static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2)
|
||||
{
|
||||
fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod;
|
||||
float det, invDet, t, u, v;
|
||||
|
||||
vectorSub(&v0v1, v1, v0);
|
||||
vectorSub(&v0v2, v2, v0);
|
||||
vectorCrossProduct(&pvec, dir, &v0v2);
|
||||
det = vectorDotProduct(&v0v1, &pvec);
|
||||
if (fabsf(det) < K_EPSILON) {
|
||||
return false;
|
||||
}
|
||||
invDet = 1.f / det;
|
||||
vectorSub(&tvec, org, v0);
|
||||
u = vectorDotProduct(&tvec, &pvec) * invDet;
|
||||
if (u < 0 || u > 1) {
|
||||
return false;
|
||||
}
|
||||
vectorCrossProduct(&quvec, &tvec, &v0v1);
|
||||
v = vectorDotProduct(dir, &quvec) * invDet;
|
||||
if (v < 0 || u + v > 1) {
|
||||
return false;
|
||||
}
|
||||
t = vectorDotProduct(&v0v2, &quvec) * invDet;
|
||||
vectorScale(&prod, dir, t);
|
||||
vectorAdd(intersection, &prod, org);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum)
|
||||
{
|
||||
if (verticesNum < 3) {
|
||||
return false;
|
||||
}
|
||||
|
||||
fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height };
|
||||
fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height };
|
||||
fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height };
|
||||
fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3);
|
||||
fpVector3_t dir = calcDirVectorFromPoints(pos, pos2);
|
||||
|
||||
fpVector3_t tmp;
|
||||
if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) {
|
||||
if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) {
|
||||
memcpy(intersect, &tmp, sizeof(fpVector3_t));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius)
|
||||
{
|
||||
// Unfortunately, we need double precision here
|
||||
double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x));
|
||||
double yIntercept = (double)startPos->y - slope * (double)startPos->x;
|
||||
|
||||
double a = (double)1.0 + sq(slope);
|
||||
double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y);
|
||||
double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius);
|
||||
|
||||
double discr = sq(b) - (double)4.0 * a * c;
|
||||
if (discr > 0)
|
||||
{
|
||||
double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a);
|
||||
double y1 = slope * x1 + yIntercept;
|
||||
double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a);
|
||||
double y2 = slope * x2 + yIntercept;
|
||||
|
||||
intersection1->x = (float)x1;
|
||||
intersection1->y = (float)y1;
|
||||
intersection2->x = (float)x2;
|
||||
intersection2->y = (float)y2;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset)
|
||||
{
|
||||
fpVector2_t* prev = &verticesOld[numVertices - 1];
|
||||
fpVector2_t* current;
|
||||
fpVector2_t* next;
|
||||
for (uint8_t i = 0; i < numVertices; i++) {
|
||||
current = &verticesOld[i];
|
||||
if (i + 1 < numVertices) {
|
||||
next = &verticesOld[i + 1];
|
||||
}
|
||||
else {
|
||||
next = &verticesOld[0];
|
||||
}
|
||||
|
||||
fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect;
|
||||
vector2Sub(&v, current, prev);
|
||||
vector2Normalize(&vn, &v);
|
||||
vector2Scale(&vs, &vn, offset);
|
||||
pcp1.x = prev->x - vs.y;
|
||||
pcp1.y = prev->y + vs.x;
|
||||
pcp2.x = current->x - vs.y;
|
||||
pcp2.y = current->y + vs.x;
|
||||
|
||||
vector2Sub(&v, next, current);
|
||||
vector2Normalize(&vn, &v);
|
||||
vector2Scale(&vs, &vn, offset);
|
||||
pcn1.x = current->x - vs.y;
|
||||
pcn1.y = current->y + vs.x;
|
||||
pcn2.x = next->x - vs.y;
|
||||
pcn2.y = next->y + vs.x;
|
||||
|
||||
if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) {
|
||||
verticesNew[i].x = intersect.x;
|
||||
verticesNew[i].y = intersect.y;
|
||||
}
|
||||
prev = current;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculates the nearest intersection point
|
||||
// Inspired by raytracing algortyhms
|
||||
static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside)
|
||||
{
|
||||
float distToIntersection = FLT_MAX;
|
||||
fpVector3_t intersect;
|
||||
|
||||
fpVector2_t i1, i2;
|
||||
if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) {
|
||||
intersect.x = i1.x;
|
||||
intersect.y = i1.y;
|
||||
}
|
||||
else {
|
||||
intersect.x = i2.x;
|
||||
intersect.y = i2.y;
|
||||
}
|
||||
|
||||
float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect);
|
||||
float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos);
|
||||
fpVector2_t p4, p5, p6, p7;
|
||||
p4.x = 0;
|
||||
p4.y = endPos->z;
|
||||
p5.x = dist2;
|
||||
p5.y = startPos->z;
|
||||
p6.x = dist1;
|
||||
p6.y = circleCenter->z;
|
||||
p7.x = dist1;
|
||||
p7.y = circleCenter->z + height;
|
||||
|
||||
fpVector2_t heightIntersection;
|
||||
if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) {
|
||||
intersect.z = heightIntersection.y;
|
||||
if (isInFront(startPos, &intersect, endPos)) {
|
||||
distToIntersection = calculateDistance3(startPos, &intersect);
|
||||
}
|
||||
}
|
||||
|
||||
fpVector3_t intersectCap;
|
||||
fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos);
|
||||
if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) {
|
||||
fpVector3_t p1 = *circleCenter;
|
||||
p1.x = circleCenter->x + radius;
|
||||
fpVector3_t p2 = *circleCenter;
|
||||
p2.y = circleCenter->y + radius;
|
||||
fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2);
|
||||
|
||||
if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter)
|
||||
&& isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius)
|
||||
&& isInFront(startPos, &intersectCap, endPos)) {
|
||||
float distanceCap = calculateDistance3(startPos, &intersectCap);
|
||||
if (distanceCap < distToIntersection) {
|
||||
distToIntersection = distanceCap;
|
||||
intersect = intersectCap;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (startPos->z > circleCenter->z + height || inside) {
|
||||
fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height };
|
||||
fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height };
|
||||
fpVector3_t p3 = *circleCenter;
|
||||
p3.z = circleCenter->z + height;
|
||||
fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2);
|
||||
|
||||
if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3)
|
||||
&& isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius)
|
||||
&& isInFront(startPos, &intersectCap, endPos)) {
|
||||
float distanceCap = calculateDistance3(startPos, &intersectCap);
|
||||
if (distanceCap < distToIntersection) {
|
||||
distToIntersection = distanceCap;
|
||||
intersect = intersectCap;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (distToIntersection < FLT_MAX) {
|
||||
*distance = distToIntersection;
|
||||
memcpy(intersection, &intersect, sizeof(fpVector3_t));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone)
|
||||
{
|
||||
float distToIntersection = FLT_MAX;
|
||||
fpVector3_t intersect;
|
||||
|
||||
fpVector2_t* prev = &vertices[numVertices - 1];
|
||||
fpVector2_t* current;
|
||||
for (uint8_t i = 0; i < numVertices; i++) {
|
||||
current = &vertices[i];
|
||||
|
||||
fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight };
|
||||
fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight };
|
||||
fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight };
|
||||
fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight};
|
||||
|
||||
fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos);
|
||||
fpVector3_t intersectCurrent;
|
||||
if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3)
|
||||
|| calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) {
|
||||
float distWall = calculateDistance3(startPos, &intersectCurrent);
|
||||
if (distWall < distToIntersection) {
|
||||
distToIntersection = distWall;
|
||||
intersect = intersectCurrent;
|
||||
}
|
||||
}
|
||||
prev = current;
|
||||
}
|
||||
|
||||
fpVector3_t intersectCap;
|
||||
if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) {
|
||||
if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos))
|
||||
{
|
||||
float distanceCap = calculateDistance3(startPos, &intersectCap);
|
||||
if (distanceCap < distToIntersection) {
|
||||
distToIntersection = distanceCap;
|
||||
intersect = intersectCap;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (startPos->z > maxHeight || isInclusiveZone) {
|
||||
if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos))
|
||||
{
|
||||
float distanceCap = calculateDistance3(startPos, &intersectCap);
|
||||
if (distanceCap < distToIntersection) {
|
||||
distToIntersection = distanceCap;
|
||||
intersect = intersectCap;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (distToIntersection < FLT_MAX) {
|
||||
*distance = distToIntersection;
|
||||
memcpy(intersection, &intersect, sizeof(fpVector3_t));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
||||
|
|
|
|||
|
|
@ -221,3 +221,4 @@
|
|||
|
||||
#define USE_EZ_TUNE
|
||||
#define USE_ADAPTIVE_FILTER
|
||||
#define USE_GEOZONE
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue