Merge pull request #11439 from SteveCEvans/compass_timing

Perform compass reads in the background

src/main/drivers/compass/compass_ak8975.c

@@ -118,39 +118,63 @@ static int16_t parseMag(uint8_t *raw, int16_t gain) {
 
 static bool ak8975Read(magDev_t *mag, int16_t *magData)
 {
-    bool ack;
+    static uint8_t buf[6];
-    uint8_t status;
+    static uint8_t status;
-    uint8_t buf[6];
+    static enum {
+        STATE_READ_STATUS1,
+        STATE_WAIT_STATUS1,
+        STATE_READ_STATUS2,
+        STATE_WAIT_STATUS2,
+        STATE_WAIT_START,
+    } state = STATE_READ_STATUS1;
 
     extDevice_t *dev = &mag->dev;
 
-    ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1);
+    switch (state) {
-    if (!ack || (status & ST1_REG_DATA_READY) == 0) {
+        default:
-        return false;
+        case STATE_READ_STATUS1:
+            busReadRegisterBufferStart(dev, AK8975_MAG_REG_ST1, &status, sizeof(status));
+            state = STATE_WAIT_STATUS1;
+            return false;
+
+        case STATE_WAIT_STATUS1:
+            if ((status & ST1_REG_DATA_READY) == 0) {
+                state = STATE_READ_STATUS1;
+                return false;
+            }
+
+            busReadRegisterBufferStart(dev, AK8975_MAG_REG_HXL, buf, sizeof(buf));
+
+            state = STATE_READ_STATUS2;
+            return false;
+
+        case STATE_READ_STATUS2:
+            busReadRegisterBufferStart(dev, AK8975_MAG_REG_ST2, &status, sizeof(status));
+            state = STATE_WAIT_STATUS2;
+            return false;
+
+        case STATE_WAIT_STATUS2:
+            busWriteRegisterStart(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again
+
+            if ((status & ST2_REG_DATA_ERROR) || (status & ST2_REG_MAG_SENSOR_OVERFLOW)) {
+                state = STATE_READ_STATUS1;
+                return false;
+            }
+
+            state = STATE_WAIT_START;
+            return false;
+
+        case STATE_WAIT_START:
+
+            magData[X] = -parseMag(buf + 0, mag->magGain[X]);
+            magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
+            magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
+
+            state = STATE_READ_STATUS1;
+            return true;
     }
 
-    busReadRegisterBuffer(dev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
+    return false;
-
-    ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1);
-    if (!ack) {
-        return false;
-    }
-
-    busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again    uint8_t status2 = buf[6];
-
-    if (status & ST2_REG_DATA_ERROR) {
-        return false;
-    }
-
-    if (status & ST2_REG_MAG_SENSOR_OVERFLOW) {
-        return false;
-    }
-
-    magData[X] = -parseMag(buf + 0, mag->magGain[X]);
-    magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
-    magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
-
-    return true;
 }
 
 bool ak8975Detect(magDev_t *mag)

src/main/drivers/compass/compass_hmc5883l.c

@@ -200,13 +200,15 @@ static void hmc5883SpiInit(const extDevice_t *dev)
 
 static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
 {
-    uint8_t buf[6];
+    static uint8_t buf[6];
+    static bool pendingRead = true;
 
     extDevice_t *dev = &mag->dev;
 
-    bool ack = busReadRegisterBuffer(dev, HMC58X3_REG_DATA, buf, 6);
+    if (pendingRead) {
+        busReadRegisterBufferStart(dev, HMC58X3_REG_DATA, buf, sizeof(buf));
 
-    if (!ack) {
+        pendingRead = false;
         return false;
     }
 
@@ -214,6 +216,8 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
     magData[Z] = (int16_t)(buf[2] << 8 | buf[3]);
     magData[Y] = (int16_t)(buf[4] << 8 | buf[5]);
 
+    pendingRead = true;
+
     return true;
 }
 

src/main/drivers/compass/compass_lis3mdl.c

@@ -103,13 +103,15 @@
 
 static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
 {
-    uint8_t buf[6];
+    static uint8_t buf[6];
+    static bool pendingRead = true;
 
     extDevice_t *dev = &mag->dev;
 
-    bool ack = busReadRegisterBuffer(dev, LIS3MDL_REG_OUT_X_L, buf, 6);
+    if (pendingRead) {
+        busReadRegisterBufferStart(dev, LIS3MDL_REG_OUT_X_L, buf, sizeof(buf));
 
-    if (!ack) {
+        pendingRead = false;
         return false;
     }
 
@@ -117,6 +119,8 @@ static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
     magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) / 4;
     magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) / 4;
 
+    pendingRead = true;
+
     return true;
 }
 

src/main/drivers/compass/compass_qmc5883l.c

@@ -91,32 +91,45 @@ static bool qmc5883lInit(magDev_t *magDev)
 
 static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
 {
-    uint8_t status;
+    static uint8_t buf[6];
-    uint8_t buf[6];
+    static uint8_t status;
-
+    static enum {
-    // set magData to zero for case of failed read
+        STATE_READ_STATUS,
-    magData[X] = 0;
+        STATE_WAIT_STATUS,
-    magData[Y] = 0;
+        STATE_WAIT_READ,
-    magData[Z] = 0;
+    } state = STATE_READ_STATUS;
 
     extDevice_t *dev = &magDev->dev;
 
-    bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_STATUS, &status, 1);
+    switch (state) {
+        default:
+        case STATE_READ_STATUS:
+            busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
+            state = STATE_WAIT_STATUS;
+            return false;
 
-    if (!ack || (status & 0x04) == 0) {
+        case STATE_WAIT_STATUS:
-        return false;
+            if ((status & 0x04) == 0) {
+                state = STATE_READ_STATUS;
+                return false;
+            }
+
+            busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf));
+            state = STATE_WAIT_READ;
+            return false;
+
+        case STATE_WAIT_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;
+
+            return true;
     }
 
-    ack = busReadRegisterBuffer(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
+    return false;
-    if (!ack) {
-        return false;
-    }
-
-    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]);
-
-    return true;
 }
 
 bool qmc5883lDetect(magDev_t *magDev)

src/main/fc/tasks.c

@@ -259,6 +259,20 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef USE_MAG
+static void taskUpdateMag(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (sensors(SENSOR_MAG)) {
+        const uint32_t newDeadline = compassUpdate(currentTimeUs);
+        if (newDeadline != 0) {
+            rescheduleTask(TASK_SELF, newDeadline);
+        }
+    }
+}
+#endif
+
 #if defined(USE_RANGEFINDER)
 void taskUpdateRangefinder(timeUs_t currentTimeUs)
 {
@@ -351,7 +365,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #endif
 
 #ifdef USE_MAG
-    [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, compassUpdate,TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
+    [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
 #endif
 
 #ifdef USE_BARO

src/main/sensors/compass.c

@@ -52,6 +52,8 @@
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 #include "sensors/gyro_init.h"
@@ -347,10 +349,14 @@ bool compassIsCalibrationComplete(void)
     return tCal == 0;
 }
 
-void compassUpdate(timeUs_t currentTimeUs)
+uint32_t compassUpdate(timeUs_t currentTimeUs)
 {
+    if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
+        // No action was taken as the read has not completed
+        schedulerIgnoreTaskExecRate();
+        return 1000; // Wait 1ms between states
+    }
 
-    magDev.read(&magDev, magADCRaw);
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         mag.magADC[axis] = magADCRaw[axis];
     }
@@ -385,5 +391,7 @@ void compassUpdate(timeUs_t currentTimeUs)
             saveConfigAndNotify();
         }
     }
+
+    return TASK_PERIOD_HZ(10);
 }
 #endif // USE_MAG

src/main/sensors/compass.h

@@ -62,7 +62,7 @@ typedef struct compassConfig_s {
 PG_DECLARE(compassConfig_t, compassConfig);
 
 bool compassIsHealthy(void);
-void compassUpdate(timeUs_t currentTime);
+uint32_t compassUpdate(timeUs_t currentTime);
 bool compassInit(void);
 void compassPreInit(void);
 void compassStartCalibration(void);