From 4ec8f83b811ee02ba6ea05e3a479089dabaf52b2 Mon Sep 17 00:00:00 2001 From: Sebastian Serrano Date: Sun, 17 Apr 2016 16:19:05 -0300 Subject: [PATCH 1/2] add MPU9250 support --- Sensors.cpp | 92 +++++++++++++++++++++++++++++++++++++++++++++++++++-- Sensors.h | 6 ++-- config.h | 3 +- def.h | 8 ++--- 4 files changed, 98 insertions(+), 11 deletions(-) diff --git a/Sensors.cpp b/Sensors.cpp index 88ea67a2..dd042199 100644 --- a/Sensors.cpp +++ b/Sensors.cpp @@ -980,7 +980,7 @@ void Mag_init() { delay(100); } -#if !defined(MPU6050_I2C_AUX_MASTER) +#if !defined(MPU6050_I2C_AUX_MASTER || MPU9250) void Device_Mag_getADC() { i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER); MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , @@ -1063,7 +1063,7 @@ static void Mag_init() { delay(100); } -#if !defined(MPU6050_I2C_AUX_MASTER) +#if !defined(MPU6050_I2C_AUX_MASTER || MPU9250) static void Device_Mag_getADC() { getADC(); } @@ -1303,6 +1303,92 @@ void Gyro_getADC () { // End Of I2C Gyroscope and Accelerometer LSM330 // ************************************************************************************************************ +// ************************************************************************************************************ +// I2C Gyroscope/Accelerometer/Compass MPU9250 +// ************************************************************************************************************ +#if defined(MPU9250) + +#if !defined(MPU9250_ADDRESS) + #define MPU9250_ADDRESS 0x68 // address pin AD0 low (GND), default for FreeIMU v0.4 and InvenSense evaluation board + //#define MPU9250_ADDRESS 0x69 // address pin AD0 high (VCC) + //The MAG acquisition + #define MAG_ADDRESS 0x0C + #define MAG_DATA_REGISTER 0x03 +#endif + + +//////////////////////////////////// +// Gyro start // +//////////////////////////////////// + +void Gyro_init() { + TWBR = ((F_CPU / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz + i2c_writeReg(MPU9250_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + delay(5); + i2c_writeReg(MPU9250_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + i2c_writeReg(MPU9250_ADDRESS, 0x1A, GYRO_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + i2c_writeReg(MPU9250_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + // enable I2C bypass for AUX I2C, allways on because we have a magnetometer on board + i2c_writeReg(MPU9250_ADDRESS, 0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0 +} + +void Gyro_getADC () { + i2c_getSixRawADC(MPU9250_ADDRESS, 0x43); + GYRO_ORIENTATION( ((rawADC[0]<<8) | rawADC[1])/4 , // range: +/- 8192; +/- 2000 deg/sec + ((rawADC[2]<<8) | rawADC[3])/4 , + ((rawADC[4]<<8) | rawADC[5])/4 ); + GYRO_Common(); +} + +//////////////////////////////////// +// Gyro end // +//////////////////////////////////// + + +//////////////////////////////////// +// ACC start // +//////////////////////////////////// +void ACC_init () { + i2c_writeReg(MPU9250_ADDRESS, 0x1C, 0x10); +} + +void ACC_getADC () { + i2c_getSixRawADC(MPU9250_ADDRESS, 0x3B); + ACC_ORIENTATION( ((rawADC[0]<<8) | rawADC[1])/8 , + ((rawADC[2]<<8) | rawADC[3])/8 , + ((rawADC[4]<<8) | rawADC[5])/8 ); + ACC_Common(); +} + +//////////////////////////////////// +// ACC end // +//////////////////////////////////// + + +//////////////////////////////////// +// MAG start // +//////////////////////////////////// +void Mag_init() { + delay(100); + i2c_writeReg(MAG_ADDRESS,0x0a,0x01); //Start the first conversion + delay(100); +} + void Device_Mag_getADC() { + i2c_getSixRawADC(MAG_ADDRESS, 0x03); + MAG_ORIENTATION( ((rawADC[1]<<8) | rawADC[0]) , + ((rawADC[3]<<8) | rawADC[2]) , + ((rawADC[5]<<8) | rawADC[4]) ); + //Start another meassurement + i2c_writeReg(MAG_ADDRESS,0x0a,0x01); +} +//////////////////////////////////// +// MAG end // +//////////////////////////////////// + +#endif +// ************************************************************************************************************ +// End Of I2C Gyroscope/Accelerometer/Compass MPU9250 +// ************************************************************************************************************ #if defined(WMP) // ************************************************************************************************************ @@ -1536,4 +1622,4 @@ void initSensors() { initS(); if (i2c_errors_count == 0) break; // no error during init => init ok } -} +} diff --git a/Sensors.h b/Sensors.h index 6ed6da71..c9611817 100644 --- a/Sensors.h +++ b/Sensors.h @@ -37,7 +37,7 @@ void i2c_read_reg_to_buf(uint8_t add, uint8_t reg, uint8_t *buf, uint8_t size); #if defined(ADCACC) #define ACC_1G 75 #endif -#if defined(MPU6050) +#if defined(MPU6050 || defined(MPU9250)) #if defined(FREEIMUv04) #define ACC_1G 255 #else @@ -54,7 +54,7 @@ void i2c_read_reg_to_buf(uint8_t add, uint8_t reg, uint8_t *buf, uint8_t size); #if defined(ITG3050) #define GYRO_SCALE (4 / 16.0 * PI / 180.0 / 1000000.0) //16.4 LSB = 1 deg/s -- 16.0 apparently gives beter results than 16.4 (empirical) #endif -#if defined(MPU6050) || defined(MPU3050) +#if defined(MPU6050) || defined(MPU3050) || defined(MPU9250) #define GYRO_SCALE (4 / 16.4 * PI / 180.0 / 1000000.0) //16.4 LSB = 1 deg/s #endif #if defined(ITG3200) @@ -67,4 +67,4 @@ void i2c_read_reg_to_buf(uint8_t add, uint8_t reg, uint8_t *buf, uint8_t size); #define GYRO_SCALE (1.0f/200e6f) #endif -#endif /* SENSORS_H_ */ +#endif /* SENSORS_H_ */ diff --git a/config.h b/config.h index dffdfa1e..1a7e7669 100644 --- a/config.h +++ b/config.h @@ -170,6 +170,7 @@ //#define MPU3050 //#define L3G4200D //#define MPU6050 //combo + ACC + //#define MPU9250 //#define LSM330 //combo + ACC /* I2C accelerometer */ @@ -1228,4 +1229,4 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /*************************************************************************************************/ #endif /* CONFIG_H_ */ - + diff --git a/def.h b/def.h index 04a93dcb..def04aed 100644 --- a/def.h +++ b/def.h @@ -1668,19 +1668,19 @@ /*************** Sensor Type definitions ********************/ /**************************************************************************************/ -#if defined(ADXL345) || defined(BMA020) || defined(BMA180) || defined(BMA280) || defined(MMA7455) || defined(ADCACC) || defined(LIS3LV02) || defined(LSM303DLx_ACC) || defined(MPU6050) || defined(LSM330) || defined(MMA8451Q) +#if defined(ADXL345) || defined(BMA020) || defined(BMA180) || defined(BMA280) || defined(MMA7455) || defined(ADCACC) || defined(LIS3LV02) || defined(LSM303DLx_ACC) || defined(MPU6050) || defined(LSM330) || defined(MMA8451Q) || defined(MPU9250) #define ACC 1 #else #define ACC 0 #endif -#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) || defined(MAG3110) +#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) || defined(MAG3110) || defined(MPU9250) #define MAG 1 #else #define MAG 0 #endif -#if defined(ITG3200) || defined(ITG3050) || defined(L3G4200D) || defined(MPU6050) || defined(LSM330) || defined(MPU3050) || defined(WMP) +#if defined(ITG3200) || defined(ITG3050) || defined(L3G4200D) || defined(MPU6050) || defined(LSM330) || defined(MPU3050) || defined(WMP) || defined(MPU9250) #define GYRO 1 #else #define GYRO 0 @@ -2029,4 +2029,4 @@ #error "you use one feature that is no longer supported or has undergone a name change" #endif -#endif /* DEF_H_ */ +#endif /* DEF_H_ */ From c59ae2eeaf8cd74a4a78845be386ed0d53b518d7 Mon Sep 17 00:00:00 2001 From: Sebastian Serrano Date: Sun, 17 Apr 2016 19:58:19 -0300 Subject: [PATCH 2/2] fix silly bug --- Sensors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sensors.h b/Sensors.h index c9611817..dbf8678f 100644 --- a/Sensors.h +++ b/Sensors.h @@ -37,7 +37,7 @@ void i2c_read_reg_to_buf(uint8_t add, uint8_t reg, uint8_t *buf, uint8_t size); #if defined(ADCACC) #define ACC_1G 75 #endif -#if defined(MPU6050 || defined(MPU9250)) +#if defined(MPU6050) || defined(MPU9250) #if defined(FREEIMUv04) #define ACC_1G 255 #else