Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add DMA support for ICM-42688-P #11160

Closed
wants to merge 2 commits into from
Closed

Add DMA support for ICM-42688-P #11160

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
01da5db
Add DMA support for ICM-42688-P
SteveCEvans Oct 22, 2021
e0c28f9
Make gyroDataIndex const
SteveCEvans Dec 28, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/main/drivers/accgyro/accgyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,8 @@ typedef struct gyroDev_s {
fp_rotationMatrix_t rotationMatrix;
uint16_t gyroSampleRateHz;
uint16_t accSampleRateHz;
uint8_t accDataReg;
uint8_t gyroDataReg;
} gyroDev_t;

typedef struct accDev_s {
Expand Down
23 changes: 14 additions & 9 deletions src/main/drivers/accgyro/accgyro_mpu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];

const bool ack = busReadRegisterBuffer(&acc->gyro->dev, MPU_RA_ACCEL_XOUT_H, data, 6);
const bool ack = busReadRegisterBuffer(&acc->gyro->dev, acc->gyro->accDataReg, data, 6);
if (!ack) {
return false;
}
Expand All @@ -197,7 +197,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];

const bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_GYRO_XOUT_H, data, 6);
const bool ack = busReadRegisterBuffer(&gyro->dev, gyro->gyroDataReg, data, 6);
if (!ack) {
return false;
}
Expand All @@ -220,7 +220,7 @@ bool mpuAccReadSPI(accDev_t *acc)
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&acc->gyro->dev);

acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
acc->gyro->dev.txBuf[0] = acc->gyro->accDataReg | 0x80;

busSegment_t segments[] = {
{NULL, NULL, 7, true, NULL},
Expand Down Expand Up @@ -277,8 +277,8 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
spiSetAtomicWait(&gyro->dev);
gyro->dev.callbackArg = (uint32_t)gyro;
gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
gyro->segments[0].len = 15;
gyro->dev.txBuf[0] = gyro->accDataReg | 0x80;
gyro->segments[0].len = gyro->gyroDataReg - gyro->accDataReg + 7;
gyro->segments[0].callback = mpuIntcallback;
gyro->segments[0].txData = gyro->dev.txBuf;
gyro->segments[0].rxData = &gyro->dev.rxBuf[1];
Expand All @@ -302,7 +302,7 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&gyro->dev);

gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80;
gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80;

busSegment_t segments[] = {
{NULL, NULL, 7, true, NULL},
Expand All @@ -324,11 +324,14 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)

case GYRO_EXTI_INT_DMA:
{
// Acc and gyro data may not be continuous (MPU6xxx has temperature in between)
const uint8_t gyroDataIndex = ((gyro->gyroDataReg - gyro->accDataReg) >> 1) + 1;

// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
// up an old value.
gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[5]);
gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[6]);
gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[7]);
gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[gyroDataIndex]);
gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[gyroDataIndex + 1]);
gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[gyroDataIndex + 2]);
break;
}

Expand Down Expand Up @@ -479,6 +482,8 @@ bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)

void mpuGyroInit(gyroDev_t *gyro)
{
gyro->accDataReg = MPU_RA_ACCEL_XOUT_H;
gyro->gyroDataReg = MPU_RA_GYRO_XOUT_H;
#ifdef USE_GYRO_EXTI
mpuIntExtiInit(gyro);
#else
Expand Down
38 changes: 4 additions & 34 deletions src/main/drivers/accgyro/accgyro_spi_icm426xx.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,21 +151,6 @@ void icm426xxAccInit(accDev_t *acc)
acc->acc_1G = 512 * 4;
}

bool icm426xxAccRead(accDev_t *acc)
{
uint8_t data[6];

const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM426XX_RA_ACCEL_DATA_X1, data, 6);
if (!ack) {
return false;
}

acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);

return true;
}
bool icm426xxSpiAccDetect(accDev_t *acc)
{
switch (acc->mpuDetectionResult.sensor) {
Expand All @@ -178,7 +163,7 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
}

acc->initFn = icm426xxAccInit;
acc->readFn = icm426xxAccRead;
acc->readFn = mpuAccReadSPI;

return true;
}
Expand All @@ -198,6 +183,8 @@ static odrEntry_t icm426xxPkhzToSupportedODRMap[] = {
void icm426xxGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->accDataReg = ICM426XX_RA_ACCEL_DATA_X1;
gyro->gyroDataReg = ICM426XX_RA_GYRO_DATA_X1;

spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM426XX_MAX_SPI_INIT_CLK_HZ));

Expand Down Expand Up @@ -252,23 +239,6 @@ void icm426xxGyroInit(gyroDev_t *gyro)
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
}

bool icm426xxGyroReadSPI(gyroDev_t *gyro)
{
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM426XX_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
STATIC_DMA_DATA_AUTO uint8_t data[7];

const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
if (!ack) {
return false;
}

gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);

return true;
}

bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
{
switch (gyro->mpuDetectionResult.sensor) {
Expand All @@ -281,7 +251,7 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
}

gyro->initFn = icm426xxGyroInit;
gyro->readFn = icm426xxGyroReadSPI;
gyro->readFn = mpuGyroReadSPI;

gyro->scale = GYRO_SCALE_2000DPS;

Expand Down