Pass currentTime as a parameter to all task functions

src/main/blackbox/blackbox.c

@@ -373,9 +373,6 @@ typedef struct blackboxSlowState_s {
 //From mixer.c:
 extern uint8_t motorCount;
 
-//From mw.c:
-extern uint32_t currentTime;
-
 //From rx.c:
 extern uint16_t rssi;
 
@@ -1024,7 +1021,7 @@ static void writeGPSHomeFrame()
     gpsHistory.GPS_home[1] = GPS_home.lon;
 }
 
-static void writeGPSFrame()
+static void writeGPSFrame(uint32_t currentTime)
 {
     blackboxWrite('G');
 
@@ -1059,7 +1056,7 @@ static void writeGPSFrame()
 /**
  * Fill the current state of the blackbox using values read from the flight controller
  */
-static void loadMainState(void)
+static void loadMainState(uint32_t currentTime)
 {
     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
     int i;
@@ -1442,7 +1439,7 @@ static void blackboxAdvanceIterationTimers()
 }
 
 // Called once every FC loop in order to log the current state
-static void blackboxLogIteration()
+static void blackboxLogIteration(uint32_t currentTime)
 {
     // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
     if (blackboxShouldLogIFrame()) {
@@ -1452,7 +1449,7 @@ static void blackboxLogIteration()
          */
         writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
 
-        loadMainState();
+        loadMainState(currentTime);
         writeIntraframe();
     } else {
         blackboxCheckAndLogArmingBeep();
@@ -1464,7 +1461,7 @@ static void blackboxLogIteration()
              */
             writeSlowFrameIfNeeded(true);
 
-            loadMainState();
+            loadMainState(currentTime);
             writeInterframe();
         }
 #ifdef GPS
@@ -1479,12 +1476,12 @@ static void blackboxLogIteration()
             if (GPS_home.lat != gpsHistory.GPS_home[0] || GPS_home.lon != gpsHistory.GPS_home[1]
                 || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
 
-                writeGPSHomeFrame();
+                writeGPSHomeFrame(currentTime);
-                writeGPSFrame();
+                writeGPSFrame(currentTime);
             } else if (gpsSol.numSat != gpsHistory.GPS_numSat || gpsSol.llh.lat != gpsHistory.GPS_coord[0]
                     || gpsSol.llh.lon != gpsHistory.GPS_coord[1]) {
                 //We could check for velocity changes as well but I doubt it changes independent of position
-                writeGPSFrame();
+                writeGPSFrame(currentTime);
             }
         }
 #endif
@@ -1497,7 +1494,7 @@ static void blackboxLogIteration()
 /**
  * Call each flight loop iteration to perform blackbox logging.
  */
-void handleBlackbox(void)
+void handleBlackbox(uint32_t currentTime)
 {
     int i;
 
@@ -1594,7 +1591,7 @@ void handleBlackbox(void)
                 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
                 blackboxSetState(BLACKBOX_STATE_RUNNING);
 
-                blackboxLogIteration();
+                blackboxLogIteration(currentTime);
             }
 
             // Keep the logging timers ticking so our log iteration continues to advance
@@ -1605,7 +1602,7 @@ void handleBlackbox(void)
             if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
                 blackboxSetState(BLACKBOX_STATE_PAUSED);
             } else {
-                blackboxLogIteration();
+                blackboxLogIteration(currentTime);
             }
 
             blackboxAdvanceIterationTimers();

src/main/blackbox/blackbox.h

@@ -22,7 +22,7 @@
 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
 
 void initBlackbox(void);
-void handleBlackbox(void);
+void handleBlackbox(uint32_t currentTime);
 void startBlackbox(void);
 void finishBlackbox(void);
 bool blackboxMayEditConfig(void);

src/main/fc/fc_tasks.c

@@ -79,15 +79,14 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
-extern uint32_t currentTime;
-
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)
 
-void taskHandleSerial(void)
+void taskHandleSerial(uint32_t currentTime)
 {
+    UNUSED(currentTime);
 #ifdef USE_CLI
     // in cli mode, all serial stuff goes to here. enter cli mode by sending #
     if (cliMode) {
@@ -98,12 +97,12 @@ void taskHandleSerial(void)
     mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA);
 }
 
-void taskUpdateBeeper(void)
+void taskUpdateBeeper(uint32_t currentTime)
 {
-    beeperUpdate();          //call periodic beeper handler
+    beeperUpdate(currentTime);          //call periodic beeper handler
 }
 
-void taskUpdateBattery(void)
+void taskUpdateBattery(uint32_t currentTime)
 {
     static uint32_t vbatLastServiced = 0;
     static uint32_t ibatLastServiced = 0;
@@ -127,7 +126,7 @@ void taskUpdateBattery(void)
 }
 
 #ifdef GPS
-void taskProcessGPS(void)
+void taskProcessGPS(uint32_t currentTime)
 {
     // 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
@@ -143,17 +142,19 @@ void taskProcessGPS(void)
 #endif
 
 #ifdef MAG
-void taskUpdateCompass(void)
+void taskUpdateCompass(uint32_t currentTime)
 {
     if (sensors(SENSOR_MAG)) {
-        updateCompass(&masterConfig.magZero);
+        updateCompass(currentTime, &masterConfig.magZero);
     }
 }
 #endif
 
 #ifdef BARO
-void taskUpdateBaro(void)
+void taskUpdateBaro(uint32_t currentTime)
 {
+    UNUSED(currentTime);
+
     if (sensors(SENSOR_BARO)) {
         const uint32_t newDeadline = baroUpdate();
         if (newDeadline != 0) {
@@ -166,8 +167,10 @@ void taskUpdateBaro(void)
 #endif
 
 #ifdef SONAR
-void taskUpdateSonar(void)
+void taskUpdateSonar(uint32_t currentTime)
 {
+    UNUSED(currentTime);
+
     if (sensors(SENSOR_SONAR)) {
         rangefinderUpdate();
     }
@@ -177,36 +180,38 @@ void taskUpdateSonar(void)
 #endif
 
 #ifdef DISPLAY
-void taskUpdateDisplay(void)
+void taskUpdateDisplay(uint32_t currentTime)
 {
     if (feature(FEATURE_DISPLAY)) {
-        updateDisplay();
+        updateDisplay(currentTime);
     }
 }
 #endif
 
 #ifdef TELEMETRY
-void taskTelemetry(void)
+void taskTelemetry(uint32_t currentTime)
 {
     telemetryCheckState();
 
     if (!cliMode && feature(FEATURE_TELEMETRY)) {
-        telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+        telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
     }
 }
 #endif
 
 #ifdef LED_STRIP
-void taskLedStrip(void)
+void taskLedStrip(uint32_t currentTime)
 {
     if (feature(FEATURE_LED_STRIP)) {
-        updateLedStrip();
+        updateLedStrip(currentTime);
     }
 }
 #endif
 
 #ifdef USE_PMW_SERVO_DRIVER
-void taskSyncPwmDriver(void) {
+void taskSyncPwmDriver(uint32_t currentTime)
+{
+    UNUSED(currentTime);
 
     if (feature(FEATURE_PWM_SERVO_DRIVER)) {
         pwmDriverSync();
@@ -215,11 +220,15 @@ void taskSyncPwmDriver(void) {
 #endif
 
 #ifdef ASYNC_GYRO_PROCESSING
-void taskAttitude(void) {
+void taskAttitude(uint32_t currentTime)
-    imuUpdateAttitude();
+{
+    imuUpdateAttitude(currentTime);
 }
 
-void taskAcc(void) {
+void taskAcc(uint32_t currentTime)
+{
+    UNUSED(currentTime);
+
     imuUpdateAccelerometer();
 }
 #endif

src/main/fc/fc_tasks.h

@@ -19,25 +19,13 @@
 
 #include <stdint.h>
 
-void taskHandleSerial(void);
+void taskMainPidLoopChecker(uint32_t currentTime);
-void taskUpdateBeeper(void);
+bool taskUpdateRxCheck(uint32_t currentTime, uint32_t currentDeltaTime);
-void taskUpdateBattery(void);
+void taskUpdateRxMain(uint32_t currentTime);
-bool taskUpdateRxCheck(uint32_t currentDeltaTime);
+void taskSystem(uint32_t currentTime);
-void taskUpdateRxMain(void);
+void taskStackCheck(uint32_t currentTime);
-void taskSystem(void);
 
-#ifdef USE_PMW_SERVO_DRIVER
+void taskMainPidLoop(uint32_t currentTime);
-void taskSyncPwmDriver(void);
+void taskGyro(uint32_t currentTime);
-#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

@@ -100,8 +100,6 @@ 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;
-
 static bool isRXDataNew;
 
 bool isCalibrating(void)
@@ -258,7 +256,7 @@ void mwArm(void)
     }
 }
 
-void processRx(void)
+void processRx(uint32_t currentTime)
 {
     static bool armedBeeperOn = false;
 
@@ -522,13 +520,13 @@ void filterRc(bool isRXDataNew)
 }
 
 // Function for loop trigger
-void taskGyro(void) {
+void taskGyro(uint32_t currentTime) {
     // 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);
+    const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 
     if (masterConfig.gyroSync) {
-        while (1) {
+        while (true) {
         #ifdef ASYNC_GYRO_PROCESSING
             if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
         #else
@@ -548,25 +546,25 @@ void taskGyro(void) {
 #endif
 }
 
-void taskMainPidLoop(void)
+void taskMainPidLoop(uint32_t currentTime)
 {
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
 #ifdef ASYNC_GYRO_PROCESSING
     if (getAsyncMode() == ASYNC_MODE_NONE) {
-        taskGyro();
+        taskGyro(currentTime);
     }
 
     if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
         imuUpdateAccelerometer();
-        imuUpdateAttitude();
+        imuUpdateAttitude(currentTime);
     }
 #else
     /* Update gyroscope */
-    taskGyro();
+    taskGyro(currentTime);
     imuUpdateAccelerometer();
-    imuUpdateAttitude();
+    imuUpdateAttitude(currentTime);
 #endif
 
 
@@ -670,21 +668,21 @@ void taskMainPidLoop(void)
 
 #ifdef BLACKBOX
     if (!cliMode && feature(FEATURE_BLACKBOX)) {
-        handleBlackbox();
+        handleBlackbox(micros());
     }
 #endif
 
 }
 
-bool taskUpdateRxCheck(uint32_t currentDeltaTime)
+bool taskUpdateRxCheck(uint32_t currentTime, uint32_t currentDeltaTime)
 {
     UNUSED(currentDeltaTime);
-    updateRx(currentTime);
+
-    return shouldProcessRx(currentTime);
+    return updateRx(currentTime);
 }
 
-void taskUpdateRxMain(void)
+void taskUpdateRxMain(uint32_t currentTime)
 {
-    processRx();
+    processRx(currentTime);
     isRXDataNew = true;
 }

src/main/flight/imu.c

@@ -573,12 +573,11 @@ void imuUpdateAccelerometer(void)
 #endif
 }
 
-void imuUpdateAttitude(void)
+void imuUpdateAttitude(uint32_t currentTime)
 {
     /* Calculate dT */
     static uint32_t previousIMUUpdateTime;
-    uint32_t currentTime = micros();
+    const float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
-    float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
     previousIMUUpdateTime = currentTime;
 
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {

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 imuUpdateAttitude(void);
+void imuUpdateAttitude(uint32_t currentTime);
 void imuUpdateAccelerometer(void);
 void imuUpdateGyroscope(uint32_t gyroUpdateDeltaUs);
 float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength);

src/main/io/beeper.c

@@ -287,7 +287,7 @@ void beeperGpsStatus(void)
  * Beeper handler function to be called periodically in loop. Updates beeper
  * state via time schedule.
  */
-void beeperUpdate(void)
+void beeperUpdate(uint32_t currentTime)
 {
     // If beeper option from AUX switch has been selected
     if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
@@ -307,7 +307,7 @@ void beeperUpdate(void)
         return;
     }
 
-    uint32_t now = millis();
+    const uint32_t now = currentTime / 1000;
     if (beeperNextToggleTime > now) {
         return;
     }
@@ -325,7 +325,7 @@ void beeperUpdate(void)
                 beeperPos == 0
                 && (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX)
             ) {
-                armingBeepTimeMicros = micros();
+                armingBeepTimeMicros = currentTime;
             }
         }
     } else {

src/main/io/beeper.h

@@ -48,7 +48,7 @@ typedef enum {
 
 void beeper(beeperMode_e mode);
 void beeperSilence(void);
-void beeperUpdate(void);
+void beeperUpdate(uint32_t currentTime);
 void beeperConfirmationBeeps(uint8_t beepCount);
 uint32_t getArmingBeepTimeMicros(void);
 beeperMode_e beeperModeForTableIndex(int idx);

src/main/io/display.c

@@ -370,19 +370,18 @@ void showStatusPage(void)
 
 }
 
-void updateDisplay(void)
+void updateDisplay(uint32_t currentTime)
 {
-    uint32_t now = micros();
     static uint8_t previousArmedState = 0;
 
     bool pageChanging = false;
-    bool updateNow = (int32_t)(now - nextDisplayUpdateAt) >= 0L;
+    bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L;
 
     if (!updateNow) {
         return;
     }
 
-    nextDisplayUpdateAt = now + DISPLAY_UPDATE_FREQUENCY;
+    nextDisplayUpdateAt = currentTime + DISPLAY_UPDATE_FREQUENCY;
 
     bool armedState = ARMING_FLAG(ARMED) ? true : false;
     bool armedStateChanged = armedState != previousArmedState;
@@ -400,7 +399,7 @@ void updateDisplay(void)
             pageChanging = true;
         }
 
-        if ((currentPageId == PAGE_WELCOME) && ((int32_t)(now - nextPageAt) >= 0L)) {
+        if ((currentPageId == PAGE_WELCOME) && ((int32_t)(currentTime - nextPageAt) >= 0L)) {
             currentPageId = PAGE_STATUS;
             pageChanging = true;
         }
@@ -465,9 +464,10 @@ void displayInit(rxConfig_t *rxConfigToUse)
     rxConfig = rxConfigToUse;
 
     displaySetPage(PAGE_WELCOME);
-    displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5));
+    const uint32_t now = micros();
+    displaySetNextPageChangeAt(now + 5* MICROSECONDS_IN_A_SECOND);
 
-    updateDisplay();
+    updateDisplay(now);
 }
 
 void displaySetNextPageChangeAt(uint32_t futureMicros)

src/main/io/display.h

@@ -23,7 +23,7 @@ typedef enum {
 
 struct rxConfig_s;
 void displayInit(struct rxConfig_s *intialRxConfig);
-void updateDisplay(void);
+void updateDisplay(uint32_t currentTime);
 
 void displaySetPage(pageId_e newPageId);
 void displaySetNextPageChangeAt(uint32_t futureMicros);

src/main/io/ledstrip.c

@@ -920,7 +920,7 @@ static applyLayerFn_timed* layerTable[] = {
     [timRing] = &applyLedThrustRingLayer
 };
 
-void updateLedStrip(void)
+void updateLedStrip(uint32_t currentTime)
 {
     if (!(ledStripInitialised && isWS2811LedStripReady())) {
         return;
@@ -935,19 +935,17 @@ void updateLedStrip(void)
     }
     ledStripEnabled = true;
 
-    uint32_t now = micros();
-
     // test all led timers, setting corresponding bits
     uint32_t timActive = 0;
     for (timId_e timId = 0; timId < timTimerCount; timId++) {
         // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
         // max delay is limited to 5s
-        int32_t delta = cmp32(now, timerVal[timId]);
+        int32_t delta = cmp32(currentTime, timerVal[timId]);
         if (delta < 0 && delta > -LED_STRIP_MS(5000))
             continue;  // not ready yet
         timActive |= 1 << timId;
         if (delta >= LED_STRIP_MS(100) || delta < 0) {
-            timerVal[timId] = now;
+            timerVal[timId] = currentTime;
         }
     }
 

src/main/io/ledstrip.h

@@ -166,7 +166,7 @@ void reevaluateLedConfig(void);
 
 void ledStripInit(ledConfig_t *ledConfigsToUse, struct hsvColor_s *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse);
 void ledStripEnable(void);
-void updateLedStrip(void);
+void updateLedStrip(uint32_t currentTime);
 
 bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex);
 

src/main/rx/rx.c

@@ -367,7 +367,7 @@ void resumeRxSignal(void)
     failsafeOnRxResume();
 }
 
-void updateRx(uint32_t currentTime)
+bool updateRx(uint32_t currentTime)
 {
     resetRxSignalReceivedFlagIfNeeded(currentTime);
 
@@ -429,10 +429,6 @@ void updateRx(uint32_t currentTime)
         }
     }
 #endif
-}
-
-bool shouldProcessRx(uint32_t currentTime)
-{
     return rxDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
 }
 

src/main/rx/rx.h

@@ -147,10 +147,9 @@ typedef uint16_t (*rcReadRawDataPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, u
 struct modeActivationCondition_s;
 void rxInit(rxConfig_t *rxConfig, struct modeActivationCondition_s *modeActivationConditions);
 void useRxConfig(rxConfig_t *rxConfigToUse);
-void updateRx(uint32_t currentTime);
+bool updateRx(uint32_t currentTime);
 bool rxIsReceivingSignal(void);
 bool rxAreFlightChannelsValid(void);
-bool shouldProcessRx(uint32_t currentTime);
 void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);
 
 void parseRcChannels(const char *input, rxConfig_t *rxConfig);

src/main/scheduler/scheduler.c

@@ -40,7 +40,6 @@ static uint32_t totalWaitingTasks;
 static uint32_t totalWaitingTasksSamples;
 static uint32_t realtimeGuardInterval;
 
-uint32_t currentTime = 0;
 uint16_t averageSystemLoadPercent = 0;
 
 #define REALTIME_GUARD_INTERVAL_MIN     10
@@ -124,16 +123,18 @@ STATIC_INLINE_UNIT_TESTED cfTask_t *queueNext(void)
     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 }
 
-void taskSystem(void)
+void taskSystem(uint32_t currentTime)
 {
-    /* Calculate system load */
+    UNUSED(currentTime);
+
+    // Calculate system load
     if (totalWaitingTasksSamples > 0) {
         averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
         totalWaitingTasksSamples = 0;
         totalWaitingTasks = 0;
     }
 
-    /* Calculate guard interval */
+    // Calculate guard interval
     uint32_t maxNonRealtimeTaskTime = 0;
     for (const cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
         if (task->staticPriority != TASK_PRIORITY_REALTIME) {
@@ -200,8 +201,9 @@ void schedulerInit(void)
 void scheduler(void)
 {
     // Cache currentTime
-    currentTime = micros();
+    const uint32_t currentTime = micros();
 
+    // Check for realtime tasks
     uint32_t timeToNextRealtimeTask = UINT32_MAX;
     for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
         const uint32_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
@@ -228,7 +230,7 @@ void scheduler(void)
                 task->taskAgeCycles = 1 + ((currentTime - task->lastSignaledAt) / task->desiredPeriod);
                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
                 waitingTasks++;
-            } else if (task->checkFunc(currentTime - task->lastExecutedAt)) {
+            } else if (task->checkFunc(currentTime, currentTime - task->lastExecutedAt)) {
                 task->lastSignaledAt = currentTime;
                 task->taskAgeCycles = 1;
                 task->dynamicPriority = 1 + task->staticPriority;
@@ -271,7 +273,7 @@ void scheduler(void)
 
         // Execute task
         const uint32_t currentTimeBeforeTaskCall = micros();
-        selectedTask->taskFunc();
+        selectedTask->taskFunc(currentTimeBeforeTaskCall);
         const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 
         selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32;

src/main/scheduler/scheduler.h

@@ -93,8 +93,8 @@ typedef enum {
 typedef struct {
     /* Configuration */
     const char * taskName;
-    bool (*checkFunc)(uint32_t currentDeltaTime);
+    bool (*checkFunc)(uint32_t currentTime, uint32_t currentDeltaTime);
-    void (*taskFunc)(void);
+    void (*taskFunc)(uint32_t currentTime);
     uint32_t desiredPeriod;         // target period of execution
     const uint8_t staticPriority;   // dynamicPriority grows in steps of this size, shouldn't be zero
 

src/main/sensors/compass.c

@@ -43,8 +43,6 @@
 mag_t mag;                   // mag access functions
 float magneticDeclination = 0.0f;       // calculated at startup from config
 
-extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
-
 int16_t magADCRaw[XYZ_AXIS_COUNT];
 int32_t magADC[XYZ_AXIS_COUNT];
 sensor_align_e magAlign = 0;
@@ -74,7 +72,7 @@ bool isCompassReady(void)
 
 static sensorCalibrationState_t calState;
 
-void updateCompass(flightDynamicsTrims_t *magZero)
+void updateCompass(uint32_t currentTime, flightDynamicsTrims_t *magZero)
 {
     static uint32_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];

src/main/sensors/compass.h

@@ -33,12 +33,10 @@ typedef enum {
 
 #define MAG_MAX  MAG_FAKE
 
-#ifdef MAG
 bool compassInit(int16_t magDeclinationFromConfig);
 union flightDynamicsTrims_u;
-void updateCompass(union flightDynamicsTrims_u *magZero);
+void updateCompass(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
 bool isCompassReady(void);
-#endif
 
 extern int32_t magADC[XYZ_AXIS_COUNT];
 

src/main/telemetry/hott.c

@@ -492,7 +492,7 @@ void checkHoTTTelemetryState(void)
         freeHoTTTelemetryPort();
 }
 
-void handleHoTTTelemetry(void)
+void handleHoTTTelemetry(uint32_t currentTime)
 {
     static uint32_t serialTimer;
 
@@ -500,27 +500,25 @@ void handleHoTTTelemetry(void)
         return;
     }
 
-    uint32_t now = micros();
+    if (shouldPrepareHoTTMessages(currentTime)) {
-
-    if (shouldPrepareHoTTMessages(now)) {
         hottPrepareMessages();
-        lastMessagesPreparedAt = now;
+        lastMessagesPreparedAt = currentTime;
     }
 
     if (shouldCheckForHoTTRequest()) {
-        hottCheckSerialData(now);
+        hottCheckSerialData(currentTime);
     }
 
     if (!hottMsg)
         return;
 
     if (hottIsSending) {
-        if(now - serialTimer < HOTT_TX_DELAY_US) {
+        if(currentTime - serialTimer < HOTT_TX_DELAY_US) {
             return;
         }
     }
     hottSendTelemetryData();
-    serialTimer = now;
+    serialTimer = currentTime;
 }
 
 #endif

src/main/telemetry/hott.h

@@ -487,7 +487,7 @@ typedef struct HOTT_AIRESC_MSG_s {
     uint8_t stop_byte;      //#44 constant value 0x7d
 } HOTT_AIRESC_MSG_t;
 
-void handleHoTTTelemetry(void);
+void handleHoTTTelemetry(uint32_t currentTime);
 void checkHoTTTelemetryState(void);
 
 void initHoTTTelemetry(telemetryConfig_t *telemetryConfig);

src/main/telemetry/mavlink.c

@@ -272,7 +272,7 @@ void mavlinkSendRCChannelsAndRSSI(void)
 }
 
 #if defined(GPS)
-void mavlinkSendPosition(void)
+void mavlinkSendPosition(uint32_t currentTime)
 {
     uint16_t msgLength;
     uint8_t gpsFixType = 0;
@@ -289,7 +289,7 @@ void mavlinkSendPosition(void)
 
     mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
-        micros(),
+        currentTime,
         // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
         gpsFixType,
         // lat Latitude in 1E7 degrees
@@ -314,7 +314,7 @@ void mavlinkSendPosition(void)
     // Global position
     mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
-        micros(),
+        currentTime,
         // lat Latitude in 1E7 degrees
         gpsSol.llh.lat,
         // lon Longitude in 1E7 degrees
@@ -501,7 +501,7 @@ void mavlinkSendHUDAndHeartbeat(void)
     mavlinkSerialWrite(mavBuffer, msgLength);
 }
 
-void processMAVLinkTelemetry(void)
+void processMAVLinkTelemetry(uint32_t currentTime)
 {
     // is executed @ TELEMETRY_MAVLINK_MAXRATE rate
     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
@@ -514,7 +514,7 @@ void processMAVLinkTelemetry(void)
 
 #ifdef GPS
     if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) {
-        mavlinkSendPosition();
+        mavlinkSendPosition(currentTime);
     }
 #endif
 
@@ -527,7 +527,7 @@ void processMAVLinkTelemetry(void)
     }
 }
 
-void handleMAVLinkTelemetry(void)
+void handleMAVLinkTelemetry(uint32_t currentTime)
 {
     if (!mavlinkTelemetryEnabled) {
         return;
@@ -537,10 +537,9 @@ void handleMAVLinkTelemetry(void)
         return;
     }
 
-    uint32_t now = micros();
+    if ((currentTime - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
-    if ((now - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
+        processMAVLinkTelemetry(currentTime);
-        processMAVLinkTelemetry();
+        lastMavlinkMessage = currentTime;
-        lastMavlinkMessage = now;
     }
 }
 

src/main/telemetry/mavlink.h

@@ -19,7 +19,7 @@
 #define TELEMETRY_MAVLINK_H_
 
 void initMAVLinkTelemetry(void);
-void handleMAVLinkTelemetry(void);
+void handleMAVLinkTelemetry(uint32_t currentTime);
 void checkMAVLinkTelemetryState(void);
 
 void freeMAVLinkTelemetryPort(void);

src/main/telemetry/telemetry.c

@@ -138,7 +138,7 @@ void telemetryCheckState(void)
 
 }
 
-void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
+void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 {
 #if defined(TELEMETRY_FRSKY)
     handleFrSkyTelemetry(rxConfig, deadband3d_throttle);
@@ -148,7 +148,9 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 #endif
 
 #if defined(TELEMETRY_HOTT)
-    handleHoTTTelemetry();
+    handleHoTTTelemetry(currentTime);
+#else
+    UNUSED(currentTime);
 #endif
 
 #if defined(TELEMETRY_SMARTPORT)
@@ -160,7 +162,9 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 #endif
 
 #if defined(TELEMETRY_MAVLINK)
-    handleMAVLinkTelemetry();
+    handleMAVLinkTelemetry(currentTime);
+#else
+    UNUSED(currentTime);
 #endif
 
 #if defined(TELEMETRY_JETIEXBUS)

src/main/telemetry/telemetry.h

@@ -56,7 +56,7 @@ extern struct serialPort_s *telemetrySharedPort;
 
 void telemetryCheckState(void);
 struct rxConfig_s;
-void telemetryProcess(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
+void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
 
 bool telemetryDetermineEnabledState(portSharing_e portSharing);