Skip to content

Commit 2f20927

Browse files
breadovenbreadoven
committed
Issue iNavFlight#10830 fixes
1 parent e0d4627 commit 2f20927

3 files changed

Lines changed: 85 additions & 70 deletions

File tree

‎src/main/io/osd.c‎

Lines changed: 74 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -6139,8 +6139,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
61396139
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)];
61406140

61416141
/* WARNING: ensure number of messages returned does not exceed messages array size
6142-
* Messages array set 1 larger than maximum expected message count of 6 */
6143-
const char *messages[7];
6142+
* Messages array set 1 larger than maximum expected message count of 7 */
6143+
const char *messages[8];
61446144
unsigned messageCount = 0;
61456145

61466146
const char *failsafeInfoMessage = NULL;
@@ -6213,66 +6213,66 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
62136213
} else if (STATE(LANDING_DETECTED)) {
62146214
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
62156215
} else {
6216+
/* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
6217+
/* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */
62166218
#ifdef USE_GEOZONE
6217-
char buf[12], buf1[12];
6218-
switch (geozone.messageState) {
6219-
case GEOZONE_MESSAGE_STATE_NFZ:
6220-
messages[messageCount++] = OSD_MSG_NFZ;
6221-
break;
6222-
case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
6223-
osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
6224-
tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
6225-
messages[messageCount++] = messageBuf;
6226-
break;
6227-
case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
6228-
messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
6229-
break;
6230-
case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
6231-
osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
6232-
if (geozone.zoneInfo == INT32_MAX) {
6233-
tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
6234-
} else {
6235-
osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
6236-
}
6237-
tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
6238-
messages[messageCount++] = messageBuf;
6239-
break;
6240-
case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
6241-
messages[messageCount++] = OSD_MSG_AVOIDING_FB;
6242-
if (!posControl.sendTo.lockSticks) {
6243-
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6244-
}
6245-
break;
6246-
case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
6247-
messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
6248-
if (!posControl.sendTo.lockSticks) {
6249-
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6250-
}
6251-
break;
6252-
case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
6253-
messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
6254-
if (!posControl.sendTo.lockSticks) {
6255-
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6256-
}
6257-
break;
6258-
case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
6259-
messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
6260-
if (!posControl.sendTo.lockSticks) {
6261-
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6262-
}
6263-
break;
6264-
case GEOZONE_MESSAGE_STATE_POS_HOLD:
6265-
messages[messageCount++] = OSD_MSG_AVOIDING_FB;
6266-
if (!geozone.sticksLocked) {
6267-
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6268-
}
6269-
break;
6270-
case GEOZONE_MESSAGE_STATE_NONE:
6271-
break;
6272-
}
6219+
char buf[12], buf1[12];
6220+
switch (geozone.messageState) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
6221+
case GEOZONE_MESSAGE_STATE_NFZ:
6222+
messages[messageCount++] = OSD_MSG_NFZ;
6223+
break;
6224+
case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
6225+
osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
6226+
tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
6227+
messages[messageCount++] = messageBuf;
6228+
break;
6229+
case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
6230+
messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
6231+
break;
6232+
case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
6233+
osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
6234+
if (geozone.zoneInfo == INT32_MAX) {
6235+
tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
6236+
} else {
6237+
osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
6238+
}
6239+
tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
6240+
messages[messageCount++] = messageBuf;
6241+
break;
6242+
case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
6243+
messages[messageCount++] = OSD_MSG_AVOIDING_FB;
6244+
if (!posControl.sendTo.lockSticks) {
6245+
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6246+
}
6247+
break;
6248+
case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
6249+
messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
6250+
if (!posControl.sendTo.lockSticks) {
6251+
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6252+
}
6253+
break;
6254+
case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
6255+
messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
6256+
if (!posControl.sendTo.lockSticks) {
6257+
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6258+
}
6259+
break;
6260+
case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
6261+
messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
6262+
if (!posControl.sendTo.lockSticks) {
6263+
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6264+
}
6265+
break;
6266+
case GEOZONE_MESSAGE_STATE_POS_HOLD:
6267+
messages[messageCount++] = OSD_MSG_AVOIDING_FB;
6268+
if (!geozone.sticksLocked) {
6269+
messages[messageCount++] = OSD_MSG_MOVE_STICKS;
6270+
}
6271+
break;
6272+
case GEOZONE_MESSAGE_STATE_NONE:
6273+
break;
6274+
}
62736275
#endif
6274-
/* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
6275-
/* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
62766276
if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
62776277
#ifdef USE_FW_AUTOLAND
62786278
if (canFwLandingBeCancelled()) {
@@ -6309,7 +6309,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
63096309
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
63106310
}
63116311
}
6312-
63136312
} else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
63146313
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
63156314
if (posControl.cruise.multicopterSpeed >= 50.0f) {
@@ -6323,12 +6322,21 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
63236322
} else if (FLIGHT_MODE(HEADFREE_MODE)) {
63246323
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
63256324
}
6326-
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
6327-
/* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes
6328-
* then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field.
6329-
* In this case indicate ALTHOLD is active via a system message */
63306325

6331-
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
6326+
if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
6327+
if (posControl.flags.isTerrainFollowEnabled) {
6328+
if (posControl.flags.estAglStatus == EST_TRUSTED) {
6329+
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_OK);
6330+
} else {
6331+
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_BAD);
6332+
}
6333+
} else if (!navigationRequiresAngleMode()) {
6334+
/* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes
6335+
* then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field.
6336+
* In this case indicate ALTHOLD is active via a system message */
6337+
6338+
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
6339+
}
63326340
}
63336341
}
63346342
}

‎src/main/io/osd.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -109,6 +109,8 @@
109109
#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH"
110110
#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)"
111111
#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)"
112+
#define OSD_MSG_SURFACE_OK "(SURFACE)"
113+
#define OSD_MSG_SURFACE_BAD "(!SURFACE INOP!)"
112114
#define OSD_MSG_AUTOTRIM "(AUTOTRIM)"
113115
#define OSD_MSG_AUTOTUNE "(AUTOTUNE)"
114116
#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO"

‎src/main/navigation/navigation_multicopter.c‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -837,10 +837,15 @@ bool isMulticopterLandingDetected(void)
837837
/* Basic condition to start looking for landing
838838
* Detection active during Failsafe only if throttle below mid hover throttle
839839
* and WP mission not active (except landing states).
840-
* Also active in non autonomous flight modes but only when thottle low */
841-
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
842-
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover())
843-
|| (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
840+
* Also active in non autonomous flight modes but only when thottle low.
841+
* Throttle low detection only allowed during Surface if AGL trusted and below 10cm */
842+
bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode();
843+
if (posControl.flags.isTerrainFollowEnabled) {
844+
throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 10.0f;
845+
}
846+
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) ||
847+
(FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) ||
848+
(throttleLowCheckAllowed && throttleStickIsLow());
844849

845850
static timeMs_t landingDetectorStartedAt;
846851

0 commit comments

Comments
 (0)