/** function to configure akm8963 mag
* @return TRUE if mag configuration finished
*/
bool imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set, void *mpu)
{
// wait before starting the configuration of the mag
// doing to early may void the mode configuration
if (get_sys_time_float() < IMU_MPU9250_MAG_STARTUP_DELAY) {
return false;
}
//config AK8963 soft reset
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_ADDR, (MPU9250_MAG_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_REG, AK8963_REG_CNTL2);
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_DO, 1);
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_wait_slave4_ready();
// Set it to continious measuring mode 2
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_ADDR, (MPU9250_MAG_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_REG, AK8963_REG_CNTL1);
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_DO, AK8963_CNTL1_CM_2);
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_wait_slave4_ready();
//Config SLV0 for continus Read
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV0_ADDR, (MPU9250_MAG_ADDR >> 1) | MPU9250_SPI_READ);
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV0_REG, AK8963_REG_HXL);
// Put the enable command as last.
mpu_set_and_wait(mpu_set, mpu, MPU9250_REG_I2C_SLV0_CTRL,
(1 << 7) | // Slave 0 enable
(7 << 0)); // Read 7 bytes (mag x,y,z + status)
return true;
}
/** function to configure hmc5883 mag
* @return TRUE if mag configuration finished
*/
bool imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu)
{
// wait before starting the configuration of the HMC58xx mag
// doing to early may void the mode configuration
if (get_sys_time_float() < ASPIRIN_2_MAG_STARTUP_DELAY) {
return false;
}
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_wait_slave4_ready();
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_wait_slave4_ready();
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ);
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM);
// Put the enable command as last.
mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_CTRL,
(1 << 7) | // Slave 0 enable
(6 << 0)); // Read 6 bytes
return true;
}
static void mpu_configure(void)
{
aspirin2_mpu60x0.length = 2;
///////////////////
// Configure power:
// MPU60X0_REG_PWR_MGMT_1
mpu_set( MPU60X0_REG_PWR_MGMT_1,
0x01); // -switch to gyroX clock
// MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
// -No standby and no wake timer
/////////////////////////
// Measurement Settings
#if PERIODIC_FREQUENCY == 60
// Accelerometer: Bandwidth 44Hz, Delay 4.9ms
// Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
# define MPU_DIG_FILTER 3
// -100Hz output = 1kHz / (9 + 1)
# define MPU_SMPLRT_DIV 9
#else
# if PERIODIC_FREQUENCY == 120
// Accelerometer: Bandwidth 44Hz, Delay 4.9ms
// Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
# define MPU_DIG_FILTER 3
// -100Hz output = 1kHz / (9 + 1)
# define MPU_SMPLRT_DIV 9
# else
# if PERIODIC_FREQUENCY == 512
// Accelerometer: Bandwidth 260Hz, Delay 0ms
// Gyroscope: Bandwidth 256Hz, Delay 0.89ms sampling 8Khz
# define MPU_DIG_FILTER 0
// -500Hz output = 1kHz / (1 + 1)
# define MPU_SMPLRT_DIV 1
# else
# error PERIODIC_FREQUENCY should be either 60Hz, 120Hz or 512Hz. Otherwise manually fix the sensor rates
# endif
# endif
#endif
// MPU60X0_REG_CONFIG
mpu_set( MPU60X0_REG_CONFIG,
(2 << 3) | // Fsync / ext sync on gyro X (bit 3->6)
(MPU_DIG_FILTER << 0) ); // Low-Pass Filter
// MPU60X0_REG_SMPLRT_DIV
mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV);
// MPU60X0_REG_GYRO_CONFIG
mpu_set( MPU60X0_REG_GYRO_CONFIG,
(3 << 3) ); // -2000deg/sec
// MPU60X0_REG_ACCEL_CONFIG
mpu_set( MPU60X0_REG_ACCEL_CONFIG,
(0 << 0) | // No HPFL
(3 << 3) ); // Full Scale = 16g
#ifndef MPU6000_NO_SLAVES
/////////////////////////////////////
// SPI Slave Configuration Section
// Power the Aux I2C Circuit:
// MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd
// MPU60X0_REG_USER_CTRL:
mpu_set( MPU60X0_REG_USER_CTRL,
(1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode
(1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface
(0 << 1) ); // Trigger a I2C_MST_RESET
// Enable the aux i2c
mpu_set( MPU60X0_REG_I2C_MST_CTRL,
(0 << 7) | // no multimaster
(0 << 6) | // do not delay IRQ waiting for all external slaves
(0 << 5) | // no slave 3 FIFO
(0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start
(8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz
mpu_set( MPU60X0_REG_I2C_MST_DELAY,
(0 << 2) | // No Delay Slave 2
(1 << 3) ); // Delay Slave 3
#ifdef IMU_ASPIRIN_VERSION_2_1
// MS5611 Send Reset
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (MS5611_ADDR0));
mpu_set( MPU60X0_REG_I2C_SLV4_DO, MS5611_REG_RESET);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(1 << 5) | // Reg_Dis: do not write the register, just the data
(0 << 0) ); // Full Speed
mpu_wait_slave4_ready();
// Wait at least 2.8ms
#endif
// HMC5883 Magnetometer Configuration
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA);
mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(0 << 0) ); // Full Speed
mpu_wait_slave4_ready();
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB);
mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(0 << 0) ); // Full Speed
mpu_wait_slave4_ready();
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE);
mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(3 << 0) ); // From now on a delayed rate of 1/4 is defined...
// HMC5883 Reading:
// a) write hmc-register to HMC
// b) read 6 bytes from HMC
mpu_set( MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ);
mpu_set( MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV0_CTRL,
(1 << 7) | // Slave 0 enable
(0 << 6) | // Byte Swap
(6 << 0) ); // Read 6 bytes
// Slave 0 Control:
#ifdef IMU_ASPIRIN_VERSION_2_1
#pragma note "Reading the MS5611"
/*
// Read MS5611 Calibration
mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ);
mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV1_CTRL,
(1 << 7) | // Slave 1 enable
(0 << 6) | // Byte Swap
(3 << 0) ); // Read 6 bytes
*/
// Full Rate Request For Pressure
mpu_set( MPU60X0_REG_I2C_SLV2_ADDR, (MS5611_ADDR0));
mpu_set( MPU60X0_REG_I2C_SLV2_DO, 0x48);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV2_CTRL,
(1 << 7) | // Slave 2 enable
(0 << 6) | // Byte Swap
(1 << 5) | // Rig Dis: Write Only
(1 << 0) ); // Write 1 byte
// Reduced rate request For Temperature: Overwrites the Pressure Request
mpu_set( MPU60X0_REG_I2C_SLV3_ADDR, (MS5611_ADDR0));
mpu_set( MPU60X0_REG_I2C_SLV3_DO, 0x58);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV3_CTRL,
(1 << 7) | // Slave 2 enable
(0 << 6) | // Byte Swap
(1 << 5) | // Rig Dis: Write Only
(1 << 0) ); // Write 1 byte
mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ);
mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV1_CTRL,
(1 << 7) | // Slave 1 enable
(0 << 6) | // Byte Swap
(3 << 0) ); // Read 6 bytes
#endif
#endif
}
