Move mw loop() code into separate methods. Cleanup related code.

If a given feature or mode is off the next task is not processed in the
current loop but will be processed during the next loop iteration for
simplification, this allowed the cleanup of return values in other code.

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;
+    static uint32_t previousTime;
-    uint32_t currentT = micros();
     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;
+    dTime = currentTime - previousTime;
-    if (dTime < UPDATE_INTERVAL)
+    if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
-        return 0;
+        return;
-    previousT = currentT;
+        
+    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 */

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);

src/main/mw.c

@@ -349,15 +349,92 @@ 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;
 
-    updateRx();
-
-    if (shouldProcessRx(currentTime)) {
     calculateRxChannelsAndUpdateFailsafe(currentTime);
 
     // in 3D mode, we need to be able to disarm by switch at any time
@@ -480,44 +557,19 @@ void loop(void)
     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) {
+void loop(void)
-        case 0:
+{
-            taskOrder++;
+    static uint32_t loopTime;
-#ifdef MAG
+
-            if (sensors(SENSOR_MAG) && compassGetADC(&masterConfig.magZero))
+    updateRx();
-                break;
+
-#endif
+    if (shouldProcessRx(currentTime)) {
-        case 1:
+    	processRx();
-            taskOrder++;
+    } else {
-#ifdef BARO
+    	// not in rc loop
-            if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY)
+    	executePeriodicTasks();
-                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
-        }
     }
 
     currentTime = micros();
@@ -537,17 +589,7 @@ void loop(void)
 
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {
-            if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
+        	updateMagHold();
-                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;
         }
 #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();

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;
     }
 }
 

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

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)
+    if ((int32_t)(currentTime - nextUpdateAt) < 0)
-        return 0;                 //each read is spaced by 100ms
+        return;
-    t = currentTime + 100000;
+
+    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

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];