Mag scheduling improvements (#13166)

* Improved QMC driver, scheduled silent interval

* add ODR to drivers and use it to set the quiet period

* add MAG_TASK_RATE debug for testing

* don't do a read if the bus is busy - thanks, Petr

* refactoring and simplification, thanks to Steve

---------

Co-authored-by: Steve Evans <SteveCEvans@users.noreply.github.com>

src/main/build/debug.c

@@ -117,4 +117,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RPM_LIMIT",
     "RC_STATS",
     "MAG_CALIB",
+    "MAG_TASK_RATE",
 };

src/main/build/debug.h

@@ -117,6 +117,7 @@ typedef enum {
     DEBUG_RPM_LIMIT,
     DEBUG_RC_STATS,
     DEBUG_MAG_CALIB,
+    DEBUG_MAG_TASK_RATE,
     DEBUG_COUNT
 } debugType_e;
 

src/main/drivers/compass/compass.h

@@ -36,4 +36,5 @@ typedef struct magDev_s {
     fp_rotationMatrix_t rotationMatrix;
     ioTag_t magIntExtiTag;
     int16_t magGain[3];
+    uint16_t magOdrHz;
 } magDev_t;

src/main/drivers/compass/compass_ak8963.c

@@ -368,6 +368,7 @@ static bool ak8963Init(magDev_t *mag)
 
     // Trigger first measurement
     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE);
+    mag->magOdrHz = 50; // arbitrary value, need to check what ODR is actually returned
     return true;
 }
 
@@ -391,7 +392,6 @@ void ak8963BusInit(const extDevice_t *dev)
 
 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
     case BUS_TYPE_MPU_SLAVE:
-        rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
 
         // Disable DMA on gyro as this upsets slave access timing
         spiDmaEnable(dev->bus->busType_u.mpuSlave.master, false);

src/main/drivers/compass/compass_ak8975.c

@@ -108,6 +108,7 @@ static bool ak8975Init(magDev_t *mag)
 
     // Trigger first measurement
     busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
+    mag->magOdrHz = 50; // arbitrary value, need to check what ODR is actually returned
     return true;
 }
 

src/main/drivers/compass/compass_hmc5883l.c

@@ -235,6 +235,7 @@ static bool hmc5883lInit(magDev_t *mag)
     delay(100);
 
     hmc5883lConfigureDataReadyInterruptHandling(mag);
+    mag->magOdrHz = 75; // HMC_CONFA_DOR_75HZ
     return true;
 }
 

src/main/drivers/compass/compass_ist8310.c

@@ -127,6 +127,8 @@ static bool ist8310Init(magDev_t *magDev)
     delay(6); 
     ack = ack && busWriteRegister(dev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE);
 
+    magDev->magOdrHz = 100;
+    // need to check what ODR is actually returned, may be a bit faster than 100Hz
     return ack;
 }
 

src/main/drivers/compass/compass_lis3mdl.c

@@ -137,7 +137,7 @@ static bool lis3mdlInit(magDev_t *mag)
     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG3, 0x00);
 
     delay(100);
-
+    mag->magOdrHz = 80; // LIS3MDL_DO_80
     return true;
 }
 

src/main/drivers/compass/compass_qmc5883l.c

@@ -68,6 +68,7 @@
 
 #define QMC5883L_REG_DATA_OUTPUT_X 0x00
 #define QMC5883L_REG_STATUS 0x06
+#define QMC5883L_REG_STATUS_DRDY 0x01
 
 #define QMC5883L_REG_ID 0x0D
 #define QMC5883_ID_VAL 0xFF
@@ -86,45 +87,43 @@ static bool qmc5883lInit(magDev_t *magDev)
         return false;
     }
 
+    magDev->magOdrHz = 200; // QMC5883L_ODR_200HZ
     return true;
 }
 
 static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
 {
     static uint8_t buf[6];
-    static uint8_t status;
+    static uint8_t status = 0;
     static enum {
-        STATE_READ_STATUS,
-        STATE_WAIT_STATUS,
-        STATE_WAIT_READ,
-    } state = STATE_READ_STATUS;
+        STATE_WAIT_DRDY,
+        STATE_READ,
+    } state = STATE_WAIT_DRDY;
 
     extDevice_t *dev = &magDev->dev;
 
     switch (state) {
         default:
-        case STATE_READ_STATUS:
-            busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
-            state = STATE_WAIT_STATUS;
-            return false;
-
-        case STATE_WAIT_STATUS:
-            if ((status & 0x01) == 0) {
-                state = STATE_READ_STATUS;
-                return false;
+        case STATE_WAIT_DRDY:
+            if (status & QMC5883L_REG_STATUS_DRDY) {
+                // New data is available
+                busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf));
+                state = STATE_READ;
+            } else {
+                // Read status register to check for data ready
+                busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
             }
-
-            busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf));
-            state = STATE_WAIT_READ;
             return false;
 
-        case STATE_WAIT_READ:
-
+        case STATE_READ:
             magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
             magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
             magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);
 
-            state = STATE_READ_STATUS;
+            state = STATE_WAIT_DRDY;
+
+            // Indicate that new data is required
+            status = 0;
 
             return true;
     }

src/main/fc/tasks.c

@@ -31,7 +31,6 @@
 #include "cms/cms.h"
 
 #include "common/color.h"
-#include "common/time.h"
 #include "common/utils.h"
 
 #include "config/feature.h"

src/main/sensors/compass.c

@@ -88,7 +88,12 @@ mag_t mag;
 
 PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
 
-#define COMPASS_READ_US 500 // Allow 500us for compass data read
+// If the i2c bus is busy, try again in 500us
+#define COMPASS_BUS_BUSY_INTERVAL_US 500
+// If we check for new mag data, and there is none, try again in 1000us
+#define COMPASS_RECHECK_INTERVAL_US 1000
+// default compass read interval, for those with no specified ODR, will be TASK_COMPASS_RATE_HZ 
+static uint32_t compassReadIntervalUs = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ);
 
 void pgResetFn_compassConfig(compassConfig_t *compassConfig)
 {
@@ -136,7 +141,6 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
 }
 
 static int16_t magADCRaw[XYZ_AXIS_COUNT];
-static int16_t magADCRawPrevious[XYZ_AXIS_COUNT];
 
 void compassPreInit(void)
 {
@@ -364,7 +368,15 @@ bool compassInit(void)
 
     buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);
 
-    compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0);
+    if (magDev.magOdrHz) {
+        // For Mags that send data at a fixed ODR, we wait some quiet period after a read before checking for new data
+        // allow two re-check intervals, plus a margin for clock variations in mag vs FC
+        uint16_t odrInterval = 1e6 / magDev.magOdrHz;
+        compassReadIntervalUs =  odrInterval - (2 * COMPASS_RECHECK_INTERVAL_US) - (odrInterval / 20);
+    } else {
+        // Mags which have no specified ODR will be pinged at the compass task rate
+        compassReadIntervalUs = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ);
+    }
 
     return true;
 }
@@ -395,25 +407,33 @@ bool compassIsCalibrationComplete(void)
 
 uint32_t compassUpdate(timeUs_t currentTimeUs)
 {
-    static uint8_t busyCount = 0;
-    uint32_t nextPeriod = COMPASS_READ_US;
+    static timeUs_t previousTaskTimeUs = 0;
+    const timeDelta_t dTaskTimeUs = cmpTimeUs(currentTimeUs, previousTaskTimeUs);
+    previousTaskTimeUs = currentTimeUs;
+    DEBUG_SET(DEBUG_MAG_TASK_RATE, 6, dTaskTimeUs);
 
-    if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
-        // No action was taken as the read has not completed
-        schedulerIgnoreTaskStateTime();
-
-        busyCount++;
-
-        return nextPeriod; // Wait COMPASS_READ_US between states
+    bool checkBusBusy = busBusy(&magDev.dev, NULL);
+    DEBUG_SET(DEBUG_MAG_TASK_RATE, 4, checkBusBusy);
+    if (checkBusBusy) {
+        // No action is taken, as the bus was busy.
+        schedulerIgnoreTaskExecRate();
+        return COMPASS_BUS_BUSY_INTERVAL_US; // come back in 500us, maybe the bus won't be busy then
     }
 
+    bool checkReadState = !magDev.read(&magDev, magADCRaw);
+    DEBUG_SET(DEBUG_MAG_TASK_RATE, 5, checkReadState);
+    if (checkReadState) {
+        // The compass reported no data available to be retrieved; it may use a state engine that has more than one read state
+        schedulerIgnoreTaskExecRate();
+        return COMPASS_RECHECK_INTERVAL_US; // come back in 1ms, maybe data will be available then
+    }
+
+    // if we get here, we have new data to parse
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        if (magADCRaw[axis] != magADCRawPrevious[axis]) {
-            // this test, and the isNewMagADCFlag itself, is only needed if we calculate magYaw in imu.c
-            mag.isNewMagADCFlag = true;
-        }
         mag.magADC[axis] = magADCRaw[axis];
     }
+    // If debug_mode is DEBUG_GPS_RESCUE_HEADING, we should update the magYaw value, after which isNewMagADCFlag will be set false
+    mag.isNewMagADCFlag = true;
 
     if (magDev.magAlignment == ALIGN_CUSTOM) {
         alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
@@ -476,11 +496,21 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
         DEBUG_SET(DEBUG_MAG_CALIB, 7, lrintf((compassBiasEstimator.lambda - compassBiasEstimator.lambda_min) * mapLambdaGain));
     }
 
-    nextPeriod = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ) - COMPASS_READ_US * busyCount;
+    if (debugMode == DEBUG_MAG_TASK_RATE) {
+        static timeUs_t previousTimeUs = 0;
+        const timeDelta_t dataIntervalUs = cmpTimeUs(currentTimeUs, previousTimeUs); // time since last data received
+        previousTimeUs = currentTimeUs;
+        const uint16_t actualCompassDataRateHz = 1e6 / dataIntervalUs;
+        timeDelta_t executeTimeUs = micros() - currentTimeUs;
+        DEBUG_SET(DEBUG_MAG_TASK_RATE, 0, TASK_COMPASS_RATE_HZ);
+        DEBUG_SET(DEBUG_MAG_TASK_RATE, 1, actualCompassDataRateHz);
+        DEBUG_SET(DEBUG_MAG_TASK_RATE, 2, dataIntervalUs);
+        DEBUG_SET(DEBUG_MAG_TASK_RATE, 3, executeTimeUs); // time in uS to complete the mag task
+    }
 
-    // Reset the busy count
-    busyCount = 0;
-    return nextPeriod;
+    // don't do the next read check until compassReadIntervalUs has expired
+    schedulerIgnoreTaskExecRate();
+    return compassReadIntervalUs;
 }
 
 // initialize the compass bias estimator

src/main/sensors/compass.h

@@ -27,7 +27,7 @@
 #include "pg/pg.h"
 #include "sensors/sensors.h"
 
-#define TASK_COMPASS_RATE_HZ 300
+#define TASK_COMPASS_RATE_HZ 40 // the base mag update rate; faster intervals will apply for higher ODR mags
 
 // Type of magnetometer used/detected
 typedef enum {