rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #22 from flightng/fix-basic-wr-of-bmi270 

[IMU driver]: Fix CAS reading timing of bmi270
This commit is contained in:
EMSR 2022-12-27 14:54:05 +08:00 committed by GitHub
parent a1aac626a3 25bb8b09a4
commit 26e13e9d60
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 117 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -101,4 +101,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RX_EXPRESSLRS_PHASELOCK",
"RX_STATE_TIME",
"SMITH_PREDICTOR",
// "BMI270_GYRO",
};

1
src/main/build/debug.h
View File

@ -99,6 +99,7 @@ typedef enum {
DEBUG_RX_EXPRESSLRS_PHASELOCK,
DEBUG_RX_STATE_TIME,
DEBUG_SMITH_PREDICTOR,
// DEBUG_BMI270_GYRO,
DEBUG_COUNT
} debugType_e;

2
src/main/drivers/accgyro/accgyro.h
View File

@ -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;

4
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -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:
{

6
src/main/drivers/accgyro/accgyro_spi_bmi160.c
View File

@ -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:
{

157
src/main/drivers/accgyro/accgyro_spi_bmi270.c
View File

@ -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
@ -165,6 +166,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 +187,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,23 +207,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);
}
}
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;
uint16_t newValue = bmi270RegisterRead16(dev, registerID);
delayMicroseconds(2);
newValue = (newValue & ~mask) | value;
bmi270RegisterWrite16(dev, registerID, newValue, delayMs);
}
// Toggle the CS to switch the device into SPI mode.
@ -356,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);
@ -439,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];
@ -454,9 +458,39 @@ 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;
int16_t *gyroData = (int16_t *)gyro->dev.rxBuf;
switch (gyro->gyroModeSPI) {
case GYRO_EXTI_INIT:
{
@ -471,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
@ -507,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;
}

6
src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c
View File

@ -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:
{

6
src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c
View File

@ -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:
{

6
src/main/drivers/accgyro/accgyro_spi_lsm6dso.c
View File

@ -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:
{