Improve OSD Glide Slope implementation

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.

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

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);

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,169 @@ 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 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;
+    uint8_t timeFrame = osdConfig()->glide_sample_time_frame;
+    const uint16_t requiredBufferSize = sampleRate * timeFrame;
+    const uint8_t minimumSampleCount = requiredBufferSize / 4;
+
+    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;
+
+    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 +2241,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 +3325,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 +3346,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 {
-                tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK);
+            if (currentGlideRatio <= 0.0f || altitude <= 0) {
+                tfp_sprintf(buff, "%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 +4585,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 +6035,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 +6651,7 @@ void osdEraseCustomItem(uint8_t item){
 
 }
 
+
 #endif // OSD
 
 unsigned getCurrentLayout(void){

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);