From 7ba22cacf5c5623a875949a48f6ae9a82260c417 Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Wed, 21 Dec 2022 20:13:18 +0800 Subject: [PATCH 1/3] update basic WR of BMI270 --- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 36 +++++++++++-------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 72608874c..f36956819 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -165,6 +165,17 @@ static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e regis } } +static uint16_t bmi270RegisterRead16(const extDevice_t *dev, bmi270Register_e registerId) +{ + uint8_t data[3] = { 0, 0, 0 }; + + if (spiReadRegMskBufRB(dev, registerId, data, 3)) { + return (uint16_t)( (data[2]<<8) | data[1] ); // LSB first since address is auto-incremented + } else { + return 0; + } +} + static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs) { spiWriteReg(dev, registerId, value); @@ -175,19 +186,17 @@ static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registe static void bmi270RegisterWriteBits(const extDevice_t *dev, bmi270Register_e registerID, bmi270ConfigMasks_e mask, uint8_t value, unsigned delayMs) { - uint8_t newValue; - if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) { - delayMicroseconds(2); - newValue = (newValue & ~mask) | value; - bmi270RegisterWrite(dev, registerID, newValue, delayMs); - } + uint8_t newValue = bmi270RegisterRead(dev, registerID); + delayMicroseconds(2); + newValue = (newValue & ~mask) | value; + bmi270RegisterWrite(dev, registerID, newValue, delayMs); } static void bmi270RegisterWrite16(const extDevice_t *dev, bmi270Register_e registerId, uint16_t data, unsigned delayMs) { uint8_t buf[2] = { - (uint8_t)(data >> 8), - (uint8_t)(data & 0x00FF) + (uint8_t)(data & 0x00FF), // LSB first since address is auto-incremented + (uint8_t)(data >> 8) }; spiWriteRegBuf(dev, registerId, buf, 2); if (delayMs) { @@ -197,13 +206,10 @@ static void bmi270RegisterWrite16(const extDevice_t *dev, bmi270Register_e regis static void bmi270RegisterWriteBits16(const extDevice_t *dev, bmi270Register_e registerID, bmi270ConfigMasks_e mask, uint16_t value, unsigned delayMs) { - uint8_t data[2] = {0}; - if (busReadRegisterBuffer(dev, registerID, data, 2)) { - delayMicroseconds(2); - uint16_t newValue = (data[0] << 8) | data[1]; - newValue = (newValue & ~mask) | value; - bmi270RegisterWrite16(dev, registerID, newValue, delayMs); - } + uint16_t newValue = bmi270RegisterRead16(dev, registerID); + delayMicroseconds(2); + newValue = (newValue & ~mask) | value; + bmi270RegisterWrite16(dev, registerID, newValue, delayMs); } static int16_t bmi270GetGyroCas(uint8_t gyroCasRaw) From 73543b4b944a6a5c3bcf3c365d247048b81c49a3 Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Thu, 22 Dec 2022 22:38:54 +0800 Subject: [PATCH 2/3] embedd cas reading into spi dma segment --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/drivers/accgyro/accgyro.h | 2 +- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 117 ++++++++++++------ 4 files changed, 80 insertions(+), 41 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 1d0ddd692..b0f09cec4 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -101,4 +101,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RX_EXPRESSLRS_PHASELOCK", "RX_STATE_TIME", "SMITH_PREDICTOR", + // "BMI270_GYRO", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index cf6aea15b..7379bb748 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -99,6 +99,7 @@ typedef enum { DEBUG_RX_EXPRESSLRS_PHASELOCK, DEBUG_RX_STATE_TIME, DEBUG_SMITH_PREDICTOR, + // DEBUG_BMI270_GYRO, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index ec4cf9ce2..bbade4603 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -118,7 +118,7 @@ typedef struct gyroDev_s { uint32_t gyroSyncEXTI; int32_t gyroShortPeriod; int32_t gyroDmaMaxDuration; - busSegment_t segments[2]; + busSegment_t segments[3]; #endif volatile bool dataReady; bool gyro_high_fsr; diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index f36956819..61168c61b 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -39,6 +39,7 @@ #include "drivers/time.h" #include "sensors/gyro.h" +// #include "build/debug.h" // 10 MHz max SPI frequency #define BMI270_MAX_SPI_CLK_HZ 10000000 @@ -212,16 +213,6 @@ static void bmi270RegisterWriteBits16(const extDevice_t *dev, bmi270Register_e r bmi270RegisterWrite16(dev, registerID, newValue, delayMs); } -static int16_t bmi270GetGyroCas(uint8_t gyroCasRaw) -{ - if (gyroCasRaw & BMI270_GYRO_CAS_SIGN_BIT_MASK) { - return (int16_t)(((int16_t)gyroCasRaw) - 128); - } else { - return (int16_t)(gyroCasRaw); - } - return 0; -} - // Toggle the CS to switch the device into SPI mode. // Device switches initializes as I2C and switches to SPI on a low to high CS transition static void bmi270EnableSPI(const extDevice_t *dev) @@ -362,8 +353,15 @@ extiCallbackRec_t bmi270IntCallbackRec; */ #ifdef USE_GYRO_EXTI // Called in ISR context -// Gyro read has just completed +// Gyro read has just completed, next is CAS read busStatus_e bmi270Intcallback(uint32_t arg) +{ + UNUSED(arg); + return BUS_READY; +} + +// CAS read has just completed +busStatus_e bmi270IntCAScallback(uint32_t arg) { gyroDev_t *gyro = (gyroDev_t *)arg; int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI); @@ -460,6 +458,36 @@ static bool bmi270AccRead(accDev_t *acc) return true; } +static int8_t bmi270ProcessGyroCas(uint8_t raw) +{ + int8_t result = 0; + raw = raw & BMI270_GYRO_CAS_MASK; + if ((raw & BMI270_GYRO_CAS_SIGN_BIT_MASK) == 0) { + result = (int8_t)(raw); + } else { + result = (int8_t)(raw - 128); + } + return result; +} + +static uint8_t bmi270ReadGyroCas(gyroDev_t *gyro) +{ + // GYRO_CAS rading + uint8_t casTxBuf[4] = {BMI270_REG_FEATURES_0_GYR_CAS | 0x80, 0, 0, 0}; + uint8_t casRxBuf[4] = {0, 0, 0, 0}; + busSegment_t segmentsCas[] = { + {.u.buffers = {casTxBuf, casRxBuf}, 4, true, NULL}, + {.u.link = {NULL, NULL}, 0, true, NULL}, + }; + + spiSequence(&gyro->dev, &segmentsCas[0]); + // Wait for completion + spiWait(&gyro->dev); + + uint8_t gyroCasRaw = segmentsCas[0].u.buffers.rxData[2]; + return bmi270ProcessGyroCas(gyroCasRaw); +} + static bool bmi270GyroReadRegister(gyroDev_t *gyro) { uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; @@ -477,12 +505,20 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro) if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { if (spiUseDMA(&gyro->dev)) { gyro->dev.callbackArg = (uint32_t)gyro; + // first segments is for acc+gyro data gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; gyro->segments[0].len = 14; gyro->segments[0].callback = bmi270Intcallback; gyro->segments[0].u.buffers.txData = gyro->dev.txBuf; gyro->segments[0].u.buffers.rxData = gyro->dev.rxBuf; gyro->segments[0].negateCS = true; + // second segment is for gyro CAS factor + gyro->dev.txBuf[14] = BMI270_REG_FEATURES_0_GYR_CAS | 0x80; + gyro->segments[1].len = 4; + gyro->segments[1].callback = bmi270IntCAScallback; + gyro->segments[1].u.buffers.txData = &gyro->dev.txBuf[14]; + gyro->segments[1].u.buffers.rxData = &gyro->dev.rxBuf[14]; + gyro->segments[1].negateCS = true; gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; } else { // Interrupts are present, but no DMA @@ -513,46 +549,47 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro) // Wait for completion spiWait(&gyro->dev); - // GYRO_CAS rading - gyro->dev.txBuf[8] = BMI270_REG_FEATURES_0_GYR_CAS | 0x80; - busSegment_t segmentsCas[] = { - {.u.buffers = {NULL, NULL}, 4, true, NULL}, - {.u.link = {NULL, NULL}, 0, true, NULL}, - }; - segmentsCas[0].u.buffers.txData = &gyro->dev.txBuf[8]; - segmentsCas[0].u.buffers.rxData = &gyro->dev.rxBuf[8]; + int8_t cas = bmi270ReadGyroCas(gyro); - spiSequence(&gyro->dev, &segmentsCas[0]); - // Wait for completion - spiWait(&gyro->dev); - - int16_t cas = bmi270GetGyroCas(segmentsCas[0].u.buffers.rxData[3] & BMI270_GYRO_CAS_MASK); - gyro->gyroADCRaw[X] = gyroData[1] - (cas * gyroData[3] / 512); + // only x axis need overflow check + int32_t tempx = gyroData[1] - (int16_t)(cas * (int16_t)(gyroData[3]) / 512); + if (tempx > 32767) { + gyro->gyroADCRaw[X] = 32767; + } else if (tempx < -32768) { + gyro->gyroADCRaw[X] = -32768; + } else { + gyro->gyroADCRaw[X] = tempx; + } gyro->gyroADCRaw[Y] = gyroData[2]; gyro->gyroADCRaw[Z] = gyroData[3]; + // DEBUG_SET(DEBUG_BMI270_GYRO, 0, lrintf(gyroData[1])); + // DEBUG_SET(DEBUG_BMI270_GYRO, 1, lrintf(gyro->gyroADCRaw[X])); + // DEBUG_SET(DEBUG_BMI270_GYRO, 2, lrintf(gyroData[3])); + // DEBUG_SET(DEBUG_BMI270_GYRO, 3, lrintf((int16_t)(cas * (int16_t)(gyroData[3]) / 512))); + break; } case GYRO_EXTI_INT_DMA: { - // GYRO_CAS rading - gyro->dev.txBuf[14] = BMI270_REG_FEATURES_0_GYR_CAS | 0x80; - busSegment_t segmentsCas[] = { - {.u.buffers = {NULL, NULL}, 4, true, NULL}, - {.u.link = {NULL, NULL}, 0, true, NULL}, - }; - segmentsCas[0].u.buffers.txData = &gyro->dev.txBuf[14]; - segmentsCas[0].u.buffers.rxData = &gyro->dev.rxBuf[14]; - - spiSequence(&gyro->dev, &segmentsCas[0]); - // Wait for completion - spiWait(&gyro->dev); - - int16_t cas = bmi270GetGyroCas(segmentsCas[0].u.buffers.rxData[3] & BMI270_GYRO_CAS_MASK); - gyro->gyroADCRaw[X] = gyroData[4] - (cas * gyroData[6] / 512); + int8_t cas = bmi270ProcessGyroCas((uint8_t)(gyroData[8])); + // only x axis need overflow check + int32_t tempx = gyroData[4] - (int16_t)(cas * (int16_t)(gyroData[6]) / 512); + if (tempx > 32767) { + gyro->gyroADCRaw[X] = 32767; + } else if (tempx < -32768) { + gyro->gyroADCRaw[X] = -32768; + } else { + gyro->gyroADCRaw[X] = tempx; + } gyro->gyroADCRaw[Y] = gyroData[5]; gyro->gyroADCRaw[Z] = gyroData[6]; + + // DEBUG_SET(DEBUG_BMI270_GYRO, 0, lrintf(gyroData[4])); + // DEBUG_SET(DEBUG_BMI270_GYRO, 1, lrintf(gyroData[6])); + // DEBUG_SET(DEBUG_BMI270_GYRO, 2, lrintf(gyro->gyroADCRaw[X])); + // DEBUG_SET(DEBUG_BMI270_GYRO, 3, lrintf((int16_t)cas)); break; } From 25bb8b09a429745b6ada96af2424ba1b419470b0 Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Sat, 24 Dec 2022 19:37:18 +0800 Subject: [PATCH 3/3] gyroData is int16 --- src/main/drivers/accgyro/accgyro_mpu.c | 4 ++-- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 6 +++--- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 4 ++-- src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c | 6 +++--- src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c | 6 +++--- src/main/drivers/accgyro/accgyro_spi_lsm6dso.c | 6 +++--- 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 1bcbe9f50..3a6688b0f 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -249,7 +249,7 @@ bool mpuAccReadSPI(accDev_t *acc) // up an old value. // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = __builtin_bswap16(accData[1]); acc->ADCRaw[Y] = __builtin_bswap16(accData[2]); acc->ADCRaw[Z] = __builtin_bswap16(accData[3]); @@ -266,7 +266,7 @@ bool mpuAccReadSPI(accDev_t *acc) bool mpuGyroReadSPI(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + int16_t *gyroData = (int16_t *)gyro->dev.rxBuf; switch (gyro->gyroModeSPI) { case GYRO_EXTI_INIT: { diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index d9e3e696d..286964404 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -339,7 +339,7 @@ static bool bmi160AccRead(accDev_t *acc) // Wait for completion spiWait(&acc->gyro->dev); - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[1]; acc->ADCRaw[Y] = accData[2]; acc->ADCRaw[Z] = accData[3]; @@ -352,7 +352,7 @@ static bool bmi160AccRead(accDev_t *acc) // up an old value. // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[4]; acc->ADCRaw[Y] = accData[5]; acc->ADCRaw[Z] = accData[6]; @@ -370,7 +370,7 @@ static bool bmi160AccRead(accDev_t *acc) static bool bmi160GyroRead(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + int16_t *gyroData = (int16_t *)gyro->dev.rxBuf; switch (gyro->gyroModeSPI) { case GYRO_EXTI_INIT: { diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 61168c61b..650ef63f0 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -443,7 +443,7 @@ static bool bmi270AccRead(accDev_t *acc) // up an old value. // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[1]; acc->ADCRaw[Y] = accData[2]; acc->ADCRaw[Z] = accData[3]; @@ -490,7 +490,7 @@ static uint8_t bmi270ReadGyroCas(gyroDev_t *gyro) static bool bmi270GyroReadRegister(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + int16_t *gyroData = (int16_t *)gyro->dev.rxBuf; switch (gyro->gyroModeSPI) { case GYRO_EXTI_INIT: { diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c b/src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c index 52c6de2a0..64e67ee20 100644 --- a/src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c @@ -102,7 +102,7 @@ bool lsm6ds3AccRead(accDev_t *acc) // Wait for completion spiWait(&acc->gyro->dev); - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[1]; acc->ADCRaw[Y] = accData[2]; acc->ADCRaw[Z] = accData[3]; @@ -115,7 +115,7 @@ bool lsm6ds3AccRead(accDev_t *acc) // up an old value. // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[4]; acc->ADCRaw[Y] = accData[5]; acc->ADCRaw[Z] = accData[6]; @@ -132,7 +132,7 @@ bool lsm6ds3AccRead(accDev_t *acc) bool lsm6ds3GyroRead(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + int16_t *gyroData = (int16_t *)gyro->dev.rxBuf; switch (gyro->gyroModeSPI) { case GYRO_EXTI_INIT: { diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c index 609f1751c..c650e2a21 100644 --- a/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c @@ -102,7 +102,7 @@ bool lsm6dslAccRead(accDev_t *acc) // Wait for completion spiWait(&acc->gyro->dev); - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[1]; acc->ADCRaw[Y] = accData[2]; acc->ADCRaw[Z] = accData[3]; @@ -115,7 +115,7 @@ bool lsm6dslAccRead(accDev_t *acc) // up an old value. // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[4]; acc->ADCRaw[Y] = accData[5]; acc->ADCRaw[Z] = accData[6]; @@ -132,7 +132,7 @@ bool lsm6dslAccRead(accDev_t *acc) bool lsm6dslGyroRead(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + int16_t *gyroData = (int16_t *)gyro->dev.rxBuf; switch (gyro->gyroModeSPI) { case GYRO_EXTI_INIT: { diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c index 3630d7564..f364b1f0e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dso.c @@ -102,7 +102,7 @@ bool lsm6dsoAccRead(accDev_t *acc) // Wait for completion spiWait(&acc->gyro->dev); - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[1]; acc->ADCRaw[Y] = accData[2]; acc->ADCRaw[Z] = accData[3]; @@ -115,7 +115,7 @@ bool lsm6dsoAccRead(accDev_t *acc) // up an old value. // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf; acc->ADCRaw[X] = accData[4]; acc->ADCRaw[Y] = accData[5]; acc->ADCRaw[Z] = accData[6]; @@ -132,7 +132,7 @@ bool lsm6dsoAccRead(accDev_t *acc) bool lsm6dsoGyroRead(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + int16_t *gyroData = (int16_t *)gyro->dev.rxBuf; switch (gyro->gyroModeSPI) { case GYRO_EXTI_INIT: {