Add task to send telemtery info

iNavFlight · Jun 25, 2024 · 1a27433 · 1a27433
1 parent a3a129e
commit 1a27433
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 3 deletions.
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
@@ -437,6 +437,10 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_HEADTRACKER, true);
 #endif
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+    setTaskEnabled(TASK_TELEMETRY_SBUS2, rxConfig->serialrx_provider == SERIALRX_SBUS2);
+#endif
+
 #ifdef USE_ADAPTIVE_FILTER
     setTaskEnabled(TASK_ADAPTIVE_FILTER, (
         gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE && 
@@ -726,4 +730,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+    [TASK_TELEMETRY_SBUS2] = {
+        .taskName = "SBUS2_TELEMETRY",
+        .taskFunc = taskSendSbus2Telemetry,
+        .desiredPeriod = TASK_PERIOD_US(300),
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 };
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
@@ -133,6 +133,10 @@ typedef enum {
     TASK_HEADTRACKER,
 #endif
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+    TASK_TELEMETRY_SBUS2,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

diff --git a/src/main/telemetry/sbus2.c b/src/main/telemetry/sbus2.c
@@ -41,10 +41,16 @@ sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY
 uint8_t sbusTelemetryDataStatus[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS] = {{}};
 
 void handleSbus2Telemetry(timeUs_t currentTimeUs) 
+{
+    UNUSED(currentTimeUs);
+
+    // update telemetry info
+}
+
+void taskSendSbus2Telemetry(timeUs_t currentTimeUs)
 {
     uint8_t telemetryPage = sbusGetCurrentTelemetryPage();
     uint8_t lastFrame = sbusGetLastFrameTime();
-
     timeUs_t elapsedTime = currentTimeUs - lastFrame - MS2US(2);
 
     // 2ms after sbus2 frame = slot 0
@@ -61,4 +67,4 @@ void handleSbus2Telemetry(timeUs_t currentTimeUs)
     }
 }
 
-#endif
+#endif
diff --git a/src/main/telemetry/sbus2.h b/src/main/telemetry/sbus2.h
@@ -108,5 +108,9 @@ extern const uint8_t Slot_ID[SBUS2_SLOT_COUNT];
 extern sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS];
 extern uint8_t sbusTelemetryDataStatus[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS];
 
+// refresh telemetry buffers 
 void handleSbus2Telemetry(timeUs_t currentTimeUs);
-#endif
+
+// time critical, send sbus2 data
+void taskSendSbus2Telemetry(timeUs_t currentTimeUs);
+#endif