tasks and integration

src/main/config/config.c

@@ -382,23 +382,31 @@ static void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig)
 #endif
 
 #ifdef ASYNC_GYRO_PROCESSING
-uint32_t getLooptime(void) {
+uint32_t getPidUpdateRate(void) {
-    return masterConfig.looptime;
+    if (masterConfig.asyncMode == ASYNC_MODE_NONE) {
-}
+        return getGyroUpdateRate();
-
-uint16_t getAccUpdateFrequency(void) {
-    if (masterConfig.asyncMode == ASYNC_MODE_ALL) {
-        return masterConfig.accTaskFrequency;
     } else {
-        return 1000000 / getLooptime();
+        return masterConfig.looptime;
     }
 }
 
-uint16_t getAttiUpdateFrequency(void) {
+uint16_t getGyroUpdateRate(void) {
+    return gyro.targetLooptime;
+}
+
+uint16_t getAccUpdateRate(void) {
     if (masterConfig.asyncMode == ASYNC_MODE_ALL) {
-        return masterConfig.attiTaskFrequency;
+        return 1000000 / masterConfig.accTaskFrequency;
     } else {
-        return 1000000 / getLooptime();
+        return getPidUpdateRate();
+    }
+}
+
+uint16_t getAttiUpdateRate(void) {
+    if (masterConfig.asyncMode == ASYNC_MODE_ALL) {
+        return 1000000 / masterConfig.attiTaskFrequency;
+    } else {
+        return getPidUpdateRate();
     }
 }
 

src/main/config/config.h

@@ -120,8 +120,9 @@ struct master_s;
 void targetConfiguration(struct master_s *config);
 
 #ifdef ASYNC_GYRO_PROCESSING
-uint32_t getLooptime(void);
+uint32_t getPidUpdateRate(void);
-uint16_t getAccUpdateFrequency(void);
+uint16_t getGyroUpdateRate(void);
-uint16_t getAttiUpdateFrequency(void);
+uint16_t getAccUpdateRate(void);
+uint16_t getAttiUpdateRate(void);
 uint8_t getAsyncMode(void);
 #endif

src/main/fc/fc_tasks.c

@@ -214,6 +214,16 @@ void taskSyncPwmDriver(void) {
 }
 #endif
 
+#ifdef ASYNC_GYRO_PROCESSING
+void taskAttitude(void) {
+    imuUpdateAttitude();
+}
+
+void taskAcc(void) {
+    imuUpdateAccelerometer();
+}
+#endif
+
 cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_SYSTEM] = {
         .taskName = "SYSTEM",
@@ -222,12 +232,49 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_HIGH,
     },
 
-    [TASK_GYROPID] = {
+    #ifdef ASYNC_GYRO_PROCESSING
-        .taskName = "GYRO/PID",
+        [TASK_PID] = {
-        .taskFunc = taskMainPidLoopChecker,
+            .taskName = "PID",
-        .desiredPeriod = 1000,
+            .taskFunc = taskMainPidLoop,
-        .staticPriority = TASK_PRIORITY_REALTIME,
+            .desiredPeriod = 1000000 / 500, // Run at 500Hz
-    },
+            .staticPriority = TASK_PRIORITY_HIGH,
+        },
+
+        [TASK_GYRO] = {
+            .taskName = "GYRO",
+            .taskFunc = taskGyro,
+            .desiredPeriod = 1000000 / 1000, //Run at 1000Hz
+            .staticPriority = TASK_PRIORITY_REALTIME,
+        },
+
+        [TASK_ACC] = {
+            .taskName = "ACC",
+            .taskFunc = taskAcc,
+            .desiredPeriod = 1000000 / 520, //520Hz is ACC bandwidth (260Hz) * 2
+            .staticPriority = TASK_PRIORITY_HIGH,
+        },
+
+        [TASK_ATTI] = {
+            .taskName = "ATTITUDE",
+            .taskFunc = taskAttitude,
+            .desiredPeriod = 1000000 / 60, //With acc LPF at 15Hz 60Hz attitude refresh should be enough
+            .staticPriority = TASK_PRIORITY_HIGH,
+        },
+
+    #else
+
+        /*
+         * Legacy synchronous PID/gyro/acc/atti mode
+         * for 64kB targets and other smaller targets
+         */
+
+        [TASK_GYROPID] = {
+            .taskName = "GYRO/PID",
+            .taskFunc = taskMainPidLoop,
+            .desiredPeriod = 1000,
+            .staticPriority = TASK_PRIORITY_REALTIME,
+        },
+    #endif
 
     [TASK_SERIAL] = {
         .taskName = "SERIAL",
@@ -342,10 +389,30 @@ cfTask_t cfTasks[TASK_COUNT] = {
 
 void fcTasksInit(void)
 {
+    /* Setup scheduler */
     schedulerInit();
 
+#ifdef ASYNC_GYRO_PROCESSING
+    rescheduleTask(TASK_PID, getPidUpdateRate());
+    setTaskEnabled(TASK_PID, true);
+
+    if (getAsyncMode() != ASYNC_MODE_NONE) {
+        rescheduleTask(TASK_GYRO, getGyroUpdateRate());
+        setTaskEnabled(TASK_GYRO, true);
+    }
+
+    if (getAsyncMode() == ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
+        rescheduleTask(TASK_ACC, getAccUpdateRate());
+        setTaskEnabled(TASK_ACC, true);
+
+        rescheduleTask(TASK_ATTI, getAttiUpdateRate());
+        setTaskEnabled(TASK_ATTI, true);
+    }
+
+#else
     rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
     setTaskEnabled(TASK_GYROPID, true);
+#endif
 
     setTaskEnabled(TASK_SERIAL, true);
 #ifdef BEEPER

src/main/fc/fc_tasks.h

@@ -19,10 +19,25 @@
 
 #include <stdint.h>
 
-void taskMainPidLoopChecker(void);
+void taskHandleSerial(void);
+void taskUpdateBeeper(void);
+void taskUpdateBattery(void);
 bool taskUpdateRxCheck(uint32_t currentDeltaTime);
 void taskUpdateRxMain(void);
 void taskSystem(void);
+
+#ifdef USE_PMW_SERVO_DRIVER
+void taskSyncPwmDriver(void);
+#endif
+
 void taskStackCheck(void);
 
+void taskMainPidLoop(void);
+void taskGyro(void);
+
+#ifdef ASYNC_GYRO_PROCESSING
+void taskAcc(void);
+void taskAttitude(void);
+#endif
+
 void fcTasksInit(void);

src/main/fc/mw.c

@@ -521,13 +521,44 @@ void filterRc(bool isRXDataNew)
     }
 }
 
+// Function for loop trigger
+void taskGyro(void) {
+    // getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
+    // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
+    uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+
+    if (masterConfig.gyroSync) {
+        while (1) {
+            if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
+                break;
+            }
+        }
+    }
+
+    gyroUpdate();
+}
+
 void taskMainPidLoop(void)
 {
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
+#ifdef ASYNC_GYRO_PROCESSING
+    if (getAsyncMode() == ASYNC_MODE_NONE) {
+        taskGyro();
+    }
+
+    if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
+        imuUpdateAccelerometer();
+        imuUpdateAttitude();
+    }
+#else
+    /* Update gyroscope */
+    taskGyro();
     imuUpdateAccelerometer();
-    imuUpdateGyroAndAttitude();
+    imuUpdateAttitude();
+#endif
+
 
     annexCode();
 
@@ -628,23 +659,6 @@ void taskMainPidLoop(void)
 
 }
 
-// Function for loop trigger
-void taskMainPidLoopChecker(void) {
-    // getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
-    // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
-    uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
-
-    if (masterConfig.gyroSync) {
-        while (1) {
-            if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
-                break;
-            }
-        }
-    }
-
-    taskMainPidLoop();
-}
-
 bool taskUpdateRxCheck(uint32_t currentDeltaTime)
 {
     UNUSED(currentDeltaTime);

src/main/flight/imu.c

@@ -303,8 +303,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     if (accWeight > 0) {
         float kpAcc = imuRuntimeConfig->dcm_kp_acc * imuGetPGainScaleFactor();
 
-        // Just scale by 1G length - That's our vector adjustment. Rather than 
+        // Just scale by 1G length - That's our vector adjustment. Rather than
-        // using one-over-exact length (which needs a costly square root), we already 
+        // using one-over-exact length (which needs a costly square root), we already
         // know the vector is enough "roughly unit length" and since it is only weighted
         // in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap
         ax *= (1.0f / GRAVITY_CMSS);
@@ -506,7 +506,7 @@ void imuUpdateAccelerometer(void)
 #endif
 }
 
-void imuUpdateGyroAndAttitude(void)
+void imuUpdateAttitude(void)
 {
     /* Calculate dT */
     static uint32_t previousIMUUpdateTime;
@@ -514,9 +514,6 @@ void imuUpdateGyroAndAttitude(void)
     float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
     previousIMUUpdateTime = currentTime;
 
-    /* Update gyroscope */
-    gyroUpdate();
-
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
 #ifdef HIL
         if (!hilActive) {

src/main/flight/imu.h

@@ -49,7 +49,7 @@ typedef struct imuRuntimeConfig_s {
 struct pidProfile_s;
 void imuConfigure(imuRuntimeConfig_t *initialImuRuntimeConfig, struct pidProfile_s *initialPidProfile);
 
-void imuUpdateGyroAndAttitude(void);
+void imuUpdateAttitude(void);
 void imuUpdateAccelerometer(void);
 float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength);
 float calculateCosTiltAngle(void);

src/main/main.c

@@ -57,6 +57,7 @@ int main(void)
 {
     init();
     loopbackInit();
+
     while (true) {
         scheduler();
         processLoopback();

src/main/scheduler/scheduler.h

@@ -42,7 +42,14 @@ typedef struct {
 typedef enum {
     /* Actual tasks */
     TASK_SYSTEM = 0,
+#ifdef ASYNC_GYRO_PROCESSING
+    TASK_PID,
+    TASK_GYRO,
+    TASK_ACC,
+    TASK_ATTI,
+#else
     TASK_GYROPID,
+#endif
     TASK_SERIAL,
     TASK_BEEPER,
     TASK_BATTERY,

src/main/scheduler/scheduler_tasks.c

@@ -0,0 +1,187 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "platform.h"
+#include "scheduler.h"
+#include "scheduler_tasks.h"
+
+cfTask_t cfTasks[TASK_COUNT] = {
+    [TASK_SYSTEM] = {
+        .taskName = "SYSTEM",
+        .taskFunc = taskSystem,
+        .desiredPeriod = 1000000 / 10,              // run every 100 ms
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+#ifdef ASYNC_GYRO_PROCESSING
+    [TASK_PID] = {
+        .taskName = "PID",
+        .taskFunc = taskMainPidLoop,
+        .desiredPeriod = 1000000 / 500, // Run at 500Hz
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+    [TASK_GYRO] = {
+        .taskName = "GYRO",
+        .taskFunc = taskGyro,
+        .desiredPeriod = 1000000 / 1000, //Run at 1000Hz
+        .staticPriority = TASK_PRIORITY_REALTIME,
+    },
+
+    [TASK_ACC] = {
+        .taskName = "ACC",
+        .taskFunc = taskAcc,
+        .desiredPeriod = 1000000 / 520, //520Hz is ACC bandwidth (260Hz) * 2
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+    [TASK_ATTI] = {
+        .taskName = "ATTITUDE",
+        .taskFunc = taskAttitude,
+        .desiredPeriod = 1000000 / 60, //With acc LPF at 15Hz 60Hz attitude refresh should be enough
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+#else
+
+    /*
+     * Legacy synchronous PID/gyro/acc/atti mode
+     * for 64kB targets and other smaller targets
+     */
+
+    [TASK_GYROPID] = {
+        .taskName = "GYRO/PID",
+        .taskFunc = taskMainPidLoop,
+        .desiredPeriod = 1000,
+        .staticPriority = TASK_PRIORITY_REALTIME,
+    },
+#endif
+
+    [TASK_SERIAL] = {
+        .taskName = "SERIAL",
+        .taskFunc = taskHandleSerial,
+        .desiredPeriod = 1000000 / 100,     // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+
+    [TASK_BEEPER] = {
+        .taskName = "BEEPER",
+        .taskFunc = taskUpdateBeeper,
+        .desiredPeriod = 1000000 / 100,     // 100 Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+    [TASK_BATTERY] = {
+        .taskName = "BATTERY",
+        .taskFunc = taskUpdateBattery,
+        .desiredPeriod = 1000000 / 50,      // 50 Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+    [TASK_RX] = {
+        .taskName = "RX",
+        .checkFunc = taskUpdateRxCheck,
+        .taskFunc = taskUpdateRxMain,
+        .desiredPeriod = 1000000 / 50,      // If event-based scheduling doesn't work, fallback to periodic scheduling
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+#ifdef GPS
+    [TASK_GPS] = {
+        .taskName = "GPS",
+        .taskFunc = taskProcessGPS,
+        .desiredPeriod = 1000000 / 25,      // GPS usually don't go raster than 10Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
+#ifdef MAG
+    [TASK_COMPASS] = {
+        .taskName = "COMPASS",
+        .taskFunc = taskUpdateCompass,
+        .desiredPeriod = 1000000 / 10,      // Compass is updated at 10 Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
+#ifdef BARO
+    [TASK_BARO] = {
+        .taskName = "BARO",
+        .taskFunc = taskUpdateBaro,
+        .desiredPeriod = 1000000 / 20,
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
+#ifdef SONAR
+    [TASK_SONAR] = {
+        .taskName = "SONAR",
+        .taskFunc = taskUpdateSonar,
+        .desiredPeriod = 70000,                 // every 70 ms, approximately 14 Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
+#ifdef DISPLAY
+    [TASK_DISPLAY] = {
+        .taskName = "DISPLAY",
+        .taskFunc = taskUpdateDisplay,
+        .desiredPeriod = 1000000 / 10,
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef TELEMETRY
+    [TASK_TELEMETRY] = {
+        .taskName = "TELEMETRY",
+        .taskFunc = taskTelemetry,
+        .desiredPeriod = 1000000 / 250,         // 250 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
+#ifdef LED_STRIP
+    [TASK_LEDSTRIP] = {
+        .taskName = "LEDSTRIP",
+        .taskFunc = taskLedStrip,
+        .desiredPeriod = 1000000 / 100,         // 100 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
+#ifdef USE_PMW_SERVO_DRIVER
+    [TASK_PWMDRIVER] = {
+        .taskName = "PWMDRIVER",
+        .taskFunc = taskSyncPwmDriver,
+        .desiredPeriod = 1000000 / 200,         // 200 Hz
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+#endif
+
+#ifdef STACK_CHECK
+    [TASK_STACK_CHECK] = {
+        .taskName = "STACKCHECK",
+        .taskFunc = taskStackCheck,
+        .desiredPeriod = 1000000 / 10,          // 10 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+};