bool ak8963Detect(magDev_t *mag) { for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { bool ack = false; uint8_t sig = 0; #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) // initialze I2C master via SPI bus (MPU9250) ack = verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR delay(10); ack = verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); ack = verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); #endif // check for AK8963 ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); if (ack && sig == AK8963_Device_ID) { // 0x48 / 01001000 / 'H' mag->init = ak8963Init; mag->read = ak8963Read; return true; } } return false; }
bool ak8963SPIWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register verifympu9250WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte return true; }
bool ak8963SPIRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes delay(8); __disable_irq(); mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C __enable_irq(); return true; }
bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) { if (queuedRead.waiting) { return false; } queuedRead.len = len_; verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; return true; }
bool ak8963Detect(mag_t *mag) { bool ack = false; uint8_t sig = 0; #ifdef USE_I2C // check for AK8963 on I2C bus ack = i2cRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' { ak8963config.read = i2cRead; ak8963config.write = i2cWrite; mag->init = ak8963Init; mag->read = ak8963Read; return true; } #endif #ifdef USE_SPI // check for AK8963 on I2C master via SPI bus (as part of MPU9250) ack = verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR delay(10); ack = verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); ack = verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); ack = ak8963SPIRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' { ak8963config.read = ak8963SPIRead; ak8963config.write = ak8963SPIWrite; mag->init = ak8963Init; mag->read = ak8963Read; return true; } #endif return false; }
static void mpu9250AccAndGyroInit(uint8_t lpf) { if (mpuSpi9250InitDone) { return; } spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed for writing to slow registers mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); #if defined (REVONANO) || defined (SPARKY2) || defined(ALIENFLIGHTF4) || defined(BLUEJAYF4) || defined(VRCORE) //mpu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_8800_32); //Fchoice_b defaults to 00 which makes fchoice 11 verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 if (lpf == 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (lpf < 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (lpf > 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF } verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops #else verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 if (lpf == 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (lpf < 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (lpf > 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF } verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops #endif verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #if defined(USE_MPU_DATA_READY_SIGNAL) verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif mpuSpi9250InitDone = true; //init done }