Improved scheduling. Betaflight Port digitalentity/cf-scheduler

Disallow arming if system load > 100 (waiting task count > 1)

Dont show inactive tasks in CLI

Realtime priority task and guard interval implementation

Dynamic guard interval. Bugfix for realtime scheduling hickups

Optimisations

Compile out CLI command help and CLI tasks command for CJMCU

Naming fixes // re-Add Gyro Sync // Fix port issues

Makefile

@@ -268,6 +268,7 @@ COMMON_SRC = build_config.c \
 		   common/typeconversion.c \
 		   common/encoding.c \
 		   common/filter.c \
+		   scheduler.c \
 		   main.c \
 		   mw.c \
 		   flight/altitudehold.c \

src/main/flight/altitudehold.h

@@ -24,6 +24,8 @@
 extern int32_t AltHold;
 extern int32_t vario;
 
+void calculateEstimatedAltitude(uint32_t currentTime);
+
 void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
 void applyAltHold(airplaneConfig_t *airplaneConfig);
 void updateAltHoldState(void);

src/main/flight/imu.c

@@ -20,15 +20,15 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <math.h>
-#include <string.h>
 
 #include "common/maths.h"
-#include "common/filter.h"
 
+#include "build_config.h"
 #include "platform.h"
 #include "debug.h"
 
 #include "common/axis.h"
+#include "common/filter.h"
 
 #include "drivers/system.h"
 #include "drivers/sensor.h"
@@ -62,30 +62,30 @@ int32_t accSum[XYZ_AXIS_COUNT];
 uint32_t accTimeSum = 0;        // keep track for integration of acc
 int accSumCount = 0;
 float accVelScale;
-float fc_acc;
 
 float throttleAngleScale;
+float fc_acc;
 float smallAngleCosZ = 0;
 
 float magneticDeclination = 0.0f;       // calculated at startup from config
+static bool isAccelUpdatedAtLeastOnce = false;
 
 static imuRuntimeConfig_t *imuRuntimeConfig;
 static pidProfile_t *pidProfile;
 static accDeadband_t *accDeadband;
 
-
+STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
-static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
 static float rMat[3][3];
 
 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 
 static float gyroScale;
 
-static void imuCompureRotationMatrix(void)
+STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 {
-    float q1q1 = q1 * q1;
+    float q1q1 = sq(q1);
-    float q2q2 = q2 * q2;
+    float q2q2 = sq(q2);
-    float q3q3 = q3 * q3;
+    float q3q3 = sq(q3);
     
     float q0q1 = q0 * q1;
     float q0q2 = q0 * q2;
@@ -128,7 +128,7 @@ void imuInit(void)
     gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
     accVelScale = 9.80665f / acc_1G / 10000.0f;
 
-    imuCompureRotationMatrix();
+    imuComputeRotationMatrix();
 }
 
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
@@ -248,7 +248,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Use measured magnetic field vector
-    recipNorm = mx * mx + my * my + mz * mz;
+    recipNorm = sq(mx) + sq(my) + sq(mz);
     if (useMag && recipNorm > 0.01f) {
         // Normalise magnetometer measurement
         recipNorm = invSqrt(recipNorm);
@@ -275,7 +275,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Use measured acceleration vector
-    recipNorm = ax * ax + ay * ay + az * az;
+    recipNorm = sq(ax) + sq(ay) + sq(az);
     if (useAcc && recipNorm > 0.01f) {
         // Normalise accelerometer measurement
         recipNorm = invSqrt(recipNorm);
@@ -327,17 +327,17 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     q3 += (qa * gz + qb * gy - qc * gx);
 
     // Normalise quaternion
-    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
     q0 *= recipNorm;
     q1 *= recipNorm;
     q2 *= recipNorm;
     q3 *= recipNorm;
 
     // Pre-compute rotation matrix from quaternion
-    imuCompureRotationMatrix();
+    imuComputeRotationMatrix();
 }
 
-static void imuUpdateEulerAngles(void)
+STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 {
     /* Compute pitch/roll angles */
     attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
@@ -364,7 +364,7 @@ static bool imuIsAccelerometerHealthy(void)
         accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
     }
 
-    accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G);
+    accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G));
 
     // Accept accel readings only in range 0.90g - 1.10g
     return (81 < accMagnitude) && (accMagnitude < 121);
@@ -389,16 +389,15 @@ static void imuCalculateEstimatedAttitude(void)
     uint32_t deltaT = currentTime - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTime;
 
-    // If reading is considered valid - apply filter
+    // Smooth and use only valid accelerometer readings
     for (axis = 0; axis < 3; axis++) {
         if (imuRuntimeConfig->acc_cut_hz > 0) {
-            accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6);
+            accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
         } else {
             accSmooth[axis] = accADC[axis];
         }
     }
 
-    // Smooth and use only valid accelerometer readings
     if (imuIsAccelerometerHealthy()) {
         useAcc = true;
     }
@@ -409,7 +408,7 @@ static void imuCalculateEstimatedAttitude(void)
 #if defined(GPS)
     else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
         // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
-        rawYawError = DECIDEGREES_TO_RADIANS(GPS_ground_course - attitude.values.yaw);
+        rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
         useYaw = true;
     }
 #endif
@@ -425,15 +424,19 @@ static void imuCalculateEstimatedAttitude(void)
     imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
 }
 
-void imuUpdateGyro(void)
+void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
-{
-    gyroUpdate();
-}
-
-void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims)
 {
     if (sensors(SENSOR_ACC)) {
         updateAccelerationReadings(accelerometerTrims);
+        isAccelUpdatedAtLeastOnce = true;
+    }
+}
+
+void imuUpdateGyroAndAttitude(void)
+{
+    gyroUpdate();
+
+    if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
         imuCalculateEstimatedAttitude();
     } else {
         accADC[X] = 0;
@@ -442,6 +445,11 @@ void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims)
     }
 }
 
+float getCosTiltAngle(void)
+{
+    return rMat[2][2];
+}
+
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 {
     /*

src/main/flight/imu.h

@@ -79,6 +79,8 @@ void imuConfigure(
 );
 
 void calculateEstimatedAltitude(uint32_t currentTime);
+void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
+void imuUpdateGyroAndAttitude(void);
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);

src/main/io/serial_cli.c

@@ -24,6 +24,7 @@
 #include <ctype.h>
 
 #include "platform.h"
+#include "scheduler.h"
 #include "version.h"
 
 #include "build_config.h"
@@ -120,6 +121,9 @@ static void cliServoMix(char *cmdline);
 static void cliSet(char *cmdline);
 static void cliGet(char *cmdline);
 static void cliStatus(char *cmdline);
+#ifndef SKIP_TASK_STATISTICS
+static void cliTasks(char *cmdline);
+#endif
 static void cliVersion(char *cmdline);
 static void cliRxRange(char *cmdline);
 
@@ -286,6 +290,9 @@ const clicmd_t cmdTable[] = {
         "\treverse <servo> <source> r|n", cliServoMix),
 #endif
     CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
+#ifndef SKIP_TASK_STATISTICS
+    CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
+#endif
     CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
 };
 #define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
@@ -2267,9 +2274,8 @@ static void cliStatus(char *cmdline)
 {
     UNUSED(cmdline);
 
-    printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s)\r\n",
+    printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s), System load: %d.%02d\r\n",
-        millis() / 1000, vbat, batteryCellCount, getBatteryStateString());
+            millis() / 1000, vbat, batteryCellCount, getBatteryStateString(), averageWaitingTasks100 / 100, averageWaitingTasks100 % 100);
-
 
     printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
 
@@ -2308,6 +2314,24 @@ static void cliStatus(char *cmdline)
     printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
 }
 
+#ifndef SKIP_TASK_STATISTICS
+static void cliTasks(char *cmdline)
+{
+    UNUSED(cmdline);
+
+    cfTaskId_e taskId;
+    cfTaskInfo_t taskInfo;
+
+    printf("Task list:\r\n");
+    for (taskId = 0; taskId < TASK_COUNT; taskId++) {
+        getTaskInfo(taskId, &taskInfo);
+        if (taskInfo.isEnabled) {
+            printf("%d - %s, max = %d us, avg = %d us, total = %d ms\r\n", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskInfo.totalExecutionTime / 1000);
+        }
+    }
+}
+#endif
+
 static void cliVersion(char *cmdline)
 {
     UNUSED(cmdline);

src/main/main.c

@@ -21,6 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
+#include "scheduler.h"
 
 #include "common/axis.h"
 #include "common/color.h"
@@ -49,6 +50,7 @@
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
 #include "drivers/sonar_hcsr04.h"
+#include "drivers/gyro_sync.h"
 
 #include "rx/rx.h"
 
@@ -93,7 +95,6 @@
 #include "build_config.h"
 #include "debug.h"
 
-extern uint32_t previousTime;
 extern uint8_t motorControlEnable;
 
 #ifdef SOFTSERIAL_LOOPBACK
@@ -120,7 +121,6 @@ void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
 void imuInit(void);
 void displayInit(rxConfig_t *intialRxConfig);
 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
-void loop(void);
 void spektrumBind(rxConfig_t *rxConfig);
 const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
 void sonarInit(const sonarHardware_t *sonarHardware);
@@ -171,6 +171,7 @@ void init(void)
     // Configure the Flash Latency cycles and enable prefetch buffer
     SetSysClock(masterConfig.emf_avoidance);
 #endif
+    //i2cSetOverclock(masterConfig.i2c_overclock);
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
     detectHardwareRevision();
@@ -262,7 +263,7 @@ void init(void)
     pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
     if (feature(FEATURE_3D))
         pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
-    if (pwm_params.motorPwmRate > 500 && !masterConfig.use_fast_pwm)
+    pwm_params.useFastPWM = masterConfig.use_fast_pwm ? true : false;
         pwm_params.idlePulse = 0; // brushed motors
 #ifdef CC3D
     pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
@@ -291,11 +292,11 @@ void init(void)
         .isInverted = false
 #endif
     };
-#ifdef NAZE
 #ifdef AFROMINI
     beeperConfig.gpioMode = Mode_Out_PP;   // AFROMINI override
     beeperConfig.isInverted = true;
 #endif
+#ifdef NAZE
     if (hardwareRevision >= NAZE32_REV5) {
         // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
         beeperConfig.gpioMode = Mode_Out_PP;
@@ -315,6 +316,7 @@ void init(void)
 #endif
 
 
+
 #ifdef USE_SPI
     spiInit(SPI1);
     spiInit(SPI2);
@@ -384,7 +386,7 @@ void init(void)
     }
 #endif
 
-    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) {
+    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }
@@ -470,8 +472,6 @@ void init(void)
     initBlackbox();
 #endif
 
-    previousTime = micros();
-
     if (masterConfig.mixerMode == MIXER_GIMBAL) {
         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
     }
@@ -540,8 +540,42 @@ void processLoopback(void) {
 int main(void) {
     init();
 
+    /* Setup scheduler */
+    rescheduleTask(TASK_GYROPID, targetLooptime);
+
+    setTaskEnabled(TASK_GYROPID, true);
+    setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));
+    setTaskEnabled(TASK_SERIAL, true);
+    setTaskEnabled(TASK_BEEPER, true);
+    setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
+    setTaskEnabled(TASK_RX, true);
+#ifdef GPS
+    setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
+#endif
+#ifdef MAG
+    setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
+#endif
+#ifdef BARO
+    setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
+#endif
+#ifdef SONAR
+    setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
+#endif
+#if defined(BARO) || defined(SONAR)
+    setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
+#endif
+#ifdef DISPLAY
+    setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
+#endif
+#ifdef TELEMETRY
+    setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
+#endif
+#ifdef LED_STRIP
+    setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
+#endif
+
     while (1) {
-        loop();
+        scheduler();
         processLoopback();
     }
 }

src/main/mw.c

@@ -21,12 +21,14 @@
 #include <math.h>
 
 #include "platform.h"
+#include "scheduler.h"
+#include "debug.h"
 
 #include "common/maths.h"
 #include "common/axis.h"
 #include "common/color.h"
-#include "common/filter.h"
 #include "common/utils.h"
+#include "common/filter.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
@@ -36,7 +38,6 @@
 #include "drivers/gpio.h"
 #include "drivers/system.h"
 #include "drivers/serial.h"
-#include "drivers/bus_bst.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/gyro_sync.h"
@@ -59,7 +60,6 @@
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/serial.h"
-#include "io/i2c_bst.h"
 #include "io/serial_cli.h"
 #include "io/serial_msp.h"
 #include "io/statusindicator.h"
@@ -91,23 +91,22 @@ enum {
     ALIGN_MAG = 2
 };
 
+//#define JITTER_DEBUG 0  // Specify debug value for jitter debug
+
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)
-#define GYRO_WATCHDOG_DELAY 100  // Watchdog for boards without interrupt for gyro
+
-#define PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER 80 // Prevent RX processing before expected loop trigger
+#define GYRO_WATCHDOG_DELAY 100 // Watchdog delay for gyro sync
-#define PREVENT_BARO_READ_PRE_LOOP_TRIGGER 150 // Prevent BARO processing before expected loop trigger
-#define GYRO_RATE 0.001f  // Gyro refresh rate 1khz
 
 // AIR MODE Reset timers
 #define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000)   // 1 sec delay to disable AIR MODE Iterm resetting
 bool allowITermShrinkOnly = false;
 
-uint32_t currentTime = 0;
-uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
-float dT = GYRO_RATE;  // dT set for gyro refresh rate
+
+float dT;
 
 int16_t magHold;
 int16_t headFreeModeHold;
@@ -117,9 +116,12 @@ uint8_t motorControlEnable = false;
 int16_t telemTemperature1;      // gyro sensor temperature
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
+extern uint32_t currentTime;
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
 static bool isRXDataNew;
+static filterStatePt1_t filteredCycleTimeState;
+uint16_t filteredCycleTime;
 
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
@@ -152,7 +154,7 @@ void updateGtuneState(void)
         }
     } else {
         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
-    	    DISABLE_FLIGHT_MODE(GTUNE_MODE);
+            DISABLE_FLIGHT_MODE(GTUNE_MODE);
         }
     }
 }
@@ -175,18 +177,16 @@ void filterRc(void){
     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
     static int16_t factor, rcInterpolationFactor;
-    static filterStatePt1_t filteredCycleTimeState;
+    uint16_t rxRefreshRate;
-    uint16_t rxRefreshRate, filteredCycleTime;
 
     // Set RC refresh rate for sampling and channels to filter
-   	initRxRefreshRate(&rxRefreshRate);
+    initRxRefreshRate(&rxRefreshRate);
 
-   	filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 
     if (isRXDataNew) {
         for (int channel=0; channel < 4; channel++) {
-        	deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+            deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
             lastCommand[channel] = rcCommand[channel];
         }
 
@@ -211,8 +211,6 @@ void annexCode(void)
     int32_t tmp, tmp2;
     int32_t axis, prop1 = 0, prop2;
 
-    static uint32_t vbatLastServiced = 0;
-    static uint32_t ibatLastServiced = 0;
     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
         prop2 = 100;
@@ -283,27 +281,9 @@ void annexCode(void)
     }
 
     if (masterConfig.rxConfig.rcSmoothing) {
-        filterRc();  // rcCommand smoothing function
+        filterRc();
     }
 
-    if (feature(FEATURE_VBAT)) {
-        if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
-            vbatLastServiced = currentTime;
-            updateBattery();
-        }
-    }
-
-    if (feature(FEATURE_CURRENT_METER)) {
-        int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
-
-        if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
-            ibatLastServiced = currentTime;
-            updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
-        }
-    }
-
-    beeperUpdate();          //call periodic beeper handler
-
     if (ARMING_FLAG(ARMED)) {
         LED0_ON;
     } else {
@@ -315,7 +295,7 @@ void annexCode(void)
             DISABLE_ARMING_FLAG(OK_TO_ARM);
         }
 
-        if (isCalibrating()) {
+        if (isCalibrating() || (averageWaitingTasks100 > 100)) {
             warningLedFlash();
             DISABLE_ARMING_FLAG(OK_TO_ARM);
         } else {
@@ -329,18 +309,6 @@ void annexCode(void)
         warningLedUpdate();
     }
 
-#ifdef TELEMETRY
-    telemetryCheckState();
-#endif
-
-    handleSerial();
-
-#ifdef GPS
-    if (sensors(SENSOR_GPS)) {
-        updateGpsIndicator(currentTime);
-    }
-#endif
-
     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
     if (gyro.temperature)
         gyro.temperature(&telemTemperature1);
@@ -443,7 +411,7 @@ void updateInflightCalibrationState(void)
         InflightcalibratingA = 50;
         AccInflightCalibrationArmed = false;
     }
-    if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
+    if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
             InflightcalibratingA = 50;
         AccInflightCalibrationActive = true;
@@ -455,7 +423,7 @@ void updateInflightCalibrationState(void)
 
 void updateMagHold(void)
 {
-    if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
+    if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
         int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
         if (dif <= -180)
             dif += 360;
@@ -468,81 +436,6 @@ void updateMagHold(void)
         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 }
 
-typedef enum {
-#ifdef MAG
-    UPDATE_COMPASS_TASK,
-#endif
-#ifdef BARO
-    UPDATE_BARO_TASK,
-#endif
-#ifdef SONAR
-    UPDATE_SONAR_TASK,
-#endif
-#if defined(BARO) || defined(SONAR)
-    CALCULATE_ALTITUDE_TASK,
-#endif
-    UPDATE_DISPLAY_TASK
-} periodicTasks;
-
-#define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
-
-
-void executePeriodicTasks(bool skipBaroUpdate)
-{
-    static int periodicTaskIndex = 0;
-
-    switch (periodicTaskIndex++) {
-#ifdef MAG
-    case UPDATE_COMPASS_TASK:
-        if (sensors(SENSOR_MAG)) {
-            updateCompass(&masterConfig.magZero);
-        }
-        break;
-#endif
-
-#ifdef BARO
-    case UPDATE_BARO_TASK:
-        if (sensors(SENSOR_BARO) && !(skipBaroUpdate)) {
-            baroUpdate(currentTime);
-        }
-        break;
-#endif
-
-#if defined(BARO) || defined(SONAR)
-    case CALCULATE_ALTITUDE_TASK:
-        if (false
-#if defined(BARO)
-            || (sensors(SENSOR_BARO) && isBaroReady())
-#endif
-#if defined(SONAR)
-            || sensors(SENSOR_SONAR)
-#endif
-            ) {
-            calculateEstimatedAltitude(currentTime);
-        }
-        break;
-#endif
-#ifdef SONAR
-    case UPDATE_SONAR_TASK:
-        if (sensors(SENSOR_SONAR)) {
-            sonarUpdate();
-        }
-        break;
-#endif
-#ifdef DISPLAY
-    case UPDATE_DISPLAY_TASK:
-        if (feature(FEATURE_DISPLAY)) {
-            updateDisplay();
-        }
-        break;
-#endif
-    }
-
-    if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {
-        periodicTaskIndex = 0;
-    }
-}
-
 void processRx(void)
 {
     static bool armedBeeperOn = false;
@@ -664,7 +557,7 @@ void processRx(void)
 
     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
         // bumpless transfer to Level mode
-    	canUseHorizonMode = false;
+        canUseHorizonMode = false;
 
         if (!FLIGHT_MODE(ANGLE_MODE)) {
             ENABLE_FLIGHT_MODE(ANGLE_MODE);
@@ -745,94 +638,47 @@ void processRx(void)
 
 }
 
-void loop(void)
-{
-    static uint32_t loopTime;
-    static bool haveProcessedRxOnceBeforeLoop = false;
-    bool skipBaroUpdate = false;
-
 #if defined(BARO) || defined(SONAR)
-    static bool haveProcessedAnnexCodeOnce = false;
+static bool haveProcessedAnnexCodeOnce = false;
 #endif
 
-    updateRx(currentTime);
+// Function for loop trigger
+bool taskMainPidLoopCheck(uint32_t currentDeltaTime) {
+	bool loopTrigger = false;
 
-   if (shouldProcessRx(currentTime) && (!((int32_t)(currentTime - (loopTime - PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER)) >= 0) || (haveProcessedRxOnceBeforeLoop))) {
+    if (gyroSyncCheckUpdate() || (currentDeltaTime >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
-        processRx();
+            loopTrigger = true;
-        isRXDataNew = true;
-        haveProcessedRxOnceBeforeLoop = true;
-
-#ifdef BARO
-        // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
-        if (haveProcessedAnnexCodeOnce) {
-            if (sensors(SENSOR_BARO)) {
-                updateAltHoldState();
-            }
-        }
-#endif
-
-#ifdef SONAR
-        // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
-        if (haveProcessedAnnexCodeOnce) {
-            if (sensors(SENSOR_SONAR)) {
-                updateSonarAltHoldState();
-            }
-        }
-#endif
-
-    } else {
-        if ((int32_t)(currentTime - (loopTime - PREVENT_BARO_READ_PRE_LOOP_TRIGGER)) >= 0) {
-            skipBaroUpdate = true;
-        }
-
-        // not processing rx this iteration
-        executePeriodicTasks(skipBaroUpdate);
-
-        // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
-        // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
-        // change this based on available hardware
-#ifdef GPS
-        if (feature(FEATURE_GPS)) {
-            gpsThread();
-        }
-#endif
     }
 
-    currentTime = micros();
+    return loopTrigger;
-    if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
+}
 
-        loopTime = currentTime + targetLooptime;
+void taskMainPidLoop(void)
+{
+    cycleTime = getTaskDeltaTime(TASK_SELF);
+    dT = (float)cycleTime * 0.000001f;
 
-        haveProcessedRxOnceBeforeLoop = false;
+    // Calculate average cycle time and average jitter
+    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
 
-        imuUpdateGyro();
+#if defined JITTER_DEBUG
+    debug[JITTER_DEBUG] = cycleTime - filteredCycleTime;
+#endif
 
-        // Determine current flight mode. When no acc needed in pid calculations we should only read gyro to reduce latency
+    imuUpdateGyroAndAttitude();
-        if (flightModeFlags) {
-            imuUpdateAcc(&currentProfile->accelerometerTrims);  // When level modes active read gyro and acc
-        }
 
-        // Measure loop rate just after reading the sensors
+    annexCode();
-        currentTime = micros();
-        cycleTime = (int32_t)(currentTime - previousTime);
-        previousTime = currentTime;
-
-        annexCode();
 
 #if defined(BARO) || defined(SONAR)
-        haveProcessedAnnexCodeOnce = true;
+    haveProcessedAnnexCodeOnce = true;
 #endif
 
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {
-        	updateMagHold();
+            updateMagHold();
         }
 #endif
 
-#ifdef GTUNE
-        updateGtuneState();
-#endif
-
 #if defined(BARO) || defined(SONAR)
         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
@@ -841,80 +687,212 @@ void loop(void)
         }
 #endif
 
-        // If we're armed, at minimum throttle, and we do arming via the
+    // If we're armed, at minimum throttle, and we do arming via the
-        // sticks, do not process yaw input from the rx.  We do this so the
+    // sticks, do not process yaw input from the rx.  We do this so the
-        // motors do not spin up while we are trying to arm or disarm.
+    // motors do not spin up while we are trying to arm or disarm.
-        // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
+    // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
-        if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
+    if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
 #ifndef USE_QUAD_MIXER_ONLY
+#ifdef USE_SERVOS
                 && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
+#endif
                 && masterConfig.mixerMode != MIXER_AIRPLANE
                 && masterConfig.mixerMode != MIXER_FLYING_WING
 #endif
-        ) {
+    ) {
-            rcCommand[YAW] = 0;
+        rcCommand[YAW] = 0;
-        }
+    }
 
-
+    if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-        if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+        rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
-            rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
+    }
-        }
 
 #ifdef GPS
-        if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS)) {
-            if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
+        if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
-                updateGpsStateForHomeAndHoldMode();
+            updateGpsStateForHomeAndHoldMode();
-            }
         }
+    }
 #endif
 
-        // PID - note this is function pointer set by setPIDController()
+    // PID - note this is function pointer set by setPIDController()
-        pid_controller(
+    pid_controller(
-            &currentProfile->pidProfile,
+        &currentProfile->pidProfile,
-            currentControlRateProfile,
+        currentControlRateProfile,
-            masterConfig.max_angle_inclination,
+        masterConfig.max_angle_inclination,
-            &currentProfile->accelerometerTrims,
+        &currentProfile->accelerometerTrims,
-            &masterConfig.rxConfig
+        &masterConfig.rxConfig
-        );
+    );
 
-        mixTable();
+    mixTable();
 
 #ifdef USE_SERVOS
-        filterServos();
+    filterServos();
-        writeServos();
+    writeServos();
 #endif
 
-        if (motorControlEnable) {
+    if (motorControlEnable) {
-            writeMotors();
+        writeMotors();
-        }
+    }
-
-        // When no level modes active read acc after motor update
-        if (!flightModeFlags) {
-        	imuUpdateAcc(&currentProfile->accelerometerTrims);
-        }
 
 #ifdef BLACKBOX
-        if (!cliMode && feature(FEATURE_BLACKBOX)) {
+    if (!cliMode && feature(FEATURE_BLACKBOX)) {
-            handleBlackbox();
+        handleBlackbox();
-        }
-#endif
-    }
-
-#ifdef TELEMETRY
-    if (!cliMode && feature(FEATURE_TELEMETRY)) {
-        telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
-    }
-#endif
-
-#ifdef USE_BST
-    bstProcess();
-    bstMasterWriteLoop();
-#endif
-
-
-#ifdef LED_STRIP
-    if (feature(FEATURE_LED_STRIP)) {
-        updateLedStrip();
     }
 #endif
 }
+
+void taskUpdateAccelerometer(void)
+{
+    imuUpdateAccelerometer(&currentProfile->accelerometerTrims);
+}
+
+void taskHandleSerial(void)
+{
+    handleSerial();
+}
+
+void taskUpdateBeeper(void)
+{
+    beeperUpdate();          //call periodic beeper handler
+}
+
+void taskUpdateBattery(void)
+{
+    static uint32_t vbatLastServiced = 0;
+    static uint32_t ibatLastServiced = 0;
+
+    if (feature(FEATURE_VBAT)) {
+        if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
+            vbatLastServiced = currentTime;
+            updateBattery();
+        }
+    }
+
+    if (feature(FEATURE_CURRENT_METER)) {
+        int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
+
+        if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
+            ibatLastServiced = currentTime;
+            updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+        }
+    }
+}
+
+bool taskUpdateRxCheck(void)
+{
+    updateRx(currentTime);
+    return shouldProcessRx(currentTime);
+}
+
+void taskUpdateRxMain(void)
+{
+    processRx();
+    isRXDataNew = true;
+
+#ifdef BARO
+    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
+    if (haveProcessedAnnexCodeOnce) {
+        if (sensors(SENSOR_BARO)) {
+            updateAltHoldState();
+        }
+    }
+#endif
+
+#ifdef SONAR
+    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
+    if (haveProcessedAnnexCodeOnce) {
+        if (sensors(SENSOR_SONAR)) {
+            updateSonarAltHoldState();
+        }
+    }
+#endif
+}
+
+#ifdef GPS
+void taskProcessGPS(void)
+{
+    // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
+    // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
+    // change this based on available hardware
+    if (feature(FEATURE_GPS)) {
+        gpsThread();
+    }
+
+    if (sensors(SENSOR_GPS)) {
+        updateGpsIndicator(currentTime);
+    }
+}
+#endif
+
+#ifdef MAG
+void taskUpdateCompass(void)
+{
+    if (sensors(SENSOR_MAG)) {
+        updateCompass(&masterConfig.magZero);
+    }
+}
+#endif
+
+#ifdef BARO
+void taskUpdateBaro(void)
+{
+    if (sensors(SENSOR_BARO)) {
+        uint32_t newDeadline = baroUpdate();
+        rescheduleTask(TASK_SELF, newDeadline);
+    }
+}
+#endif
+
+#ifdef SONAR
+void taskUpdateSonar(void)
+{
+    if (sensors(SENSOR_SONAR)) {
+        sonarUpdate();
+    }
+}
+#endif
+
+#if defined(BARO) || defined(SONAR)
+void taskCalculateAltitude(void)
+{
+    if (false
+#if defined(BARO)
+        || (sensors(SENSOR_BARO) && isBaroReady())
+#endif
+#if defined(SONAR)
+        || sensors(SENSOR_SONAR)
+#endif
+        ) {
+        calculateEstimatedAltitude(currentTime);
+    }}
+#endif
+
+#ifdef DISPLAY
+void taskUpdateDisplay(void)
+{
+    if (feature(FEATURE_DISPLAY)) {
+        updateDisplay();
+    }
+}
+#endif
+
+#ifdef TELEMETRY
+void taskTelemetry(void)
+{
+    telemetryCheckState();
+
+    if (!cliMode && feature(FEATURE_TELEMETRY)) {
+        telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+    }
+}
+#endif
+
+#ifdef LED_STRIP
+void taskLedStrip(void)
+{
+    if (feature(FEATURE_LED_STRIP)) {
+        updateLedStrip();
+    }
+}
+#endif

src/main/scheduler.c

@@ -0,0 +1,396 @@
+/*
+ * 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 "debug.h"
+
+#include "common/maths.h"
+
+#include "drivers/system.h"
+
+//#define SCHEDULER_DEBUG
+
+cfTaskId_e currentTaskId = TASK_NONE;
+
+static uint32_t totalWaitingTasks;
+static uint32_t totalWaitingTasksSamples;
+static uint32_t realtimeGuardInterval;
+
+uint32_t currentTime = 0;
+uint16_t averageWaitingTasks100 = 0;
+
+typedef struct {
+    /* Configuration */
+    const char * taskName;
+    bool (*checkFunc)(uint32_t currentDeltaTime);
+    void (*taskFunc)(void);
+    bool isEnabled;
+    uint32_t desiredPeriod;     // target period of execution
+    uint8_t staticPriority;     // dynamicPriority grows in steps of this size, shouldn't be zero
+
+    /* Scheduling */
+    uint8_t dynamicPriority;    // measurement of how old task was last executed, used to avoid task starvation
+    uint32_t lastExecutedAt;    // last time of invocation
+    uint32_t lastSignaledAt;    // time of invocation event for event-driven tasks
+    uint16_t taskAgeCycles;
+
+    /* Statistics */
+    uint32_t averageExecutionTime;  // Moving averate over 6 samples, used to calculate guard interval
+    uint32_t taskLatestDeltaTime;   //
+#ifndef SKIP_TASK_STATISTICS
+    uint32_t maxExecutionTime;
+    uint32_t totalExecutionTime;    // total time consumed by task since boot
+#endif
+} cfTask_t;
+
+bool taskMainPidLoopCheck(uint32_t currentDeltaTime);
+void taskMainPidLoop(void);
+void taskUpdateAccelerometer(void);
+void taskHandleSerial(void);
+void taskUpdateBeeper(void);
+void taskUpdateBattery(void);
+bool taskUpdateRxCheck(uint32_t currentDeltaTime);
+void taskUpdateRxMain(void);
+void taskProcessGPS(void);
+void taskUpdateCompass(void);
+void taskUpdateBaro(void);
+void taskUpdateSonar(void);
+void taskCalculateAltitude(void);
+void taskUpdateDisplay(void);
+void taskTelemetry(void);
+void taskLedStrip(void);
+void taskSystem(void);
+
+static cfTask_t cfTasks[TASK_COUNT] = {
+    [TASK_SYSTEM] = {
+        .isEnabled = true,
+        .taskName = "SYSTEM",
+        .taskFunc = taskSystem,
+        .desiredPeriod = 1000000 / 10,              // run every 100 ms
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+    [TASK_GYROPID] = {
+        .taskName = "GYRO/PID",
+        .checkFunc = taskMainPidLoopCheck,
+        .taskFunc = taskMainPidLoop,
+        .desiredPeriod = 1000,
+        .staticPriority = TASK_PRIORITY_REALTIME,
+    },
+
+    [TASK_ACCEL] = {
+        .taskName = "ACCEL",
+        .taskFunc = taskUpdateAccelerometer,
+        .desiredPeriod = 1000000 / 100,
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+    [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 / 10,      // 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 = 1000000 / 20,
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
+#if defined(BARO) || defined(SONAR)
+    [TASK_ALTITUDE] = {
+        .taskName = "ALTITUDE",
+        .taskFunc = taskCalculateAltitude,
+        .desiredPeriod = 1000000 / 40,
+        .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
+};
+
+#define REALTIME_GUARD_INTERVAL_MIN     10
+#define REALTIME_GUARD_INTERVAL_MAX     300
+#define REALTIME_GUARD_INTERVAL_MARGIN  25
+
+void taskSystem(void)
+{
+    uint8_t taskId;
+
+    /* Calculate system load */
+    if (totalWaitingTasksSamples > 0) {
+        averageWaitingTasks100 = 100 * totalWaitingTasks / totalWaitingTasksSamples;
+        totalWaitingTasksSamples = 0;
+        totalWaitingTasks = 0;
+    }
+
+    /* Calculate guard interval */
+    uint32_t maxNonRealtimeTaskTime = 0;
+    for (taskId = 0; taskId < TASK_COUNT; taskId++) {
+        if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
+            maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
+        }
+    }
+
+    realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
+#if defined SCHEDULER_DEBUG
+    debug[2] = realtimeGuardInterval;
+#endif
+}
+
+#ifndef SKIP_TASK_STATISTICS
+void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
+{
+    taskInfo->taskName = cfTasks[taskId].taskName;
+    taskInfo->isEnabled= cfTasks[taskId].isEnabled;
+    taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
+    taskInfo->staticPriority = cfTasks[taskId].staticPriority;
+    taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
+    taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
+    taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
+}
+#endif
+
+void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
+{
+    if (taskId == TASK_SELF)
+        taskId = currentTaskId;
+
+    if (taskId < TASK_COUNT) {
+        cfTasks[taskId].desiredPeriod = MAX(100, newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
+    }
+}
+
+void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState)
+{
+    if (taskId == TASK_SELF)
+        taskId = currentTaskId;
+
+    if (taskId < TASK_COUNT) {
+        cfTasks[taskId].isEnabled = newEnabledState;
+    }
+}
+
+uint32_t getTaskDeltaTime(cfTaskId_e taskId)
+{
+    if (taskId == TASK_SELF)
+        taskId = currentTaskId;
+
+    if (taskId < TASK_COUNT) {
+        return cfTasks[taskId].taskLatestDeltaTime;
+    }
+    else {
+        return 0;
+    }
+}
+
+void scheduler(void)
+{
+    uint8_t taskId;
+    uint8_t selectedTaskId;
+    uint8_t selectedTaskDynPrio;
+    uint16_t waitingTasks = 0;
+    uint32_t timeToNextRealtimeTask = UINT32_MAX;
+
+    /* Cache currentTime */
+    currentTime = micros();
+
+    /* The task to be invoked */
+    selectedTaskId = TASK_NONE;
+    selectedTaskDynPrio = 0;
+
+    /* Check for realtime tasks */
+    for (taskId = 0; taskId < TASK_COUNT; taskId++) {
+        if (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME) {
+            uint32_t nextExecuteAt = cfTasks[taskId].lastExecutedAt + cfTasks[taskId].desiredPeriod;
+            if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
+                timeToNextRealtimeTask = 0;
+            }
+            else {
+                uint32_t newTimeInterval = nextExecuteAt - currentTime;
+                timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
+            }
+        }
+    }
+
+    bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
+
+    /* Update task dynamic priorities */
+    for (taskId = 0; taskId < TASK_COUNT; taskId++) {
+        if (cfTasks[taskId].isEnabled) {
+            /* Task has checkFunc - event driven */
+            if (cfTasks[taskId].checkFunc != NULL) {
+                /* Increase priority for event driven tasks */
+                if (cfTasks[taskId].dynamicPriority > 0) {
+                    cfTasks[taskId].taskAgeCycles = 1 + ((currentTime - cfTasks[taskId].lastSignaledAt) / cfTasks[taskId].desiredPeriod);
+                    cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
+                    waitingTasks++;
+                }
+                else if (cfTasks[taskId].checkFunc(currentTime - cfTasks[taskId].lastExecutedAt)) {
+                    cfTasks[taskId].lastSignaledAt = currentTime;
+                    cfTasks[taskId].taskAgeCycles = 1;
+                    cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority;
+                    waitingTasks++;
+                }
+                else {
+                    cfTasks[taskId].taskAgeCycles = 0;
+                }
+            }
+            /* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */
+            else {
+                // Task age is calculated from last execution
+                cfTasks[taskId].taskAgeCycles = ((currentTime - cfTasks[taskId].lastExecutedAt) / cfTasks[taskId].desiredPeriod);
+                if (cfTasks[taskId].taskAgeCycles > 0) {
+                    cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
+                    waitingTasks++;
+                }
+            }
+
+            /* limit new priority to avoid overflow of uint8_t */
+            cfTasks[taskId].dynamicPriority = MIN(cfTasks[taskId].dynamicPriority, TASK_PRIORITY_MAX);;
+
+            bool taskCanBeChosenForScheduling =
+                (outsideRealtimeGuardInterval) ||
+                (cfTasks[taskId].taskAgeCycles > 1) ||
+                (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME);
+
+            if (taskCanBeChosenForScheduling && (cfTasks[taskId].dynamicPriority > selectedTaskDynPrio)) {
+                selectedTaskDynPrio = cfTasks[taskId].dynamicPriority;
+                selectedTaskId = taskId;
+            }
+        }
+    }
+
+    totalWaitingTasksSamples += 1;
+    totalWaitingTasks += waitingTasks;
+
+    /* Found a task that should be run */
+    if (selectedTaskId != TASK_NONE) {
+        cfTasks[selectedTaskId].taskLatestDeltaTime = currentTime - cfTasks[selectedTaskId].lastExecutedAt;
+        cfTasks[selectedTaskId].lastExecutedAt = currentTime;
+        cfTasks[selectedTaskId].dynamicPriority = 0;
+
+        currentTaskId = selectedTaskId;
+
+        uint32_t currentTimeBeforeTaskCall = micros();
+
+        /* Execute task */
+        if (cfTasks[selectedTaskId].taskFunc != NULL) {
+            cfTasks[selectedTaskId].taskFunc();
+        }
+
+        uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
+
+        cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
+#ifndef SKIP_TASK_STATISTICS
+        cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
+        cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
+#endif
+#if defined SCHEDULER_DEBUG
+        debug[3] = (micros() - currentTime) - taskExecutionTime;
+#endif
+    }
+    else {
+        currentTaskId = TASK_NONE;
+#if defined SCHEDULER_DEBUG
+        debug[3] = (micros() - currentTime);
+#endif
+    }
+}

src/main/scheduler.h

@@ -0,0 +1,90 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+typedef enum {
+    TASK_PRIORITY_IDLE = 0,     // Disables dynamic scheduling, task is executed only if no other task is active this cycle
+    TASK_PRIORITY_LOW = 1,
+    TASK_PRIORITY_MEDIUM = 3,
+    TASK_PRIORITY_HIGH = 5,
+    TASK_PRIORITY_REALTIME = 6,
+    TASK_PRIORITY_MAX = 255
+} cfTaskPriority_e;
+
+typedef struct {
+    const char * taskName;
+    bool         isEnabled;
+    uint32_t     desiredPeriod;
+    uint8_t      staticPriority;
+    uint32_t     maxExecutionTime;
+    uint32_t     totalExecutionTime;
+    uint32_t     lastExecutionTime;
+    uint32_t     averageExecutionTime;
+} cfTaskInfo_t;
+
+typedef enum {
+    /* Actual tasks */
+    TASK_SYSTEM = 0,
+    TASK_GYROPID,
+    TASK_ACCEL,
+    TASK_SERIAL,
+    TASK_BEEPER,
+    TASK_BATTERY,
+    TASK_RX,
+#ifdef GPS
+    TASK_GPS,
+#endif
+#ifdef MAG
+    TASK_COMPASS,
+#endif
+#ifdef BARO
+    TASK_BARO,
+#endif
+#ifdef SONAR
+    TASK_SONAR,
+#endif
+#if defined(BARO) || defined(SONAR)
+    TASK_ALTITUDE,
+#endif
+#ifdef DISPLAY
+    TASK_DISPLAY,
+#endif
+#ifdef TELEMETRY
+    TASK_TELEMETRY,
+#endif
+#ifdef LED_STRIP
+    TASK_LEDSTRIP,
+#endif
+
+    /* Count of real tasks */
+    TASK_COUNT,
+
+    /* Service task IDs */
+    TASK_NONE = TASK_COUNT,
+    TASK_SELF
+} cfTaskId_e;
+
+extern uint16_t cpuLoad;
+extern uint16_t averageWaitingTasks100;
+
+void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo);
+void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
+void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
+uint32_t getTaskDeltaTime(cfTaskId_e taskId);
+
+void scheduler(void);

src/main/sensors/barometer.c

@@ -20,6 +20,7 @@
 #include <math.h>
 
 #include "platform.h"
+#include "scheduler.h"
 
 #include "common/maths.h"
 
@@ -112,8 +113,7 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres
 
 typedef enum {
     BAROMETER_NEEDS_SAMPLES = 0,
-    BAROMETER_NEEDS_CALCULATION,
+    BAROMETER_NEEDS_CALCULATION
-    BAROMETER_NEEDS_PROCESSING
 } barometerState_e;
 
 
@@ -121,37 +121,28 @@ bool isBaroReady(void) {
 	return baroReady;
 }
 
-void baroUpdate(uint32_t currentTime)
+uint32_t baroUpdate(void)
 {
-    static uint32_t baroDeadline = 0;
     static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
 
-    if ((int32_t)(currentTime - baroDeadline) < 0)
-        return;
-
-    baroDeadline = 0;
     switch (state) {
+        default:
         case BAROMETER_NEEDS_SAMPLES:
             baro.get_ut();
             baro.start_up();
             state = BAROMETER_NEEDS_CALCULATION;
-            baroDeadline += baro.up_delay;
+            return baro.up_delay;
         break;
 
         case BAROMETER_NEEDS_CALCULATION:
             baro.get_up();
             baro.start_ut();
-            baroDeadline += baro.ut_delay;
             baro.calculate(&baroPressure, &baroTemperature);
-            state = BAROMETER_NEEDS_PROCESSING;
-        break;
-
-        case BAROMETER_NEEDS_PROCESSING:
-            state = BAROMETER_NEEDS_SAMPLES;
             baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
+            state = BAROMETER_NEEDS_SAMPLES;
+            return baro.ut_delay;
         break;
     }
-    baroDeadline += micros();        // make sure deadline is set after calling baro callbacks
 }
 
 int32_t baroCalculateAltitude(void)

src/main/sensors/barometer.h

@@ -26,7 +26,7 @@ typedef enum {
 } baroSensor_e;
 
 #define BARO_SAMPLE_COUNT_MAX   48
-#define BARO_MAX BARO_BMP280
+#define BARO_MAX BARO_MS5611
 
 typedef struct barometerConfig_s {
     uint8_t baro_sample_count;              // size of baro filter array
@@ -42,7 +42,7 @@ extern int32_t baroTemperature;             // Use temperature for telemetry
 void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
 bool isBaroCalibrationComplete(void);
 void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
-void baroUpdate(uint32_t currentTime);
+uint32_t baroUpdate(void);
 bool isBaroReady(void);
 int32_t baroCalculateAltitude(void);
 void performBaroCalibrationCycle(void);

src/main/target/CJMCU/target.h

@@ -72,6 +72,9 @@
 
 #if (FLASH_SIZE > 64)
 #define BLACKBOX
+#else
+#define SKIP_TASK_STATISTICS
+#define SKIP_CLI_COMMAND_HELP
 #endif
 
 //#undef USE_CLI