Skip to content

Needs testing: Issue #10830 Surface Mode Fixes #11425

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
breadoven merged 7 commits into from
May 23, 2026
+105 −87
Merged

Needs testing: Issue #10830 Surface Mode Fixes #11425

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2f20927
Issue #10830 fixes
breadoven Mar 13, 2026
394395e
Update navigation_multicopter.c
breadoven May 16, 2026
eee931d
fix descent below 10cm
breadoven May 17, 2026
d53f92c
Potential fix for pull request finding
sensei-hacker May 18, 2026
3533d16
correct typo
breadoven May 18, 2026
ffcb7c8
fixes
breadoven May 21, 2026
0f061d3
Update navigation_pos_estimator_agl.c
breadoven May 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
140 changes: 74 additions & 66 deletions src/main/io/osd.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Comment thread
breadoven marked this conversation as resolved.
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()) {
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
}
}
}
}
Expand Down
2 changes: 2 additions & 0 deletions src/main/io/osd.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 UNRELIABLE!)"
#define OSD_MSG_AUTOTRIM "(AUTOTRIM)"
#define OSD_MSG_AUTOTUNE "(AUTOTUNE)"
#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO"
Expand Down
31 changes: 24 additions & 7 deletions src/main/navigation/navigation_multicopter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,15 +122,24 @@ 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 {
updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
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();
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
Expand Down Expand Up @@ -837,10 +846,18 @@ 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 throttle low.
* 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());

static timeMs_t landingDetectorStartedAt;

Expand Down
19 changes: 5 additions & 14 deletions src/main/navigation/navigation_pos_estimator_agl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
Loading