mpu9150: setSleepEnabled( false ) must be done as the first action fo…

Arduino/MPU9150/Examples/MPU9150_raw/MPU9150_raw.ino

@@ -52,6 +52,8 @@ int16_t mx, my, mz;
 #define LED_PIN 13
 bool blinkState = false;
 
+long lasttime;
+
 void setup() {
     // join I2C bus (I2Cdev library doesn't do this automatically)
     Wire.begin();
@@ -64,45 +66,58 @@ void setup() {
     // initialize device
     Serial.println("Initializing I2C devices...");
     accelGyroMag.initialize();
+    accelGyroMag.setupCompas();
+
+//     accelGyroMag.setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
+//     accelGyroMag.setClockSource(MPU9150_CLOCK_PLL_XGYRO);
+//     accelGyroMag.setFullScaleGyroRange(MPU9150_GYRO_FS_250);
+//     accelGyroMag.setFullScaleAccelRange(MPU9150_ACCEL_FS_2);
 
     // verify connection
     Serial.println("Testing device connections...");
     Serial.println(accelGyroMag.testConnection() ? "MPU9150 connection successful" : "MPU9150 connection failed");
 
     // configure Arduino LED for
     pinMode(LED_PIN, OUTPUT);
+    lasttime = millis();
 }
 
 void loop() {
+    long time = millis();
     // read raw accel/gyro/mag measurements from device
-    accelGyroMag.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+    accelGyroMag.getMotion9new(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
 
     // these methods (and a few others) are also available
     //accelGyroMag.getAcceleration(&ax, &ay, &az);
     //accelGyroMag.getRotation(&gx, &gy, &gz);
 
+    Serial.print("time:   ");
+    Serial.print( time - lasttime );
     // display tab-separated accel/gyro/mag x/y/z values
-//  Serial.print("a/g/m:\t");
-//  Serial.print(ax); Serial.print("\t");
-//  Serial.print(ay); Serial.print("\t");
-//  Serial.print(az); Serial.print("\t");
-//  Serial.print(gx); Serial.print("\t");
-//  Serial.print(gy); Serial.print("\t");
-//  Serial.print(gz); Serial.print("\t");
-    Serial.print(int(mx)*int(mx)); Serial.print("\t");
-    Serial.print(int(my)*int(my)); Serial.print("\t");
-    Serial.print(int(mz)*int(mz)); Serial.print("\t | ");
-
-    const float N = 256;
-    float mag = mx*mx/N + my*my/N + mz*mz/N;
-
-    Serial.print(mag); Serial.print("\t");
-    for (int i=0; i<mag; i++)
-        Serial.print("*");
+ Serial.print(": a/g/m:   ");
+ Serial.print(ax); Serial.print("   ");
+ Serial.print(ay); Serial.print("   ");
+ Serial.print(az); Serial.print("   ");
+ Serial.print(gx); Serial.print("   ");
+ Serial.print(gy); Serial.print("   ");
+ Serial.print(gz); Serial.print("   ");
+    Serial.print(mx); Serial.print("   ");
+    Serial.print(my); Serial.print("   ");
+    Serial.print(mz); Serial.print("    | ");
+// 
+// //     const float N = 256;
+// //     float mag = mx*mx/N + my*my/N + mz*mz/N;
+// // 
+// //     Serial.print(mag); Serial.print("   ");
+// //     for (int i=0; i<mag; i++)
+// //         Serial.print("*");
     Serial.print("\n");
 
     // blink LED to indicate activity
     blinkState = !blinkState;
     digitalWrite(LED_PIN, blinkState);
-    delay(50);
+
+    lasttime = time;
+    while ( (millis() - lasttime) < 5 ){;;} // wait up to 5 ms
+//     delay(5);
 }

Arduino/MPU9150/MPU9150.cpp

@@ -61,10 +61,10 @@ MPU9150::MPU9150(uint8_t address) {
  * the default internal clock source.
  */
 void MPU9150::initialize() {
+    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
     setClockSource(MPU9150_CLOCK_PLL_XGYRO);
     setFullScaleGyroRange(MPU9150_GYRO_FS_250);
     setFullScaleAccelRange(MPU9150_ACCEL_FS_2);
-    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
 }
 
 /** Verify the I2C connection.
@@ -3150,4 +3150,125 @@ uint8_t MPU9150::getDMPConfig2() {
 }
 void MPU9150::setDMPConfig2(uint8_t config) {
     I2Cdev::writeByte(devAddr, MPU9150_RA_DMP_CFG_2, config);
-}
+}
+
+
+// compass
+
+
+void MPU9150::setupCompas(){
+//http://pansenti.wordpress.com/2013/03/26/pansentis-invensense-mpu-9150-software-for-arduino-is-now-on-github/
+//Thank you to pansenti for setup code.
+
+//   MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
+
+  I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x00); //PowerDownMode
+  I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x0F); //SelfTest
+  I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x00); //PowerDownMode
+//   MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
+//   MPU9150_writeSensor(0x0A, 0x0F); //SelfTest
+//   MPU9150_writeSensor(0x0A, 0x00); //PowerDownMode
+
+//   MPU9150_I2C_ADDRESS = devAddr;      //change Adress to MPU
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_MST_CTRL, 0x40); //Wait for Data at Slave0
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV0_ADDR, 0x8C); //Set i2c address at slave0 at 0x0C
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV0_REG, 0x02); //Set where reading at slave 0 starts    
+//   MPU9150_writeSensor(0x24, 0x40); //Wait for Data at Slave0
+//   MPU9150_writeSensor(0x25, 0x8C); //Set i2c address at slave0 at 0x0C
+//   MPU9150_writeSensor(0x26, 0x02); //Set where reading at slave 0 starts
+
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV0_CTRL, 0x88); //set offset at start reading and enable
+//   MPU9150_writeSensor(0x27, 0x88); //set offset at start reading and enable
+
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV1_ADDR, 0x0C); //set i2c address at slv1 at 0x0C
+//     MPU9150_writeSensor(0x28, 0x0C); //set i2c address at slv1 at 0x0C
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV1_REG, 0x0A); //Set where reading at slave 1 starts
+//     MPU9150_writeSensor(0x29, 0x0A); //Set where reading at slave 1 starts
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV1_CTRL, 0x81);  //Enable at set length to 1    
+//   MPU9150_writeSensor(0x2A, 0x81); //Enable at set length to 1
+
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV1_DO, 0x01);   //overvride register    
+//   MPU9150_writeSensor(0x64, 0x01); //overvride register
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_MST_DELAY_CTRL, 0x03);   //set delay rate
+//   MPU9150_writeSensor(0x67, 0x03); //set delay rate
+    I2Cdev::writeByte(devAddr, 0x01, 0x80);
+//   MPU9150_writeSensor(0x01, 0x80);
+
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV4_CTRL, 0x04);  //set i2c slv4 delay
+//   MPU9150_writeSensor(0x34, 0x04); //set i2c slv4 delay
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV1_DO, 0x00);   //overvride register    
+//   MPU9150_writeSensor(0x64, 0x00); //override register
+    I2Cdev::writeByte(devAddr, MPU9150_RA_USER_CTRL, 0x00);   //clear usr setting
+//   MPU9150_writeSensor(0x6A, 0x00); //clear usr setting
+    I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV1_DO, 0x01);   //overvride register    
+//   MPU9150_writeSensor(0x64, 0x01); //override register
+    I2Cdev::writeByte(devAddr, MPU9150_RA_USER_CTRL, 0x20);   //enable master i2c mode
+//   MPU9150_writeSensor(0x6A, 0x20); //enable master i2c mode
+      I2Cdev::writeByte(devAddr, MPU9150_RA_I2C_SLV4_CTRL, 0x13);  //disable i2c slv4 
+//   MPU9150_writeSensor(0x34, 0x13); //disable slv4 
+}
+
+/** Get raw 9-axis motion sensor readings (accel/gyro/compass).
+ * FUNCTION NOT FULLY IMPLEMENTED YET.
+ * @param ax 16-bit signed integer container for accelerometer X-axis value
+ * @param ay 16-bit signed integer container for accelerometer Y-axis value
+ * @param az 16-bit signed integer container for accelerometer Z-axis value
+ * @param gx 16-bit signed integer container for gyroscope X-axis value
+ * @param gy 16-bit signed integer container for gyroscope Y-axis value
+ * @param gz 16-bit signed integer container for gyroscope Z-axis value
+ * @param mx 16-bit signed integer container for magnetometer X-axis value
+ * @param my 16-bit signed integer container for magnetometer Y-axis value
+ * @param mz 16-bit signed integer container for magnetometer Z-axis value
+ * @see getMotion6()
+ * @see getAcceleration()
+ * @see getRotation()
+ * @see MPU9150_RA_ACCEL_XOUT_H
+ */
+
+#define MPU9150_CMPS_XOUT_L        0x4A   // R
+#define MPU9150_CMPS_XOUT_H        0x4B   // R
+#define MPU9150_CMPS_YOUT_L        0x4C   // R
+#define MPU9150_CMPS_YOUT_H        0x4D   // R
+#define MPU9150_CMPS_ZOUT_L        0x4E   // R
+#define MPU9150_CMPS_ZOUT_H        0x4F   // R
+
+void MPU9150::getMotion9new(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
+
+    //get accel and gyro
+    getMotion6(ax, ay, az, gx, gy, gz);
+
+    I2Cdev::readBytes(devAddr, MPU9150_CMPS_XOUT_L, 6, buffer);
+
+    *mx = (((int16_t)buffer[1]) << 8) | buffer[0]; // low and high other way around from others
+    *my = (((int16_t)buffer[3]) << 8) | buffer[2];
+    *mz = (((int16_t)buffer[5]) << 8) | buffer[4];
+}
+
+void MPU9150::getMotion6buffer(int8_t* inbuffer ) {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_ACCEL_XOUT_H, 14, buffer);
+    uint8_t k = 0;
+    for ( uint8_t i=0; i<6; i++ ){
+	inbuffer[k] = buffer[i];
+	k++;
+    }
+    for ( uint8_t i=8; i<14; i++ ){
+	inbuffer[k] = buffer[i];
+	k++;
+    }
+}
+
+void MPU9150::getCompassBuffer(int8_t * inbuffer) {
+
+    //get accel and gyro
+//     getMotion6(ax, ay, az, gx, gy, gz);
+
+    I2Cdev::readBytes(devAddr, MPU9150_CMPS_XOUT_L, 6, buffer);
+
+    for ( uint8_t i=0; i<6; i++ ){
+	inbuffer[i] = buffer[i];
+    }
+
+//     *mx = (((int16_t)buffer[0]) << 8) | buffer[1];
+//     *my = (((int16_t)buffer[2]) << 8) | buffer[3];
+//     *mz = (((int16_t)buffer[4]) << 8) | buffer[5];
+}

Arduino/MPU9150/MPU9150.h

@@ -593,6 +593,12 @@ class MPU9150 {
         bool getIntI2CMasterStatus();
         bool getIntDataReadyStatus();
 
+	void setupCompas();
+	void getMotion9new(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
+
+	void getCompassBuffer(int8_t * inbuffer);
+	void getMotion6buffer(int8_t * inbuffer );
+
         // ACCEL_*OUT_* registers
         void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
         void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);