From 2f2092709b341a60377bdb15c5f9e00ccaf6bf5f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 13 Mar 2026 13:04:14 +0000 Subject: [PATCH 1/7] Issue #10830 fixes --- src/main/io/osd.c | 140 ++++++++++--------- src/main/io/osd.h | 2 + src/main/navigation/navigation_multicopter.c | 13 +- 3 files changed, 85 insertions(+), 70 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4fe19a4040..300c3e2ef57 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6139,8 +6139,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)]; /* WARNING: ensure number of messages returned does not exceed messages array size - * Messages array set 1 larger than maximum expected message count of 6 */ - const char *messages[7]; + * Messages array set 1 larger than maximum expected message count of 7 */ + const char *messages[8]; unsigned messageCount = 0; const char *failsafeInfoMessage = NULL; @@ -6213,66 +6213,66 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } else { + /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ + /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */ #ifdef USE_GEOZONE - char buf[12], buf1[12]; - switch (geozone.messageState) { - case GEOZONE_MESSAGE_STATE_NFZ: - messages[messageCount++] = OSD_MSG_NFZ; - break; - case GEOZONE_MESSAGE_STATE_LEAVING_FZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); - tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: - messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; - break; - case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); - if (geozone.zoneInfo == INT32_MAX) { - tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); - } else { - osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); - } - tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_FB: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: - messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: - messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: - messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_POS_HOLD: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_NONE: - break; - } + char buf[12], buf1[12]; + switch (geozone.messageState) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_POS_HOLD: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } #endif - /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ - /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ #ifdef USE_FW_AUTOLAND if (canFwLandingBeCancelled()) { @@ -6309,7 +6309,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } - } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { @@ -6323,12 +6322,21 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); } - if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { - /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes - * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field. - * In this case indicate ALTHOLD is active via a system message */ - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + if (posControl.flags.isTerrainFollowEnabled) { + if (posControl.flags.estAglStatus == EST_TRUSTED) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_OK); + } else { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_BAD); + } + } else if (!navigationRequiresAngleMode()) { + /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes + * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field. + * In this case indicate ALTHOLD is active via a system message */ + + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + } } } } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 88240ff84c0..7a8ec3cb682 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -109,6 +109,8 @@ #define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" #define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" +#define OSD_MSG_SURFACE_OK "(SURFACE)" +#define OSD_MSG_SURFACE_BAD "(!SURFACE INOP!)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" #define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 7da742829c5..5901f1ad9e1 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -837,10 +837,15 @@ bool isMulticopterLandingDetected(void) /* Basic condition to start looking for landing * Detection active during Failsafe only if throttle below mid hover throttle * and WP mission not active (except landing states). - * Also active in non autonomous flight modes but only when thottle low */ - bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) - || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) - || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow()); + * Also active in non autonomous flight modes but only when thottle low. + * Throttle low detection only allowed during Surface if AGL trusted and below 10cm */ + bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode(); + if (posControl.flags.isTerrainFollowEnabled) { + throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 10.0f; + } + bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || + (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || + (throttleLowCheckAllowed && throttleStickIsLow()); static timeMs_t landingDetectorStartedAt; From 394395ec557196576d4f1c4218ca9a825357b057 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 16 May 2026 15:13:31 +0100 Subject: [PATCH 2/7] Update navigation_multicopter.c --- src/main/navigation/navigation_multicopter.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 5901f1ad9e1..690987ecade 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -130,7 +130,11 @@ bool adjustMulticopterAltitudeFromRCInput(void) updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET); } else { - updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); + const int16_t throttleIdle = getThrottleIdleValue(); + const int16_t throttleMid = rcLookupThrottleMid(); + const int16_t descentRate = scaleRange(constrain(rcCommand[THROTTLE], throttleIdle, throttleMid), throttleIdle, throttleMid, -200, -50); + + updateClimbRateToAltitudeController(descentRate, 0, ROC_TO_ALT_CONSTANT); } // In surface tracking we always indicate that we're adjusting altitude @@ -841,7 +845,7 @@ bool isMulticopterLandingDetected(void) * Throttle low detection only allowed during Surface if AGL trusted and below 10cm */ bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode(); if (posControl.flags.isTerrainFollowEnabled) { - throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 10.0f; + throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f; } bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || From eee931d27fd60d170bf20e4a88f3622dedd971fb Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 17 May 2026 11:11:23 +0100 Subject: [PATCH 3/7] fix descent below 10cm --- src/main/navigation/navigation_multicopter.c | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 690987ecade..37fa3f9f4cd 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -122,19 +122,23 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) bool adjustMulticopterAltitudeFromRCInput(void) { if (posControl.flags.isTerrainFollowEnabled) { - const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle(), 0, navConfig()->general.max_terrain_follow_altitude); + const int16_t altTarget = scaleRange(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle(), 0, navConfig()->general.max_terrain_follow_altitude); // In terrain follow mode we apply different logic for terrain control - if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { + if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10) { // We have solid terrain sensor signal - directly map throttle to altitude updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET); } else { - const int16_t throttleIdle = getThrottleIdleValue(); - const int16_t throttleMid = rcLookupThrottleMid(); - const int16_t descentRate = scaleRange(constrain(rcCommand[THROTTLE], throttleIdle, throttleMid), throttleIdle, throttleMid, -200, -50); + int16_t climbRate = -50; - updateClimbRateToAltitudeController(descentRate, 0, ROC_TO_ALT_CONSTANT); + if (posControl.flags.estAglStatus != EST_TRUSTED) { + const int16_t throttleIdle = getThrottleIdleValue(); + const int16_t throttleMid = rcLookupThrottleMid(); + climbRate = scaleRange(constrain(rcCommand[THROTTLE], throttleIdle, throttleMid), throttleIdle, throttleMid, -200, -50); + } + + updateClimbRateToAltitudeController(climbRate, 0, ROC_TO_ALT_CONSTANT); } // In surface tracking we always indicate that we're adjusting altitude From d53f92c9c3b9d2fc6382aa2846d5f9ae76ba4304 Mon Sep 17 00:00:00 2001 From: Sensei Date: Sun, 17 May 2026 20:56:24 -0500 Subject: [PATCH 4/7] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/main/navigation/navigation_multicopter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 37fa3f9f4cd..d0d92531cf2 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -845,7 +845,7 @@ bool isMulticopterLandingDetected(void) /* Basic condition to start looking for landing * Detection active during Failsafe only if throttle below mid hover throttle * and WP mission not active (except landing states). - * Also active in non autonomous flight modes but only when thottle low. + * Also active in non autonomous flight modes but only when throttle low. * Throttle low detection only allowed during Surface if AGL trusted and below 10cm */ bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode(); if (posControl.flags.isTerrainFollowEnabled) { From 3533d16af7a6b5ec2c94cdebae1642a2b7cfdb52 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 18 May 2026 10:24:12 +0100 Subject: [PATCH 5/7] correct typo --- src/main/navigation/navigation_multicopter.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index d0d92531cf2..a83a3541681 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -846,11 +846,14 @@ bool isMulticopterLandingDetected(void) * Detection active during Failsafe only if throttle below mid hover throttle * and WP mission not active (except landing states). * Also active in non autonomous flight modes but only when throttle low. - * Throttle low detection only allowed during Surface if AGL trusted and below 10cm */ + * Throttle low detection only allowed during Surface if AGL trusted and below 50cm */ + bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode(); + if (posControl.flags.isTerrainFollowEnabled) { throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f; } + bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || (throttleLowCheckAllowed && throttleStickIsLow()); From ffcb7c8afbce75f5a209ebc245ef13a3c3f0957e Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 21 May 2026 11:01:11 +0100 Subject: [PATCH 6/7] fixes --- src/main/io/osd.h | 2 +- src/main/navigation/navigation_multicopter.c | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7a8ec3cb682..e55aeb22461 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -110,7 +110,7 @@ #define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" #define OSD_MSG_SURFACE_OK "(SURFACE)" -#define OSD_MSG_SURFACE_BAD "(!SURFACE INOP!)" +#define OSD_MSG_SURFACE_BAD "(!SURFACE UNRELIABLE!)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" #define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a83a3541681..5d4b7a5b425 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -132,6 +132,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) else { int16_t climbRate = -50; + // Increase descent rate when throttle stick below mid from min rate of 0.5m/s up to max 2 m/s if (posControl.flags.estAglStatus != EST_TRUSTED) { const int16_t throttleIdle = getThrottleIdleValue(); const int16_t throttleMid = rcLookupThrottleMid(); From 0f061d371d00e783f8a27897ff1e2b49f083ad73 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 22 May 2026 18:31:34 +0100 Subject: [PATCH 7/7] Update navigation_pos_estimator_agl.c --- .../navigation/navigation_pos_estimator_agl.c | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 38788e20a77..3e9021abaf6 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -49,24 +49,15 @@ extern navigationPosEstimator_t posEstimator; void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) { const float surfaceDtUs = currentTimeUs - posEstimator.surface.lastUpdateTime; - float newReliabilityMeasurement = 0; + uint8_t newReliabilityMeasurement = 0; // default to 0 for negative values, out of range or failed hardware bool surfaceMeasurementWithinRange = false; posEstimator.surface.lastUpdateTime = currentTimeUs; - if (newSurfaceAlt >= 0) { - if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { - newReliabilityMeasurement = 1.0f; - surfaceMeasurementWithinRange = true; - posEstimator.surface.alt = newSurfaceAlt; - } - else { - newReliabilityMeasurement = 0.0f; - } - } - else { - // Negative values - out of range or failed hardware - newReliabilityMeasurement = 0.0f; + if (newSurfaceAlt >= 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { + newReliabilityMeasurement = 1; + surfaceMeasurementWithinRange = true; + posEstimator.surface.alt = newSurfaceAlt; } /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */