diff --git a/docs/Settings.md b/docs/Settings.md index 39e3de90d52..6294c957fa6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4872,6 +4872,26 @@ Value under which the OSD axis g force indicators will blink (g) --- +### osd_glide_sample_rate + +Glide slope sampling rate in Hz (1-4 Hz). Higher rates give more responsive glide slope calculations but use more CPU. + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 1 | 4 | + +--- + +### osd_glide_sample_time_frame + +Glide slope sampling time frame in seconds (5-60 seconds). Longer frames provide more stable glide slope estimates. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 5 | 60 | + +--- + ### osd_highlight_djis_missing_font_symbols Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6ef471e3bb0..bd37e16ac13 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3909,6 +3909,20 @@ groups: field: osd_switch_indicators_align_left type: bool default_value: ON + - name: osd_glide_sample_rate + description: "Glide slope sampling rate in Hz (1-4 Hz). Higher rates give more responsive glide slope calculations but use more CPU." + field: glide_sample_rate + type: uint8_t + min: 1 + max: 4 + default_value: 2 + - name: osd_glide_sample_time_frame + description: "Glide slope sampling time frame in seconds (5-60 seconds). Longer frames provide more stable glide slope estimates." + field: glide_sample_time_frame + type: uint8_t + min: 5 + max: 60 + default_value: 10 - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index af0f826c88c..c8cc7769921 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -345,7 +345,9 @@ void beeperUpdate(timeUs_t currentTimeUs) if (!beeperIsOn) { #ifdef USE_DSHOT if (isMotorProtocolDshot() && !areMotorsRunning() && beeperConfig()->dshot_beeper_enabled - && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs()) + && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs() + && currentBeeperEntry->sequence[beeperPos] != 0 // added beeper timeout so dshot does not beep on "off" + && !(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) // added beeper ignore to dshot beacon { lastDshotBeeperCommandTimeUs = currentTimeUs; sendDShotCommand(beeperConfig()->dshot_beeper_tone); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 55bca838f34..853f822e7ec 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -191,6 +191,24 @@ typedef struct statistic_s { int32_t flightStartMWh; } statistic_t; +#define MAX_GLIDE_BUFFER_SIZE 240 // Maximum samples: 4 Hz * 60 seconds + +typedef struct glidePositionSample_s { + uint32_t distance_cm; // Total travel distance + int32_t altitude_cm; // Altitude +} glidePositionSample_t; + + +// Lazy-allocated glide buffer +static glidePositionSample_t *glideBuffer = NULL; +static uint16_t glideBufferAllocatedSize = 0; +static uint16_t glideBufferCurrentSize = 0; + +// Calculated glide ratio (distance per unit altitude descent) +// Available for use by multiple OSD elements +static float currentGlideRatio = 0.0f; +static bool useGlideElement = false; // Whether any glide element is enabled, used to determine whether glide ratio calculation needs to be performed + static statistic_t stats; static timeUs_t resumeRefreshAt = 0; @@ -1308,21 +1326,16 @@ static inline int32_t osdGetAltitudeMsl(void) } uint16_t osdGetRemainingGlideTime(void) { - float value = getEstimatedActualVelocity(Z); - static pt1Filter_t glideTimeFilterState; - const timeMs_t curTimeMs = millis(); - static timeMs_t glideTimeUpdatedMs; - - value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); - glideTimeUpdatedMs = curTimeMs; - - if (value < 0) { - value = osdGetAltitude() / abs((int)value); - } else { - value = 0; + // Use glide ratio if available and valid + uint16_t glideTime = 0; + if (currentGlideRatio > 0.0f) { + int32_t altitude = osdGetAltitude(); + int16_t groundSpeed = gpsSol.groundSpeed; + if (altitude > 0 && groundSpeed > 0) { + glideTime = (uint16_t)((float)altitude * currentGlideRatio / groundSpeed); + } } - - return (uint16_t)roundf(value); + return glideTime; } static bool osdIsHeadingValid(void) @@ -1828,6 +1841,187 @@ static bool osdElementEnabled(uint8_t elementID, bool onlyCurrentLayout) { return elementEnabled; } +// Manage lazy allocation and reallocation of glide buffer +// Returns the current buffer size, or 0 if allocation failed +static uint16_t ensureGlideBufferAllocated(uint16_t requiredSize) +{ + // Clamp to maximum size + if (requiredSize > MAX_GLIDE_BUFFER_SIZE) { + requiredSize = MAX_GLIDE_BUFFER_SIZE; + } + + if (requiredSize == 0) { + // Free buffer if no longer needed + free(glideBuffer); + glideBuffer = NULL; + glideBufferAllocatedSize = 0; + glideBufferCurrentSize = 0; + return 0; + } + + // If already allocated with correct size, return it + if (glideBuffer != NULL && glideBufferAllocatedSize == requiredSize) { + return requiredSize; + } + + // Need to allocate or reallocate + glidePositionSample_t *newBuffer = (glidePositionSample_t *)realloc(glideBuffer, requiredSize * sizeof(glidePositionSample_t)); + + if (newBuffer == NULL) { + return 0; // Allocation failed, keep old buffer + } + + glideBuffer = newBuffer; + glideBufferAllocatedSize = requiredSize; + + // Reset sample tracking when buffer changes size + glideBufferCurrentSize = 0; + + return requiredSize; +} + +static bool isDataValidForGlideRatio(void) { + // Check if we have been ascending for more than 4 seconds, which would indicate that the glide ratio is not valid + static timeMs_t lastDescentTime = 0; + if (getEstimatedActualVelocity(Z) < 0) { // Descending + lastDescentTime = millis(); + } else if (millis() - lastDescentTime > 4000) { // Not descending for more than 4 seconds + return false; + } + + // Check if the throttle is above a certain threshold, which would indicate that we are under power and the glide ratio is not valid + if (getThrottlePercent(true) > 10) { + return false; + } + + return true; +} + + +// Linear regression: calculate glide ratio from position samples +// Returns glide ratio (horizontal distance per 1 unit vertical descent) +// Returns 0 if insufficient data or invalid conditions +static float calculateGlideRatioFromBuffer(const glidePositionSample_t *buffer, uint8_t sampleCount) +{ + // Least-squares linear regression: y = mx + b + // where x = horizontal distance, y = altitude + // We need: sumX, sumY, sumX², sumXY, and n (sample count) + + float sumX = 0.0f; // sum of distances + float sumY = 0.0f; // sum of altitudes + float sumXY = 0.0f; // sum of (distance * altitude) + float sumX2 = 0.0f; // sum of (distance²) + + for (uint8_t i = 0; i < sampleCount; i++) { + float x = (float)buffer[i].distance_cm; + float y = (float)buffer[i].altitude_cm; + + sumX += x; + sumY += y; + sumXY += x * y; + sumX2 += x * x; + } + + // Slope formula: m = (n·Σxy - Σx·Σy) / (n·Σx² - (Σx)²) + float n = (float)sampleCount; + float numerator = n * sumXY - sumX * sumY; + float denominator = n * sumX2 - sumX * sumX; + + // Avoid division by zero or degenerate cases + if (fabsf(denominator) < 1e-6f) { + return 0.0f; // Not enough variation in distance + } + + float slope = numerator / denominator; // altitude_change / distance_change + + // For descent, slope should be negative + if (slope >= 0.0f) { + return 0.0f; // Not descending + } + + // Glide ratio = distance / |altitude_change| = 1 / |slope| + float glideRatio = -1.0f / slope; + + // Sanity check: reasonable glide ratios are 1-100 + if (glideRatio > 0.1f && glideRatio < 100.0f) { + return glideRatio; + } + + return 0.0f; // Out of reasonable range +} + +// Update glide ratio calculation +// Called regularly to maintain glide ratio buffer regardless of OSD element visibility +// This ensures glide ratio is available for all OSD elements that need it +static void updateGlideRatioCalculation(void) { + uint8_t sampleRate = osdConfig()->glide_sample_rate > 0 ? osdConfig()->glide_sample_rate : 1; // Default to 1 sample/sec if misconfigured + uint8_t timeFrame = osdConfig()->glide_sample_time_frame > 0 ? osdConfig()->glide_sample_time_frame : 5; // Default to 5 seconds if misconfigured + const uint16_t requiredBufferSize = sampleRate * timeFrame; + const uint8_t minimumSampleCount = requiredBufferSize / 4; + + static uint16_t previousBufferSize = 0; + uint16_t bufferSize = ensureGlideBufferAllocated(requiredBufferSize); + + if (bufferSize == 0) { + // Allocation failed + currentGlideRatio = 0.0f; + return; + } + + static uint8_t glideBufferIndex = 0; + static timeMs_t glideLastSampleTime = 0; + static uint8_t samplesSinceLastClear = 0; + const timeMs_t currentTime = millis(); + const uint16_t sampleIntervalMs = 1000 / sampleRate; + + // Reset sampling state if buffer size changed + if (bufferSize != previousBufferSize) { + previousBufferSize = bufferSize; + glideBufferIndex = 0; + samplesSinceLastClear = 0; + glideLastSampleTime = 0; // Reset to take sample immediately after resize + } + + if (currentTime - glideLastSampleTime >= sampleIntervalMs) { + // Record a new sample + glideLastSampleTime = currentTime; + if (!isDataValidForGlideRatio()) { + // Conditions not valid for glide ratio, reset buffer + for (uint16_t i = 0; i < bufferSize; i++) { + glideBuffer[i].distance_cm = 0; + glideBuffer[i].altitude_cm = 0; + } + currentGlideRatio = 0.0f; + samplesSinceLastClear = 0; + glideBufferIndex = 0; + } + else { + glideBuffer[glideBufferIndex].distance_cm = getTotalTravelDistance(); + glideBuffer[glideBufferIndex].altitude_cm = osdGetAltitude(); + glideBufferIndex = (glideBufferIndex + 1) % bufferSize; + + if (samplesSinceLastClear < bufferSize) { + samplesSinceLastClear++; + } + + if (samplesSinceLastClear >= minimumSampleCount) { + // Calculate glide ratio using only the valid samples collected + currentGlideRatio = calculateGlideRatioFromBuffer(glideBuffer, samplesSinceLastClear); + } + else { + currentGlideRatio = 0.0f; // Not enough samples yet + } + } + } +} + +static void enableGlideRatioCalculation(void) { + if (!useGlideElement) { + useGlideElement = true; + updateGlideRatioCalculation(); // Start calculation immediately when element is enabled + } +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -2065,18 +2259,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GLIDESLOPE: { - float horizontalSpeed = gpsSol.groundSpeed; - float sinkRate = -getEstimatedActualVelocity(Z); - static pt1Filter_t gsFilterState; - const timeMs_t currentTimeMs = millis(); - static timeMs_t gsUpdatedTimeMs; - float glideSlope = horizontalSpeed / sinkRate; - glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); - gsUpdatedTimeMs = currentTimeMs; - + enableGlideRatioCalculation(); // Ensure glide ratio calculation is running if this element is enabled buff[0] = SYM_GLIDESLOPE; - if (glideSlope > 0.0f && glideSlope < 100.0f) { - osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3, false); + if (currentGlideRatio > 0.0f && currentGlideRatio < 100.0f) { + osdFormatCentiNumber(buff + 1, currentGlideRatio * 100.0f, 0, 2, 0, 3, false); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -3157,6 +3343,7 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_GLIDE_TIME_REMAINING: { + enableGlideRatioCalculation(); uint16_t glideTime = osdGetRemainingGlideTime(); buff[0] = SYM_GLIDE_MINS; if (glideTime > 0) { @@ -3177,14 +3364,18 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_GLIDE_RANGE: { - uint16_t glideSeconds = osdGetRemainingGlideTime(); + enableGlideRatioCalculation(); + int32_t altitude = osdGetAltitude(); buff[0] = SYM_GLIDE_DIST; - if (glideSeconds > 0) { - uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0, 3); - } else { + if (currentGlideRatio <= 0.0f || altitude <= 0) { tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); buff[5] = '\0'; + break; + } + else + { + int32_t glideRangeCm = (int32_t)(currentGlideRatio * altitude); + osdFormatDistanceSymbol(buff + 1, glideRangeCm, 0, 3); } break; } @@ -4412,7 +4603,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT, .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT, - .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT + .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT, + .glide_sample_rate = SETTING_OSD_GLIDE_SAMPLE_RATE_DEFAULT, + .glide_sample_time_frame = SETTING_OSD_GLIDE_SAMPLE_TIME_FRAME_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) @@ -5860,6 +6053,10 @@ static bool osdIsPageDownStickCommandHeld(void) static void osdRefresh(timeUs_t currentTimeUs) { osdFilterData(currentTimeUs); + + if (useGlideElement) { + updateGlideRatioCalculation(); + } #ifdef USE_CMS if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { @@ -6472,6 +6669,7 @@ void osdEraseCustomItem(uint8_t item){ } + #endif // OSD unsigned getCurrentLayout(void){ diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 88240ff84c0..5d60375977c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -540,6 +540,8 @@ typedef struct osdConfig_s { uint8_t geozoneDistanceWarning; // Distance to fence or action bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action #endif + uint8_t glide_sample_rate; // Glide slope sampling rate in Hz (default 2) + uint8_t glide_sample_time_frame; // Glide slope sampling time frame in seconds (default 10) } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig);