diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 31b35f4a00..df4407e776 100755
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -362,8 +362,8 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 }
 
 #ifdef BARO
-#define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
-
+// 40hz update rate (20hz LPF on acc)
+#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
 
 #define DEGREES_80_IN_DECIDEGREES 800
 
@@ -405,10 +405,9 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
     return baroPID;
 }
 
-int getEstimatedAltitude(void)
+void calculateEstimatedAltitude(uint32_t currentTime)
 {
-    static uint32_t previousT;
-    uint32_t currentT = micros();
+    static uint32_t previousTime;
     uint32_t dTime;
     int32_t baroVel;
     float dt;
@@ -420,10 +419,11 @@ int getEstimatedAltitude(void)
     static float accAlt = 0.0f;
     static int32_t lastBaroAlt;
 
-    dTime = currentT - previousT;
-    if (dTime < UPDATE_INTERVAL)
-        return 0;
-    previousT = currentT;
+    dTime = currentTime - previousTime;
+    if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
+        return;
+        
+    previousTime = currentTime;
 
     if (!isBaroCalibrationComplete()) {
         performBaroCalibrationCycle();
@@ -469,7 +469,5 @@ int getEstimatedAltitude(void)
     BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
 
     accZ_old = accZ_tmp;
-
-    return 1;
 }
 #endif /* BARO */
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index ad2b0478a8..d7c6707bcf 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s {
 
 void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband);
 
-int getEstimatedAltitude(void);
+void calculateEstimatedAltitude(uint32_t currentTime);
 void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration);
 void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
diff --git a/src/main/mw.c b/src/main/mw.c
index 4f96476beb..4303d6e254 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -349,175 +349,227 @@ void updateInflightCalibrationState(void)
     }
 }
 
-void loop(void)
+void updateMagHold(void)
+{
+    if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
+        int16_t dif = heading - magHold;
+        if (dif <= -180)
+            dif += 360;
+        if (dif >= +180)
+            dif -= 360;
+        dif *= -masterConfig.yaw_control_direction;
+        if (f.SMALL_ANGLE)
+            rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30;    // 18 deg
+    } else
+        magHold = heading;
+}
+
+typedef enum {
+#ifdef MAG
+	UPDATE_COMPASS_TASK,
+#endif
+#ifdef BARO
+	UPDATE_BARO_TASK,
+	CALCULATE_ALTITUDE_TASK,
+#endif
+#ifdef SONAR
+	UPDATE_SONAR_TASK,
+#endif
+	UPDATE_GPS_TASK
+} periodicTasks;
+
+#define PERIODIC_TASK_COUNT (UPDATE_GPS_TASK + 1)
+
+
+void executePeriodicTasks(void)
+{
+    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)) {
+        	baroUpdate(currentTime);
+        }
+        break;
+
+    case CALCULATE_ALTITUDE_TASK:
+        if (sensors(SENSOR_BARO) && isBaroReady()) {
+        	calculateEstimatedAltitude(currentTime);
+        }
+		break;
+#endif
+
+    case UPDATE_GPS_TASK:
+        // 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();
+        }
+        break;
+#ifdef SONAR
+    case UPDATE_SONAR_TASK:
+        if (sensors(SENSOR_SONAR)) {
+            Sonar_update();
+        }
+        break;
+#endif
+    }
+
+    if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {
+    	periodicTaskIndex = 0;
+    }
+}
+
+void processRx(void)
 {
     int i;
-    static uint32_t loopTime;
     uint32_t auxState = 0;
 
+    calculateRxChannelsAndUpdateFailsafe(currentTime);
+
+    // in 3D mode, we need to be able to disarm by switch at any time
+    if (feature(FEATURE_3D)) {
+        if (!rcOptions[BOXARM])
+            mwDisarm();
+    }
+
+    updateRSSI(currentTime);
+
+    if (feature(FEATURE_FAILSAFE)) {
+
+        if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
+            failsafe->vTable->enable();
+        }
+
+        failsafe->vTable->updateState();
+    }
+
+    throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+
+    if (throttleStatus == THROTTLE_LOW) {
+        resetErrorAngle();
+        resetErrorGyro();
+    }
+
+    processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm);
+
+    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
+        updateInflightCalibrationState();
+    }
+
+    // Check AUX switches
+
+    // auxState is a bitmask, 3 bits per channel. aux1 is first.
+    // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8
+    //
+    // the three bits are as follows:
+    // bit 1 is SET when the stick is less than 1300
+    // bit 2 is SET when the stick is between 1300 and 1700
+    // bit 3 is SET when the stick is above 1700
+    // if the value is 1300 or 1700 NONE of the three bits are set.
+
+    for (i = 0; i < 4; i++) {
+        auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) |
+                (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) |
+                (rcData[AUX1 + i] > 1700) << (3 * i + 2);
+        auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) |
+                (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) |
+                (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16;
+    }
+    for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
+        rcOptions[i] = (auxState & currentProfile.activate[i]) > 0;
+
+    if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
+        // bumpless transfer to Level mode
+        if (!f.ANGLE_MODE) {
+            resetErrorAngle();
+            f.ANGLE_MODE = 1;
+        }
+    } else {
+        f.ANGLE_MODE = 0; // failsafe support
+    }
+
+    if (rcOptions[BOXHORIZON]) {
+        f.ANGLE_MODE = 0;
+        if (!f.HORIZON_MODE) {
+            resetErrorAngle();
+            f.HORIZON_MODE = 1;
+        }
+    } else {
+        f.HORIZON_MODE = 0;
+    }
+
+    if (f.ANGLE_MODE || f.HORIZON_MODE) {
+        LED1_ON;
+    } else {
+        LED1_OFF;
+    }
+
+#ifdef BARO
+    if (sensors(SENSOR_BARO)) {
+        updateAltHoldState();
+    }
+#endif
+
+#ifdef  MAG
+    if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
+        if (rcOptions[BOXMAG]) {
+            if (!f.MAG_MODE) {
+                f.MAG_MODE = 1;
+                magHold = heading;
+            }
+        } else {
+            f.MAG_MODE = 0;
+        }
+        if (rcOptions[BOXHEADFREE]) {
+            if (!f.HEADFREE_MODE) {
+                f.HEADFREE_MODE = 1;
+            }
+        } else {
+            f.HEADFREE_MODE = 0;
+        }
+        if (rcOptions[BOXHEADADJ]) {
+            headFreeModeHold = heading; // acquire new heading
+        }
+    }
+#endif
+
+    if (sensors(SENSOR_GPS)) {
+        updateGpsWaypointsAndMode();
+    }
+
+    if (rcOptions[BOXPASSTHRU]) {
+        f.PASSTHRU_MODE = 1;
+    } else {
+        f.PASSTHRU_MODE = 0;
+    }
+
+    if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
+        f.HEADFREE_MODE = 0;
+    }
+}
+
+void loop(void)
+{
+    static uint32_t loopTime;
+
     updateRx();
 
     if (shouldProcessRx(currentTime)) {
-        calculateRxChannelsAndUpdateFailsafe(currentTime);
-
-        // in 3D mode, we need to be able to disarm by switch at any time
-        if (feature(FEATURE_3D)) {
-            if (!rcOptions[BOXARM])
-                mwDisarm();
-        }
-
-        updateRSSI(currentTime);
-
-        if (feature(FEATURE_FAILSAFE)) {
-
-            if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
-                failsafe->vTable->enable();
-            }
-
-            failsafe->vTable->updateState();
-        }
-
-        throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
-
-        if (throttleStatus == THROTTLE_LOW) {
-            resetErrorAngle();
-            resetErrorGyro();
-        }
-
-        processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm);
-
-        if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
-            updateInflightCalibrationState();
-        }
-
-        // Check AUX switches
-
-        // auxState is a bitmask, 3 bits per channel. aux1 is first.
-        // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8
-        //
-        // the three bits are as follows:
-        // bit 1 is SET when the stick is less than 1300
-        // bit 2 is SET when the stick is between 1300 and 1700
-        // bit 3 is SET when the stick is above 1700
-        // if the value is 1300 or 1700 NONE of the three bits are set.
-
-        for (i = 0; i < 4; i++) {
-            auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) |
-                    (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) |
-                    (rcData[AUX1 + i] > 1700) << (3 * i + 2);
-            auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) |
-                    (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) |
-                    (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16;
-        }
-        for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
-            rcOptions[i] = (auxState & currentProfile.activate[i]) > 0;
-
-        if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
-            // bumpless transfer to Level mode
-            if (!f.ANGLE_MODE) {
-                resetErrorAngle();
-                f.ANGLE_MODE = 1;
-            }
-        } else {
-            f.ANGLE_MODE = 0; // failsafe support
-        }
-
-        if (rcOptions[BOXHORIZON]) {
-            f.ANGLE_MODE = 0;
-            if (!f.HORIZON_MODE) {
-                resetErrorAngle();
-                f.HORIZON_MODE = 1;
-            }
-        } else {
-            f.HORIZON_MODE = 0;
-        }
-
-        if (f.ANGLE_MODE || f.HORIZON_MODE) {
-            LED1_ON;
-        } else {
-            LED1_OFF;
-        }
-
-#ifdef BARO
-        if (sensors(SENSOR_BARO)) {
-            updateAltHoldState();
-        }
-#endif
-
-#ifdef  MAG
-        if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
-            if (rcOptions[BOXMAG]) {
-                if (!f.MAG_MODE) {
-                    f.MAG_MODE = 1;
-                    magHold = heading;
-                }
-            } else {
-                f.MAG_MODE = 0;
-            }
-            if (rcOptions[BOXHEADFREE]) {
-                if (!f.HEADFREE_MODE) {
-                    f.HEADFREE_MODE = 1;
-                }
-            } else {
-                f.HEADFREE_MODE = 0;
-            }
-            if (rcOptions[BOXHEADADJ]) {
-                headFreeModeHold = heading; // acquire new heading
-            }
-        }
-#endif
-
-        if (sensors(SENSOR_GPS)) {
-            updateGpsWaypointsAndMode();
-        }
-
-        if (rcOptions[BOXPASSTHRU]) {
-            f.PASSTHRU_MODE = 1;
-        } else {
-            f.PASSTHRU_MODE = 0;
-        }
-
-        if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
-            f.HEADFREE_MODE = 0;
-        }
-    } else {                    // not in rc loop
-        static int taskOrder = 0;    // never call all function in the same loop, to avoid high delay spikes
-        switch (taskOrder) {
-        case 0:
-            taskOrder++;
-#ifdef MAG
-            if (sensors(SENSOR_MAG) && compassGetADC(&masterConfig.magZero))
-                break;
-#endif
-        case 1:
-            taskOrder++;
-#ifdef BARO
-            if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY)
-                break;
-#endif
-        case 2:
-            taskOrder++;
-#ifdef BARO
-            if (sensors(SENSOR_BARO) && getEstimatedAltitude())
-                break;
-#endif
-        case 3:
-            // 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
-            taskOrder++;
-            if (feature(FEATURE_GPS)) {
-                gpsThread();
-                break;
-            }
-        case 4:
-            taskOrder = 0;
-#ifdef SONAR
-            if (sensors(SENSOR_SONAR)) {
-                Sonar_update();
-            }
-#endif
-        }
+    	processRx();
+    } else {
+    	// not in rc loop
+    	executePeriodicTasks();
     }
 
     currentTime = micros();
@@ -537,17 +589,7 @@ void loop(void)
 
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {
-            if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
-                int16_t dif = heading - magHold;
-                if (dif <= -180)
-                    dif += 360;
-                if (dif >= +180)
-                    dif -= 360;
-                dif *= -masterConfig.yaw_control_direction;
-                if (f.SMALL_ANGLE)
-                    rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30;    // 18 deg
-            } else
-                magHold = heading;
+        	updateMagHold();
         }
 #endif
 
@@ -570,7 +612,12 @@ void loop(void)
         }
 
         // PID - note this is function pointer set by setPIDController()
-        pid_controller(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, &currentProfile.accelerometerTrims);
+        pid_controller(
+			&currentProfile.pidProfile,
+			&currentProfile.controlRateConfig,
+			masterConfig.max_angle_inclination,
+			&currentProfile.accelerometerTrims
+        );
 
         mixTable();
         writeServos();
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 1ca148e8ee..808dbb0948 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -30,7 +30,7 @@ baro_t baro;                        // barometer access functions
 uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
 int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
-uint32_t baroPressureSum = 0;
+static uint32_t baroPressureSum = 0;
 int32_t BaroAlt = 0;
 
 #ifdef BARO
@@ -82,13 +82,19 @@ typedef enum {
     BAROMETER_NEEDS_CALCULATION
 } barometerState_e;
 
-barometerAction_e baroUpdate(uint32_t currentTime)
+static bool baroReady = false;
+
+bool isBaroReady(void) {
+	return baroReady;
+}
+
+void baroUpdate(uint32_t currentTime)
 {
     static uint32_t baroDeadline = 0;
     static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
 
     if ((int32_t)(currentTime - baroDeadline) < 0)
-        return BAROMETER_ACTION_NOT_READY;
+        return;
 
     baroDeadline = currentTime;
 
@@ -98,17 +104,17 @@ barometerAction_e baroUpdate(uint32_t currentTime)
             baro.start_ut();
             baroDeadline += baro.ut_delay;
             baro.calculate(&baroPressure, &baroTemperature);
+            baroReady = true;
             state = BAROMETER_NEEDS_SAMPLES;
-            return BAROMETER_ACTION_PERFORMED_CALCULATION;
+		break;
 
         case BAROMETER_NEEDS_SAMPLES:
-        default:
             baro.get_ut();
             baro.start_up();
             baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
             state = BAROMETER_NEEDS_CALCULATION;
             baroDeadline += baro.up_delay;
-            return BAROMETER_ACTION_OBTAINED_SAMPLES;
+		break;
     }
 }
 
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 4e32933e25..b829857840 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -26,19 +26,14 @@ typedef struct barometerConfig_s {
     float baro_cf_alt;                      // apply CF to use ACC for height estimation
 } barometerConfig_t;
 
-typedef enum {
-    BAROMETER_ACTION_NOT_READY = 0,
-    BAROMETER_ACTION_OBTAINED_SAMPLES,
-    BAROMETER_ACTION_PERFORMED_CALCULATION
-} barometerAction_e;
-
 extern int32_t BaroAlt;
 
 #ifdef BARO
 void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
 bool isBaroCalibrationComplete(void);
 void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
-barometerAction_e baroUpdate(uint32_t currentTime);
+void baroUpdate(uint32_t currentTime);
+bool isBaroReady(void);
 int32_t baroCalculateAltitude(void);
 void performBaroCalibrationCycle(void);
 #endif
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 864e94ba04..fb36fa77e0 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -50,23 +50,25 @@ void compassInit(void)
     magInit = 1;
 }
 
-int compassGetADC(flightDynamicsTrims_t *magZero)
+#define COMPASS_UPDATE_FREQUENCY_10HZ (1000 * 100)
+
+void updateCompass(flightDynamicsTrims_t *magZero)
 {
-    static uint32_t t, tCal = 0;
+    static uint32_t nextUpdateAt, tCal = 0;
     static flightDynamicsTrims_t magZeroTempMin;
     static flightDynamicsTrims_t magZeroTempMax;
     uint32_t axis;
 
-    if ((int32_t)(currentTime - t) < 0)
-        return 0;                 //each read is spaced by 100ms
-    t = currentTime + 100000;
+    if ((int32_t)(currentTime - nextUpdateAt) < 0)
+        return;
+
+    nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ;
 
-    // Read mag sensor
     hmc5883lRead(magADC);
     alignSensors(magADC, magADC, magAlign);
 
     if (f.CALIBRATE_MAG) {
-        tCal = t;
+        tCal = nextUpdateAt;
         for (axis = 0; axis < 3; axis++) {
             magZero->raw[axis] = 0;
             magZeroTempMin.raw[axis] = magADC[axis];
@@ -82,7 +84,7 @@ int compassGetADC(flightDynamicsTrims_t *magZero)
     }
 
     if (tCal != 0) {
-        if ((t - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
+        if ((nextUpdateAt - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
             LED0_TOGGLE;
             for (axis = 0; axis < 3; axis++) {
                 if (magADC[axis] < magZeroTempMin.raw[axis])
@@ -99,7 +101,5 @@ int compassGetADC(flightDynamicsTrims_t *magZero)
             saveAndReloadCurrentProfileToCurrentProfileSlot();
         }
     }
-
-    return 1;
 }
 #endif
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 2f4d95d836..eb1540aa4f 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -19,7 +19,7 @@
 
 #ifdef MAG
 void compassInit(void);
-int compassGetADC(flightDynamicsTrims_t *magZero);
+void updateCompass(flightDynamicsTrims_t *magZero);
 #endif
 
 extern int16_t magADC[XYZ_AXIS_COUNT];