diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index f6df5875a..5908c69d0 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -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) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index ed7e59451..9ec2e64dd 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -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 } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index ed2f7ca97..46247082c 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -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); diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c index 74a684c35..3cdd97b3e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro/accgyro_mpu3050.c @@ -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; } diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c index a008d201f..1bdeb73c4 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro/accgyro_mpu6050.c @@ -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 } diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index abec649c8..f211c98b5 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -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); } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index e6210c4bb..c5e9ac65b 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -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; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 7514b7ae9..1fe8f2fd5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -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); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 39777822a..c3ac21f92 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -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 diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 0eb572373..3f0267b60 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -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);