From a8834ad14ba59dfbc6cd1d37c01ff86cf5457100 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Tue, 28 Nov 2023 11:55:01 +1100 Subject: [PATCH] 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 --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/drivers/compass/compass.h | 1 + src/main/drivers/compass/compass_ak8963.c | 2 +- src/main/drivers/compass/compass_ak8975.c | 1 + src/main/drivers/compass/compass_hmc5883l.c | 1 + src/main/drivers/compass/compass_ist8310.c | 2 + src/main/drivers/compass/compass_lis3mdl.c | 2 +- src/main/drivers/compass/compass_qmc5883l.c | 39 ++++++------ src/main/fc/tasks.c | 1 - src/main/sensors/compass.c | 70 +++++++++++++++------ src/main/sensors/compass.h | 2 +- 12 files changed, 79 insertions(+), 44 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index b0ff3996ba..9b5594a0a1 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -117,4 +117,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RPM_LIMIT", "RC_STATS", "MAG_CALIB", + "MAG_TASK_RATE", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 22605aace3..21a7e52cd5 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index ffab6dde4e..c20fe9ecd2 100644 --- a/src/main/drivers/compass/compass.h +++ b/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; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 1c505b1c90..bbe60b559a 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/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); diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index 492bc496a0..c539636bdd 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/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; } diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index 1614208870..cc74fac721 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/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; } diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c index 9f716b9fbd..ede7698586 100644 --- a/src/main/drivers/compass/compass_ist8310.c +++ b/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; } diff --git a/src/main/drivers/compass/compass_lis3mdl.c b/src/main/drivers/compass/compass_lis3mdl.c index b9c5e62ed5..b89a12cf80 100644 --- a/src/main/drivers/compass/compass_lis3mdl.c +++ b/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; } diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index 6f827cd4c0..6073e087b2 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/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; } diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index d3924036a6..096a332ba7 100644 --- a/src/main/fc/tasks.c +++ b/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" diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index e4ab44ceff..9e4b09be56 100644 --- a/src/main/sensors/compass.c +++ b/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 diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 84f80f46c9..408cb76bcf 100644 --- a/src/main/sensors/compass.h +++ b/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 {