From 8ac81f86b20574976c9933aac11e1e3a0c519b4d Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sun, 27 Feb 2022 13:56:32 +0000 Subject: [PATCH] Perform compass reads in the background --- src/main/drivers/compass/compass_ak8975.c | 80 +++++++++++++-------- src/main/drivers/compass/compass_hmc5883l.c | 10 ++- src/main/drivers/compass/compass_lis3mdl.c | 10 ++- src/main/drivers/compass/compass_qmc5883l.c | 53 ++++++++------ src/main/fc/tasks.c | 16 ++++- src/main/sensors/compass.c | 12 +++- src/main/sensors/compass.h | 2 +- 7 files changed, 125 insertions(+), 58 deletions(-) diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index 137a63f106..0348f19e2a 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/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; - uint8_t status; - uint8_t buf[6]; + static uint8_t buf[6]; + static uint8_t status; + 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); - if (!ack || (status & ST1_REG_DATA_READY) == 0) { - return false; + switch (state) { + default: + 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 - - 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; + return false; } bool ak8975Detect(magDev_t *mag) diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index ed475de7b6..f31c60f14e 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/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; } diff --git a/src/main/drivers/compass/compass_lis3mdl.c b/src/main/drivers/compass/compass_lis3mdl.c index 5be0a9457e..b9c5e62ed5 100644 --- a/src/main/drivers/compass/compass_lis3mdl.c +++ b/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; } diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index 73df4bae69..79cdb8080c 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/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; - uint8_t buf[6]; - - // set magData to zero for case of failed read - magData[X] = 0; - magData[Y] = 0; - magData[Z] = 0; + static uint8_t buf[6]; + static uint8_t status; + static enum { + STATE_READ_STATUS, + STATE_WAIT_STATUS, + STATE_WAIT_READ, + } 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) { - return false; + case STATE_WAIT_STATUS: + 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); - 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; + return false; } bool qmc5883lDetect(magDev_t *magDev) diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index f3bcd62ce5..7657ffb79b 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -264,6 +264,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) { @@ -356,7 +370,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 diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 5f077c9bfc..6d7de3fbac 100644 --- a/src/main/sensors/compass.c +++ b/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 diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 31da302042..f040c03b54 100644 --- a/src/main/sensors/compass.h +++ b/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);