diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 004834ddbc..1e8437a256 100644 --- a/src/main/fc/fc_core.c +++ b/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) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 2da50dc574..6dcd3a3c16 100644 --- a/src/main/fc/fc_core.h +++ b/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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index cf52d8bf3e..8c39afa0e0 100644 --- a/src/main/fc/fc_tasks.c +++ b/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, - .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms + .subTaskName = "UPDATE", + .taskFunc = taskMain, + .desiredPeriod = TASK_PERIOD_HZ(100), .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, }, diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index d5ea17b74e..73d77b4614 100644 --- a/src/main/scheduler/scheduler.c +++ b/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) diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index e55cfae51a..4a41e623e9 100644 --- a/src/main/scheduler/scheduler.h +++ b/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