From ee6cd98c782ab738f574f975057ff5f9a2e98620 Mon Sep 17 00:00:00 2001
From: Hans Christian Olaussen <41271048+klutvott123@users.noreply.github.com>
Date: Sat, 2 Apr 2022 14:24:17 +0200
Subject: [PATCH] Ensure telemetry/heartbeat at minimum 50Hz

Ensure that telemetry/heartbeat frames are sent at minimum 50Hz. This is to keep sending at least something at a consistent rate to the rx even if the user disables sensors.
---
 src/main/telemetry/crsf.c | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index a6d4c864a4..2fbb3a2421 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -89,6 +89,9 @@ typedef struct mspBuffer_s {
 static mspBuffer_t mspRxBuffer;
 
 #if defined(USE_CRSF_V3)
+
+#define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms
+
 static bool isCrsfV3Running = false;
 typedef struct {
     uint8_t hasPendingReply:1;
@@ -581,6 +584,7 @@ typedef enum {
     CRSF_FRAME_BATTERY_SENSOR_INDEX,
     CRSF_FRAME_FLIGHT_MODE_INDEX,
     CRSF_FRAME_GPS_INDEX,
+    CRSF_FRAME_HEARTBEAT_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -650,6 +654,15 @@ static void processCrsf(void)
         crsfFinalize(dst);
     }
 #endif
+
+#if defined(USE_CRSF_V3)
+    if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfFrameHeartbeat(dst);
+        crsfFinalize(dst);
+    }
+#endif
+
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
 
@@ -690,6 +703,14 @@ void initCrsfTelemetry(void)
         crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
     }
 #endif
+
+#if defined(USE_CRSF_V3)
+    while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) {
+        // schedule heartbeat to ensure that telemetry/heartbeat frames are sent at minimum 50Hz
+        crsfSchedule[index++] = BIT(CRSF_FRAME_HEARTBEAT_INDEX);
+    }
+#endif
+
     crsfScheduleCount = (uint8_t)index;
 
 #if defined(USE_CRSF_CMS_TELEMETRY)