Removed tasks from PID loop that don't belong there.

src/main/fc/fc_core.c

@@ -905,33 +905,19 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
 #endif
 }
 
-static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
+static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
 {
     uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {
         startTime = micros();
     }
 
-    // Read out gyro temperature if used for telemmetry
-    if (feature(FEATURE_TELEMETRY)) {
-        gyroReadTemperature();
-    }
-
 #ifdef USE_MAG
     if (sensors(SENSOR_MAG)) {
         updateMagHold();
     }
 #endif
 
-#ifdef USE_SDCARD
-    afatfs_poll();
-#endif
-
-#if defined(USE_VCP)
-    DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted());
-    DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected());
-#endif
-
 #ifdef USE_BLACKBOX
     if (!cliMode && blackboxConfig()->device) {
         blackboxUpdate(currentTimeUs);
@@ -943,6 +929,25 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
 }
 
+void taskMain(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+#ifdef USE_SDCARD
+    afatfs_poll();
+#endif
+}
+
+#ifdef USE_TELEMETRY
+void subTaskTelemetryPollSensors(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    // Read out gyro temperature if used for telemmetry
+    gyroReadTemperature();
+}
+#endif
+
 static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
 {
     uint32_t startTime = 0;
@@ -1017,7 +1022,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
     // 0 - gyroUpdate()
     // 1 - subTaskPidController()
     // 2 - subTaskMotorUpdate()
-    // 3 - subTaskMainSubprocesses()
+    // 3 - subTaskPidSubprocesses()
     gyroUpdate(currentTimeUs);
     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
 
@@ -1025,7 +1030,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
         subTaskRcCommand(currentTimeUs);
         subTaskPidController(currentTimeUs);
         subTaskMotorUpdate(currentTimeUs);
-        subTaskMainSubprocesses(currentTimeUs);
+        subTaskPidSubprocesses(currentTimeUs);
     }
 
     if (debugMode == DEBUG_CYCLETIME) {

src/main/fc/fc_core.h

@@ -56,3 +56,6 @@ bool isAirmodeActivated();
 timeUs_t getLastDisarmTimeUs(void);
 bool isTryingToArm();
 void resetTryingToArm();
+
+void taskMain(timeUs_t currentTimeUs);
+void subTaskTelemetryPollSensors(timeUs_t currentTimeUs);

src/main/fc/fc_tasks.c

@@ -186,6 +186,8 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
 static void taskTelemetry(timeUs_t currentTimeUs)
 {
     if (!cliMode && feature(FEATURE_TELEMETRY)) {
+        subTaskTelemetryPollSensors(currentTimeUs);
+
         telemetryProcess(currentTimeUs);
     }
 }
@@ -205,6 +207,9 @@ void taskCameraControl(uint32_t currentTime)
 void fcTasksInit(void)
 {
     schedulerInit();
+
+    setTaskEnabled(TASK_SYSTEM, true);
+
     setTaskEnabled(TASK_SERIAL, true);
     rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
 
@@ -318,10 +323,19 @@ void fcTasksInit(void)
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
+    [TASK_SYSTEM_LOAD] = {
+        .taskName = "SYSTEM",
+        .subTaskName = "LOAD",
+        .taskFunc = taskSystemLoad,
+        .desiredPeriod = TASK_PERIOD_HZ(10),        // 10Hz, every 100 ms
+        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
+    },
+
     [TASK_SYSTEM] = {
         .taskName = "SYSTEM",
-        .taskFunc = taskSystem,
+        .subTaskName = "UPDATE",
-        .desiredPeriod = TASK_PERIOD_HZ(10),        // 10Hz, every 100 ms
+        .taskFunc = taskMain,
+        .desiredPeriod = TASK_PERIOD_HZ(100),
         .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
     },
 

src/main/scheduler/scheduler.c

@@ -123,7 +123,7 @@ FAST_CODE cfTask_t *queueNext(void)
     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 }
 
-void taskSystem(timeUs_t currentTimeUs)
+void taskSystemLoad(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
@@ -225,7 +225,7 @@ void schedulerInit(void)
 {
     calculateTaskStatistics = true;
     queueClear();
-    queueAdd(&cfTasks[TASK_SYSTEM]);
+    queueAdd(&cfTasks[TASK_SYSTEM_LOAD]);
 }
 
 FAST_CODE void scheduler(void)

src/main/scheduler/scheduler.h

@@ -58,6 +58,7 @@ typedef struct {
 typedef enum {
     /* Actual tasks */
     TASK_SYSTEM = 0,
+    TASK_SYSTEM_LOAD,
     TASK_GYROPID,
     TASK_ACCEL,
     TASK_ATTITUDE,
@@ -179,7 +180,7 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId);
 
 void schedulerInit(void);
 void scheduler(void);
-void taskSystem(timeUs_t currentTime);
+void taskSystemLoad(timeUs_t currentTime);
 
 #define LOAD_PERCENTAGE_ONE 100