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..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)
+#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_ */