Remove accgyro mpu read, write and ISR update function pointers
This commit is contained in:
parent
5e63724f7d
commit
03d1409fd2
|
@ -53,7 +53,6 @@ typedef struct gyroDev_s {
|
|||
sensorGyroInitFuncPtr initFn; // initialize function
|
||||
sensorGyroReadFuncPtr readFn; // read 3 axis data function
|
||||
sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available
|
||||
sensorGyroUpdateFuncPtr updateFn;
|
||||
extiCallbackRec_t exti;
|
||||
busDevice_t bus;
|
||||
float scale; // scalefactor
|
||||
|
@ -66,14 +65,14 @@ typedef struct gyroDev_s {
|
|||
gyroRateKHz_e gyroRateKHz;
|
||||
uint8_t mpuDividerDrops;
|
||||
bool dataReady;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
#endif
|
||||
sensor_align_e gyroAlign;
|
||||
mpuDetectionResult_t mpuDetectionResult;
|
||||
ioTag_t mpuIntExtiTag;
|
||||
mpuConfiguration_t mpuConfiguration;
|
||||
bool gyro_high_fsr;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
#endif
|
||||
} gyroDev_t;
|
||||
|
||||
typedef struct accDev_s {
|
||||
|
@ -84,13 +83,13 @@ typedef struct accDev_s {
|
|||
int16_t ADCRaw[XYZ_AXIS_COUNT];
|
||||
char revisionCode; // a revision code for the sensor, if known
|
||||
bool dataReady;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
#endif
|
||||
sensor_align_e accAlign;
|
||||
mpuDetectionResult_t mpuDetectionResult;
|
||||
mpuConfiguration_t mpuConfiguration;
|
||||
bool acc_high_fsr;
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
#endif
|
||||
} accDev_t;
|
||||
|
||||
static inline void accDevLock(accDev_t *acc)
|
||||
|
|
|
@ -64,23 +64,18 @@ mpuResetFnPtr mpuResetFn;
|
|||
|
||||
#define MPU_INQUIRY_MASK 0x7E
|
||||
|
||||
#ifdef USE_I2C
|
||||
static void mpu6050FindRevision(gyroDev_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
UNUSED(ack);
|
||||
|
||||
uint8_t readBuffer[6];
|
||||
uint8_t revision;
|
||||
uint8_t productId;
|
||||
|
||||
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
|
||||
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
|
||||
|
||||
// determine product ID and accel revision
|
||||
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6);
|
||||
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
|
||||
if (revision) {
|
||||
/* Congrats, these parts are better. */
|
||||
// determine product ID and revision
|
||||
uint8_t readBuffer[6];
|
||||
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6);
|
||||
uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
|
||||
if (ack && revision) {
|
||||
// Congrats, these parts are better
|
||||
if (revision == 1) {
|
||||
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
|
||||
} else if (revision == 2) {
|
||||
|
@ -91,9 +86,10 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
|
|||
failureMode(FAILURE_ACC_INCOMPATIBLE);
|
||||
}
|
||||
} else {
|
||||
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1);
|
||||
uint8_t productId;
|
||||
ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1);
|
||||
revision = productId & 0x0F;
|
||||
if (!revision) {
|
||||
if (!ack || revision == 0) {
|
||||
failureMode(FAILURE_ACC_INCOMPATIBLE);
|
||||
} else if (revision == 4) {
|
||||
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
|
||||
|
@ -102,6 +98,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Gyro interrupt service routine
|
||||
|
@ -117,19 +114,14 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
|||
#endif
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
gyro->dataReady = true;
|
||||
if (gyro->updateFn) {
|
||||
gyro->updateFn(gyro);
|
||||
}
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
const uint32_t now2Us = micros();
|
||||
debug[1] = (uint16_t)(now2Us - nowUs);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static void mpuIntExtiInit(gyroDev_t *gyro)
|
||||
{
|
||||
#if defined(MPU_INT_EXTI)
|
||||
if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
|
||||
return;
|
||||
}
|
||||
|
@ -156,31 +148,14 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
|
|||
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
|
||||
EXTIEnable(mpuIntIO, true);
|
||||
#endif
|
||||
|
||||
#else
|
||||
UNUSED(gyro);
|
||||
#endif
|
||||
}
|
||||
|
||||
static bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t* data, uint8_t length)
|
||||
{
|
||||
UNUSED(bus);
|
||||
const bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
|
||||
return ack;
|
||||
}
|
||||
|
||||
static bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||
{
|
||||
UNUSED(bus);
|
||||
const bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
|
||||
return ack;
|
||||
}
|
||||
#endif // MPU_INT_EXTI
|
||||
|
||||
bool mpuAccRead(accDev_t *acc)
|
||||
{
|
||||
uint8_t data[6];
|
||||
|
||||
const bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6);
|
||||
const bool ack = busReadRegisterBuffer(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6);
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
@ -192,18 +167,11 @@ bool mpuAccRead(accDev_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
|
||||
{
|
||||
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
|
||||
gyro->updateFn = updateFn;
|
||||
}
|
||||
}
|
||||
|
||||
bool mpuGyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t data[6];
|
||||
|
||||
const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6);
|
||||
const bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6);
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
@ -250,8 +218,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
sensor = mpu6000SpiDetect(&gyro->bus);
|
||||
if (sensor != MPU_NONE) {
|
||||
gyro->mpuDetectionResult.sensor = sensor;
|
||||
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
|
||||
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
@ -267,8 +233,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
|
||||
if (sensor != MPU_NONE) {
|
||||
gyro->mpuDetectionResult.sensor = sensor;
|
||||
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
|
||||
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
@ -283,8 +247,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
sensor = mpu9250SpiDetect(&gyro->bus);
|
||||
if (sensor != MPU_NONE) {
|
||||
gyro->mpuDetectionResult.sensor = sensor;
|
||||
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
|
||||
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
|
||||
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
|
||||
return true;
|
||||
}
|
||||
|
@ -300,8 +262,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
sensor = icm20649SpiDetect(&gyro->bus);
|
||||
if (sensor != MPU_NONE) {
|
||||
gyro->mpuDetectionResult.sensor = sensor;
|
||||
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
|
||||
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
@ -316,8 +276,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
sensor = icm20689SpiDetect(&gyro->bus);
|
||||
if (sensor != MPU_NONE) {
|
||||
gyro->mpuDetectionResult.sensor = sensor;
|
||||
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
|
||||
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
@ -332,8 +290,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
sensor = bmi160Detect(&gyro->bus);
|
||||
if (sensor != MPU_NONE) {
|
||||
gyro->mpuDetectionResult.sensor = sensor;
|
||||
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
|
||||
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
@ -347,27 +303,27 @@ void mpuDetect(gyroDev_t *gyro)
|
|||
// MPU datasheet specifies 30ms.
|
||||
delay(35);
|
||||
|
||||
uint8_t sig = 0;
|
||||
#ifdef USE_I2C
|
||||
gyro->bus.bustype = BUSTYPE_I2C;
|
||||
gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE;
|
||||
gyro->bus.busdev_u.i2c.address = MPU_ADDRESS;
|
||||
bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
|
||||
uint8_t sig = 0;
|
||||
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
|
||||
#else
|
||||
bool ack = false;
|
||||
#endif
|
||||
if (ack) {
|
||||
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
|
||||
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
|
||||
} else {
|
||||
|
||||
if (!ack) {
|
||||
#ifdef USE_SPI
|
||||
detectSPISensorsAndUpdateDetectionResult(gyro);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef USE_I2C
|
||||
// If an MPU3050 is connected sig will contain 0.
|
||||
uint8_t inquiryResult;
|
||||
ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
|
||||
ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
|
||||
inquiryResult &= MPU_INQUIRY_MASK;
|
||||
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
|
||||
gyro->mpuDetectionResult.sensor = MPU_3050;
|
||||
|
@ -381,9 +337,14 @@ void mpuDetect(gyroDev_t *gyro)
|
|||
} else if (sig == MPU6500_WHO_AM_I_CONST) {
|
||||
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void mpuGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
#ifdef MPU_INT_EXTI
|
||||
mpuIntExtiInit(gyro);
|
||||
#else
|
||||
UNUSED(gyro);
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -140,15 +140,11 @@
|
|||
// RF = Register Flag
|
||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||
|
||||
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t* data, uint8_t length);
|
||||
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||
typedef void (*mpuResetFnPtr)(void);
|
||||
|
||||
extern mpuResetFnPtr mpuResetFn;
|
||||
|
||||
typedef struct mpuConfiguration_s {
|
||||
mpuReadRegisterFnPtr readFn;
|
||||
mpuWriteRegisterFnPtr writeFn;
|
||||
mpuResetFnPtr resetFn;
|
||||
} mpuConfiguration_t;
|
||||
|
||||
|
@ -213,4 +209,3 @@ bool mpuAccRead(struct accDev_s *acc);
|
|||
bool mpuGyroRead(struct gyroDev_s *gyro);
|
||||
bool mpuGyroReadSPI(struct gyroDev_s *gyro);
|
||||
void mpuDetect(struct gyroDev_s *gyro);
|
||||
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
|
||||
|
|
|
@ -50,25 +50,24 @@
|
|||
|
||||
static void mpu3050Init(gyroDev_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
||||
|
||||
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
|
||||
if (!ack)
|
||||
const bool ack = busWriteRegister(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
|
||||
if (!ack) {
|
||||
failureMode(FAILURE_ACC_INIT);
|
||||
}
|
||||
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
|
||||
busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0);
|
||||
busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||
busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
}
|
||||
|
||||
static bool mpu3050GyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t data[6];
|
||||
|
||||
const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_GYRO_OUT, data, 6);
|
||||
const bool ack = busReadRegisterBuffer(&gyro->bus, MPU3050_GYRO_OUT, data, 6);
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
@ -83,7 +82,7 @@ static bool mpu3050GyroRead(gyroDev_t *gyro)
|
|||
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
|
||||
{
|
||||
uint8_t buf[2];
|
||||
if (!gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) {
|
||||
if (!busReadRegisterBuffer(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -77,23 +77,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
|
|||
{
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||
delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
||||
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
||||
busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
||||
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
||||
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
||||
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
||||
|
||||
// ACC Init stuff.
|
||||
// Accel scale 8g (4096 LSB/g)
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG,
|
||||
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG,
|
||||
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
|
||||
|
||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
@ -54,34 +54,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
|
|||
{
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
|
||||
delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
|
||||
delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||
delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
delay(15);
|
||||
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
|
||||
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||
busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||
delay(100);
|
||||
|
||||
// Data ready interrupt configuration
|
||||
#ifdef USE_MPU9250_MAG
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
#else
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
|
||||
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
|
||||
#endif
|
||||
delay(15);
|
||||
|
||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
|
||||
busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
|
||||
#endif
|
||||
delay(15);
|
||||
}
|
||||
|
|
|
@ -95,12 +95,12 @@ void icm20649AccInit(accDev_t *acc)
|
|||
|
||||
spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);
|
||||
|
||||
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
|
||||
spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
|
||||
delay(15);
|
||||
const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
|
||||
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
|
||||
spiBusWriteRegister(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
|
||||
delay(15);
|
||||
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
|
||||
spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
|
||||
delay(15);
|
||||
}
|
||||
|
||||
|
@ -123,31 +123,31 @@ void icm20649GyroInit(gyroDev_t *gyro)
|
|||
|
||||
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed
|
||||
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
|
||||
delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
|
||||
delay(15);
|
||||
const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
|
||||
uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
|
||||
raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3;
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||
delay(100);
|
||||
|
||||
// Data ready interrupt configuration
|
||||
// back to bank 0
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
delay(15);
|
||||
|
||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
|
||||
spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -187,7 +187,7 @@ bool icm20649AccRead(accDev_t *acc)
|
|||
{
|
||||
uint8_t data[6];
|
||||
|
||||
const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
|
||||
const bool ack = spiBusReadRegisterBuffer(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -119,32 +119,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
|
|||
|
||||
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON);
|
||||
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||
delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
||||
delay(100);
|
||||
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||
// spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||
// delay(100);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
delay(15);
|
||||
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
|
||||
delay(15);
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||
delay(100);
|
||||
|
||||
// Data ready interrupt configuration
|
||||
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
// spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
|
||||
delay(15);
|
||||
|
||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
||||
#endif
|
||||
|
||||
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);
|
||||
|
|
|
@ -171,6 +171,7 @@ uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg)
|
|||
|
||||
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
|
||||
{
|
||||
bus->bustype = BUSTYPE_SPI;
|
||||
bus->busdev_u.spi.instance = instance;
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -42,5 +42,4 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
|
|||
struct gyroDev_s;
|
||||
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);
|
||||
|
|
Loading…
Reference in New Issue