mpu9150: rewrite from scratch

This driver has been rewritten from scratch.  It is implemented as 3
seperate drivers now (but all included as part of the mpu9150 UPM
library):

AK8975 (Magnetometer)
MPU60X0 (Accelerometer, Gyroscope, and Temperature sensor)
MPU9150 (composed of AK8975 and MPU60X0)

Each driver can be used independently and includes examples in
C++/JS/Python.

Commonly used capabilities are supported, and methods/register
definitions exist to easily implement any desired functionality that
is missing.  Interrupt support has also been added.

Scaling support has also been properly implemented for both the
Accelerometer and Gyroscope.

Signed-off-by: Jon Trulson <jtrulson@ics.com>
Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>

examples/c++/CMakeLists.txt

@@ -129,6 +129,8 @@ add_executable (hp20x-example hp20x.cxx)
 add_executable (pn532-example pn532.cxx)
 add_executable (pn532-writeurl-example pn532-writeurl.cxx)
 add_executable (sainsmartks-example sainsmartks.cxx)
+add_executable (mpu60x0-example mpu60x0.cxx)
+add_executable (ak8975-example ak8975.cxx)
 
 include_directories (${PROJECT_SOURCE_DIR}/src/hmc5883l)
 include_directories (${PROJECT_SOURCE_DIR}/src/grove)
@@ -364,3 +366,5 @@ target_link_libraries (hp20x-example hp20x ${CMAKE_THREAD_LIBS_INIT})
 target_link_libraries (pn532-example pn532 ${CMAKE_THREAD_LIBS_INIT})
 target_link_libraries (pn532-writeurl-example pn532 ${CMAKE_THREAD_LIBS_INIT})
 target_link_libraries (sainsmartks-example i2clcd ${CMAKE_THREAD_LIBS_INIT})
+target_link_libraries (mpu60x0-example mpu9150 ${CMAKE_THREAD_LIBS_INIT})
+target_link_libraries (ak8975-example mpu9150 ${CMAKE_THREAD_LIBS_INIT})

examples/c++/ak8975.cxx

@@ -0,0 +1,72 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <iostream>
+#include <signal.h>
+#include "mpu9150.h"
+
+using namespace std;
+
+int shouldRun = true;
+
+void sig_handler(int signo)
+{
+  if (signo == SIGINT)
+    shouldRun = false;
+}
+
+
+int main(int argc, char **argv)
+{
+  signal(SIGINT, sig_handler);
+//! [Interesting]
+
+  upm::AK8975 *sensor = new upm::AK8975();
+
+  sensor->init();
+
+  while (shouldRun)
+    {
+      sensor->update();
+      
+      float x, y, z;
+      
+      sensor->getMagnetometer(&x, &y, &z);
+      cout << "Magnetometer:  ";
+      cout << "MX = " << x << " MY = " << y << " MZ = " << z << endl;
+
+      cout << endl;
+
+      usleep(500000);
+    }
+
+//! [Interesting]
+
+  cout << "Exiting..." << endl;
+  
+  delete sensor;
+  
+  return 0;
+}

examples/c++/mpu60x0.cxx

@@ -0,0 +1,77 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <iostream>
+#include <signal.h>
+#include "mpu9150.h"
+
+using namespace std;
+
+int shouldRun = true;
+
+void sig_handler(int signo)
+{
+  if (signo == SIGINT)
+    shouldRun = false;
+}
+
+
+int main(int argc, char **argv)
+{
+  signal(SIGINT, sig_handler);
+//! [Interesting]
+
+  upm::MPU60X0 *sensor = new upm::MPU60X0();
+
+  sensor->init();
+
+  while (shouldRun)
+    {
+      sensor->update();
+      
+      float x, y, z;
+      
+      sensor->getAccelerometer(&x, &y, &z);
+      cout << "Accelerometer: ";
+      cout << "AX: " << x << " AY: " << y << " AZ: " << z << endl;
+
+      sensor->getGyroscope(&x, &y, &z);
+      cout << "Gryoscope:     ";
+      cout << "GX: " << x << " GY: " << y << " GZ: " << z << endl;
+      
+      cout << "Temperature:   " << sensor->getTemperature() << endl;
+      cout << endl;
+
+      usleep(500000);
+    }
+
+//! [Interesting]
+
+  cout << "Exiting..." << endl;
+  
+  delete sensor;
+  
+  return 0;
+}

examples/c++/mpu9150.cxx

@@ -1,6 +1,6 @@
 /*
- * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
- * Copyright (c) 2014 Intel Corporation.
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -24,36 +24,58 @@
 
 #include <unistd.h>
 #include <iostream>
+#include <signal.h>
 #include "mpu9150.h"
 
-int
-main(int argc, char **argv)
+using namespace std;
+
+int shouldRun = true;
+
+void sig_handler(int signo)
 {
-    //! [Interesting]
-    upm::Vector3D data;
-    upm::MPU9150 *sensor = new upm::MPU9150(0, ADDR);
-    sensor->getData ();
-    sensor->getAcceleromter (&data);
-    std::cout << "*************************************************" << std::endl;
-    std::cout << "DEVICE ID (" << (int) sensor->getDeviceID () << ")" << std::endl;
-    std::cout << "*************************************************" << std::endl;
-    std::cout << "ACCELEROMETER :: X (" << data.axisX << ")" << " Y (" << data.axisY << ")"
-                << " Z (" << data.axisZ << ")" << std::endl;
-
-    sensor->getGyro (&data);
-    std::cout << "GYRO :: X (" << data.axisX << ")" << " Y (" << data.axisY << ")"
-                << " Z (" << data.axisZ << ")" << std::endl;
-
-    sensor->getMagnometer (&data);
-    std::cout << "MAGNOMETER :: X (" << data.axisX << ")" << " Y (" << data.axisY << ")"
-                << " Z (" << data.axisZ << ")" << std::endl;
-    std::cout << "TEMPERATURE (" << sensor->getTemperature () << ")" << std::endl;
-    std::cout << "*************************************************" << std::endl;
-    //! [Interesting]
-
-    std::cout << "exiting application" << std::endl;
-
-    delete sensor;
-
-    return 0;
+  if (signo == SIGINT)
+    shouldRun = false;
+}
+
+
+int main(int argc, char **argv)
+{
+  signal(SIGINT, sig_handler);
+//! [Interesting]
+
+  upm::MPU9150 *sensor = new upm::MPU9150();
+
+  sensor->init();
+
+  while (shouldRun)
+    {
+      sensor->update();
+      
+      float x, y, z;
+      
+      sensor->getAccelerometer(&x, &y, &z);
+      cout << "Accelerometer: ";
+      cout << "AX: " << x << " AY: " << y << " AZ: " << z << endl;
+
+      sensor->getGyroscope(&x, &y, &z);
+      cout << "Gryoscope:     ";
+      cout << "GX: " << x << " GY: " << y << " GZ: " << z << endl;
+      
+      sensor->getMagnetometer(&x, &y, &z);
+      cout << "Magnetometer:  ";
+      cout << "MX = " << x << " MY = " << y << " MZ = " << z << endl;
+
+      cout << "Temperature:   " << sensor->getTemperature() << endl;
+      cout << endl;
+
+      usleep(500000);
+    }
+
+//! [Interesting]
+
+  cout << "Exiting..." << endl;
+  
+  delete sensor;
+  
+  return 0;
 }

examples/javascript/ak8975.js

@@ -0,0 +1,64 @@
+/*jslint node:true, vars:true, bitwise:true, unparam:true */
+/*jshint unused:true */
+
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+var sensorObj = require('jsupm_mpu9150');
+
+// Instantiate an AK8975 on default I2C bus and address
+var sensor = new sensorObj.AK8975();
+
+// Initialize the device with default values
+sensor.init();
+
+var x = new sensorObj.new_floatp();
+var y = new sensorObj.new_floatp();
+var z = new sensorObj.new_floatp();
+
+// Output data every half second until interrupted
+setInterval(function()
+{
+    sensor.update();
+    
+    sensor.getMagnetometer(x, y, z);
+    console.log("Magnetometer:  MX: " + sensorObj.floatp_value(x) + 
+                " MY: " + sensorObj.floatp_value(y) + 
+                " MZ: " + sensorObj.floatp_value(z));
+
+    console.log();
+
+}, 500);
+
+// exit on ^C
+process.on('SIGINT', function()
+{
+    sensor = null;
+    sensorObj.cleanUp();
+    sensorObj = null;
+    console.log("Exiting.");
+    process.exit(0);
+});
+

examples/javascript/mpu60x0.js

@@ -0,0 +1,71 @@
+/*jslint node:true, vars:true, bitwise:true, unparam:true */
+/*jshint unused:true */
+
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+var sensorObj = require('jsupm_mpu9150');
+
+// Instantiate an MPU60X0 on default I2C bus and address
+var sensor = new sensorObj.MPU60X0();
+
+// Initialize the device with default values
+sensor.init();
+
+var x = new sensorObj.new_floatp();
+var y = new sensorObj.new_floatp();
+var z = new sensorObj.new_floatp();
+
+// Output data every half second until interrupted
+setInterval(function()
+{
+    sensor.update();
+    
+    sensor.getAccelerometer(x, y, z);
+    console.log("Accelerometer: AX: " + sensorObj.floatp_value(x) + 
+                " AY: " + sensorObj.floatp_value(y) + 
+                " AZ: " + sensorObj.floatp_value(z));
+
+    sensor.getGyroscope(x, y, z);
+    console.log("Gyroscope:     GX: " + sensorObj.floatp_value(x) + 
+                " AY: " + sensorObj.floatp_value(y) + 
+                " AZ: " + sensorObj.floatp_value(z));
+ 
+    console.log("Temperature:   " + sensor.getTemperature());
+ 
+    console.log();
+
+}, 500);
+
+// exit on ^C
+process.on('SIGINT', function()
+{
+    sensor = null;
+    sensorObj.cleanUp();
+    sensorObj = null;
+    console.log("Exiting.");
+    process.exit(0);
+});
+

examples/javascript/mpu9150.js

@@ -0,0 +1,76 @@
+/*jslint node:true, vars:true, bitwise:true, unparam:true */
+/*jshint unused:true */
+
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+var sensorObj = require('jsupm_mpu9150');
+
+// Instantiate an MPU9105 on default I2C bus and address
+var sensor = new sensorObj.MPU9150();
+
+// Initialize the device with default values
+sensor.init();
+
+var x = new sensorObj.new_floatp();
+var y = new sensorObj.new_floatp();
+var z = new sensorObj.new_floatp();
+
+// Output data every half second until interrupted
+setInterval(function()
+{
+    sensor.update();
+    
+    sensor.getAccelerometer(x, y, z);
+    console.log("Accelerometer: AX: " + sensorObj.floatp_value(x) + 
+                " AY: " + sensorObj.floatp_value(y) + 
+                " AZ: " + sensorObj.floatp_value(z));
+
+    sensor.getGyroscope(x, y, z);
+    console.log("Gyroscope:     GX: " + sensorObj.floatp_value(x) + 
+                " AY: " + sensorObj.floatp_value(y) + 
+                " AZ: " + sensorObj.floatp_value(z));
+ 
+    sensor.getMagnetometer(x, y, z);
+    console.log("Magnetometer:  MX: " + sensorObj.floatp_value(x) + 
+                " MY: " + sensorObj.floatp_value(y) + 
+                " MZ: " + sensorObj.floatp_value(z));
+
+    console.log("Temperature:   " + sensor.getTemperature());
+ 
+    console.log();
+
+}, 500);
+
+// exit on ^C
+process.on('SIGINT', function()
+{
+    sensor = null;
+    sensorObj.cleanUp();
+    sensorObj = null;
+    console.log("Exiting.");
+    process.exit(0);
+});
+

examples/python/ak8975.py

@@ -0,0 +1,59 @@
+#!/usr/bin/python
+# Author: Jon Trulson <jtrulson@ics.com>
+# Copyright (c) 2015 Intel Corporation.
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files (the
+# "Software"), to deal in the Software without restriction, including
+# without limitation the rights to use, copy, modify, merge, publish,
+# distribute, sublicense, and/or sell copies of the Software, and to
+# permit persons to whom the Software is furnished to do so, subject to
+# the following conditions:
+#
+# The above copyright notice and this permission notice shall be
+# included in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+# LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+# OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+import time, sys, signal, atexit
+import pyupm_mpu9150 as sensorObj
+
+# Instantiate an AK8975 on I2C bus 0
+sensor = sensorObj.AK8975()
+
+## Exit handlers ##
+# This function stops python from printing a stacktrace when you hit control-C
+def SIGINTHandler(signum, frame):
+	raise SystemExit
+
+# This function lets you run code on exit
+def exitHandler():
+	print "Exiting"
+	sys.exit(0)
+
+# Register exit handlers
+atexit.register(exitHandler)
+signal.signal(signal.SIGINT, SIGINTHandler)
+
+sensor.init()
+
+x = sensorObj.new_floatp()
+y = sensorObj.new_floatp()
+z = sensorObj.new_floatp()
+
+while (1):
+        sensor.update()
+        sensor.getMagnetometer(x, y, z)
+        print "Magnetometer:  MX: ", sensorObj.floatp_value(x), 
+        print " MY: ", sensorObj.floatp_value(y),
+        print " MZ: ", sensorObj.floatp_value(z)
+
+        print
+
+	time.sleep(.5)

examples/python/mpu60x0.py

@@ -0,0 +1,65 @@
+#!/usr/bin/python
+# Author: Jon Trulson <jtrulson@ics.com>
+# Copyright (c) 2015 Intel Corporation.
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files (the
+# "Software"), to deal in the Software without restriction, including
+# without limitation the rights to use, copy, modify, merge, publish,
+# distribute, sublicense, and/or sell copies of the Software, and to
+# permit persons to whom the Software is furnished to do so, subject to
+# the following conditions:
+#
+# The above copyright notice and this permission notice shall be
+# included in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+# LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+# OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+import time, sys, signal, atexit
+import pyupm_mpu9150 as sensorObj
+
+# Instantiate an MPU60X0 on I2C bus 0
+sensor = sensorObj.MPU60X0()
+
+## Exit handlers ##
+# This function stops python from printing a stacktrace when you hit control-C
+def SIGINTHandler(signum, frame):
+	raise SystemExit
+
+# This function lets you run code on exit
+def exitHandler():
+	print "Exiting"
+	sys.exit(0)
+
+# Register exit handlers
+atexit.register(exitHandler)
+signal.signal(signal.SIGINT, SIGINTHandler)
+
+sensor.init()
+
+x = sensorObj.new_floatp()
+y = sensorObj.new_floatp()
+z = sensorObj.new_floatp()
+
+while (1):
+        sensor.update()
+        sensor.getAccelerometer(x, y, z)
+        print "Accelerometer: AX: ", sensorObj.floatp_value(x), 
+        print " AY: ", sensorObj.floatp_value(y),
+        print " AZ: ", sensorObj.floatp_value(z)
+
+        sensor.getGyroscope(x, y, z)
+        print "Gyroscope:     GX: ", sensorObj.floatp_value(x), 
+        print " GY: ", sensorObj.floatp_value(y),
+        print " GZ: ", sensorObj.floatp_value(z)
+
+        print "Temperature:  ", sensor.getTemperature()
+        print
+
+	time.sleep(.5)

examples/python/mpu9150.py

@@ -0,0 +1,70 @@
+#!/usr/bin/python
+# Author: Jon Trulson <jtrulson@ics.com>
+# Copyright (c) 2015 Intel Corporation.
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files (the
+# "Software"), to deal in the Software without restriction, including
+# without limitation the rights to use, copy, modify, merge, publish,
+# distribute, sublicense, and/or sell copies of the Software, and to
+# permit persons to whom the Software is furnished to do so, subject to
+# the following conditions:
+#
+# The above copyright notice and this permission notice shall be
+# included in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+# LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+# OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+import time, sys, signal, atexit
+import pyupm_mpu9150 as sensorObj
+
+# Instantiate an MPU9150 on I2C bus 0
+sensor = sensorObj.MPU9150()
+
+## Exit handlers ##
+# This function stops python from printing a stacktrace when you hit control-C
+def SIGINTHandler(signum, frame):
+	raise SystemExit
+
+# This function lets you run code on exit
+def exitHandler():
+	print "Exiting"
+	sys.exit(0)
+
+# Register exit handlers
+atexit.register(exitHandler)
+signal.signal(signal.SIGINT, SIGINTHandler)
+
+sensor.init()
+
+x = sensorObj.new_floatp()
+y = sensorObj.new_floatp()
+z = sensorObj.new_floatp()
+
+while (1):
+        sensor.update()
+        sensor.getAccelerometer(x, y, z)
+        print "Accelerometer: AX: ", sensorObj.floatp_value(x), 
+        print " AY: ", sensorObj.floatp_value(y),
+        print " AZ: ", sensorObj.floatp_value(z)
+
+        sensor.getGyroscope(x, y, z)
+        print "Gyroscope:     GX: ", sensorObj.floatp_value(x), 
+        print " GY: ", sensorObj.floatp_value(y),
+        print " GZ: ", sensorObj.floatp_value(z)
+
+        sensor.getMagnetometer(x, y, z)
+        print "Magnetometer:  MX: ", sensorObj.floatp_value(x), 
+        print " MY: ", sensorObj.floatp_value(y),
+        print " MZ: ", sensorObj.floatp_value(z)
+
+        print "Temperature:  ", sensor.getTemperature()
+        print
+
+	time.sleep(.5)

src/mpu9150/CMakeLists.txt

@@ -1,5 +1,5 @@
 set (libname "mpu9150")
-set (libdescription "giro, acceleromter and magnometer sensor based on mpu9150")
-set (module_src ${libname}.cxx)
-set (module_h ${libname}.h)
+set (libdescription "gyro, acceleromter and magnometer sensor based on mpu9150")
+set (module_src ${libname}.cxx ak8975.cxx mpu60x0.cxx)
+set (module_h ${libname}.h ak8975.h mpu60x0.h)
 upm_module_init()

src/mpu9150/ak8975.cxx

@@ -0,0 +1,232 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <iostream>
+#include <string>
+
+#include "ak8975.h"
+
+using namespace upm;
+using namespace std;
+
+
+AK8975::AK8975(int bus, uint8_t address):
+  m_i2c(bus)
+{
+  m_addr = address;
+  m_xCoeff = 0.0;
+  m_yCoeff = 0.0;
+  m_zCoeff = 0.0;
+
+  mraa_result_t rv;
+  if ( (rv = m_i2c.address(m_addr)) != MRAA_SUCCESS)
+    {
+      cerr << __FUNCTION__ << ": Could not initialize i2c address. " << endl;
+      mraa_result_print(rv);
+      return;
+    }
+}
+
+AK8975::~AK8975()
+{
+}
+
+bool AK8975::init()
+{
+  // we put the device in 'fuse mode', and then read the compensation
+  // coefficients and store them.
+
+  // first, set power down mode
+
+  if (!setMode(CNTL_PWRDWN))
+    {
+      cerr << __FUNCTION__ << ": Unable to set PWRDWN mode" << endl;
+      return false;
+    }
+
+  if (!setMode(CNTL_FUSE_ACCESS))
+    {
+      cerr << __FUNCTION__ << ": Unable to set FUSE mode" << endl;
+      return false;
+    }
+
+  // Read each byte and store
+  m_xCoeff = (float)m_i2c.readReg(REG_ASAX);
+  m_yCoeff = (float)m_i2c.readReg(REG_ASAY);
+  m_zCoeff = (float)m_i2c.readReg(REG_ASAZ);
+
+  // now, place back in power down mode
+  if (!setMode(CNTL_PWRDWN))
+    {
+      cerr << __FUNCTION__ << ": Unable to reset PWRDWN mode" << endl;
+      return false;
+    }
+
+  return true;
+}
+
+bool AK8975::setMode(CNTL_MODES_T mode)
+{
+  mraa_result_t rv;
+  if ((rv = m_i2c.writeReg(REG_CNTL, mode)) != MRAA_SUCCESS)
+    {
+      cerr << __FUNCTION__ << ": failed:" << endl;
+      mraa_result_print(rv);
+      return false;
+    } 
+
+  // sleep at least 100us for for mode transition to complete
+  usleep(150);
+
+  return true;
+}
+
+bool AK8975::isReady()
+{
+  uint8_t rdy = m_i2c.readReg(REG_ST1);
+  
+  if (rdy & ST1_DRDY)
+    return true;
+
+  return false;
+}
+
+bool AK8975::waitforDeviceReady()
+{
+  const int maxRetries = 20;
+
+  int retries = 0;
+
+  while (retries < maxRetries)
+    {
+      if (isReady())
+        return true;
+
+      usleep(5000);
+      retries++;
+    }
+  
+  cerr << __FUNCTION__ << ": timeout waiting for device to become ready"
+       << endl;
+
+  return false;
+}
+
+bool AK8975::update(bool selfTest)
+{
+  // this flag (selfTest) is used so that we can read values without
+  // specifically taking a measurement.  For example, selfTest will
+  // pass true to this method so that the test results aren't
+  // overwritten by a measurement.
+  if (!selfTest)
+    {
+      // First set measurement mode (take a measurement)
+      if (!setMode(CNTL_MEASURE))
+        {
+          cerr << __FUNCTION__ << ": Unable to set MEASURE mode" << endl;
+          return false;
+        }
+    }
+
+  if (!waitforDeviceReady())
+    return false;
+
+  // hope it worked.  Now read out the values and store them (uncompensated)
+  uint8_t data[6];
+  m_i2c.readBytesReg(REG_HXL, data, 6);
+
+  int16_t x, y, z;
+  x = ( (data[1] << 8) | data[0] );
+  y = ( (data[3] << 8) | data[2] );
+  z = ( (data[5] << 8) | data[4] );
+
+  m_xData = float(x);
+  m_yData = float(y);
+  m_zData = float(z);
+
+  return true;
+}
+
+bool AK8975::selfTest()
+{
+  mraa_result_t rv;
+
+  // set power down first
+  if (!setMode(CNTL_PWRDWN))
+    {
+      cerr << __FUNCTION__ << ": Unable to set PWRDWN mode" << endl;
+      return false;
+    }
+
+  // enable self test bit
+  if ((rv = m_i2c.writeReg(REG_ASTC, ASTC_SELF)) != MRAA_SUCCESS)
+    {
+      cerr << __FUNCTION__ << ": failed to enable self test:" << endl;
+      mraa_result_print(rv);
+      return false;
+    } 
+  
+  // now set self test mode
+  if (!setMode(CNTL_SELFTEST))
+    {
+      cerr << __FUNCTION__ << ": Unable to set SELFTEST mode" << endl;
+      return false;
+    }
+  
+  // now update current data (without enabling a measurement)
+  update(true);
+  
+  // Now, reset self test register
+  uint8_t reg = m_i2c.readReg(REG_ASTC);
+  reg &= ~ASTC_SELF;
+  if ((rv = m_i2c.writeReg(REG_ASTC, reg)) != MRAA_SUCCESS)
+    {
+      cerr << __FUNCTION__ << ": failed to disable self test:" << endl;
+      mraa_result_print(rv);
+      return false;
+    } 
+
+  // after self-test measurement, device transitions to power down mode
+  return true;
+}
+
+float AK8975::adjustValue(float value, float adj)
+{
+  // apply the proper compensation to value.  This equation is taken
+  // from the AK8975 datasheet, section 8.3.11
+
+  return ( value * ((((adj - 128.0) * 0.5) / 128.0) + 1.0) );
+} 
+
+void AK8975::getMagnetometer(float *x, float *y, float *z)
+{
+  if (x)
+    *x = adjustValue(m_xData, m_xCoeff);
+  if (y)
+    *y = adjustValue(m_yData, m_yCoeff);
+  if (z)
+    *z = adjustValue(m_zData, m_zCoeff);
+}
+

src/mpu9150/ak8975.h

@@ -0,0 +1,233 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <string>
+#include <mraa/i2c.hpp>
+
+#define AK8975_I2C_BUS 0
+#define AK8975_DEFAULT_I2C_ADDR 0x0c
+
+namespace upm {
+  
+  /**
+   * @library mpu9150
+   * @sensor ak8975
+   * @comname AK8975 3-axis Magnetometer
+   * @altname AK9875
+   * @type compass
+   * @man grove
+   * @con i2c
+   *
+   * @brief API for the AK8975 magnetometer
+   *
+   * This is a 3-axis magnetometer, which can be used alone, or
+   * coupled with another device (such as the mcu9150 9-axis motion
+   * sensor). 
+   *
+   * @snippet ak8975.cxx Interesting
+   */
+  class AK8975 {
+  public:
+
+    /**
+     * AK8975 registers
+     */
+    typedef enum {
+      REG_WIA                   = 0x00, // device id
+
+      REG_INFO                  = 0x01, // undocumented (AK proprietary data)
+
+      REG_ST1                   = 0x02, // status 1
+
+      REG_HXL                   = 0x03, // magnetometer data, X axis low byte
+      REG_HXH                   = 0x04, // magnetometer data, X axis high byte
+      REG_HYL                   = 0x05,
+      REG_HYH                   = 0x06,
+      REG_HZL                   = 0x07,
+      REG_HZH                   = 0x08,
+
+      REG_ST2                   = 0x09, // status 2
+
+      REG_CNTL                  = 0x0a, // control
+
+      // REG_RSV 0x0b reserved
+
+      REG_ASTC                  = 0x0c, // self test (internal mag field)
+
+      // REG_TS1, REG_TS2 0x0d, 0x0e reserved/factory test
+
+      // REG_I2CDIS 0x0f, I2C disable.  Not a good idea to use or support.
+      //  write a 0x1b to disable i2c.  This requires a power cycle to undo.
+
+      // These registers hold factory calibrated co-efficients needed to
+      // properly compensate for production variations.  They can only be
+      // read when device is in fuse mode.  They are used to adjust the
+      // measured mag field values.
+      REG_ASAX                  = 0x10, // X calibration
+      REG_ASAY                  = 0x11,
+      REG_ASAZ                  = 0x12
+    } AK8975_REG_T;
+    
+    /**
+     * ST1 bits
+     */
+    typedef enum {
+      ST1_DRDY                  = 0x01 // data ready bit
+    } ST1_BITS_T;
+    
+    /**
+     * ST2 bits
+     */
+    typedef enum {
+      ST2_DERR                  = 0x04, // data error
+      ST2_HOFL                  = 0x08  // measurement overflow
+    } ST2_BITS_T;
+    
+    /**
+     * CNTL register, operating mode values
+     */
+    typedef enum {
+      CNTL_PWRDWN               = 0x00, // power down
+      CNTL_MEASURE              = 0x01, // single measurement
+      CNTL_SELFTEST             = 0x08,
+      CNTL_FUSE_ACCESS          = 0x0f  // access fuse (coeff) registers
+    } CNTL_MODES_T;
+    
+    /**
+     * ASTC (self test control) bits
+     */
+    typedef enum {
+      ASTC_SELF                 = 0x40 // enable self test
+    } ASTC_BITS_T;
+
+    /**
+     * ak8975 constructor
+     *
+     * @param bus i2c bus to use
+     * @param address the address for this device
+     */
+    AK8975(int bus=AK8975_I2C_BUS, uint8_t address=AK8975_DEFAULT_I2C_ADDR);
+
+    /**
+     * AK8975 Destructor
+     */
+    ~AK8975();
+    
+    /**
+     * set up initial values and start operation
+     *
+     * @param dsr the data sampling rate: one of the DSR_BITS_T values
+     * @return true if successful
+     */
+    bool init();
+
+    /**
+     * put the chip into a specific mode
+     *
+     * @param mode one of the CNTL_MODES_T values
+     * @return true if successful
+     */
+    bool setMode(CNTL_MODES_T mode);
+
+    /**
+     * check to see if the ST1_DRDY bit is set, indicating the device
+     * can accept commands
+     *
+     * @return true if device is ready, false otherwise
+     */
+    bool isReady();
+
+    /**
+     * check to see if device is ready and sleep/retry if not.
+     * Returns once device indicates it's ready.
+     *
+     * @return true if device is ready, false if retries exhausted
+     */
+    bool waitforDeviceReady();
+
+    /**
+     * take a measurement
+     *
+     * @param selfTest true if we are running a self test, false
+     * (default) otherwise.
+     * @return true if successful, false otherwise
+     */
+    bool update(bool selfTest=false);
+
+    /**
+     * do a self test sequence.  When self test is executed, the
+     * device activates internal calibrated magnets, and measures
+     * them, updating the measurement registers.  Once complete, the
+     * data can be read as usual (getMagnetometer()) and the returned
+     * values compared against the following limits to determine
+     * correctness:
+     *
+     * -100 < X < +100; -100 < Y < +100; -1000 < Z < -300
+     *
+     * @return true if successful, false otherwise
+     */
+    bool selfTest();
+
+    /**
+     * return the compensated values for the x, y, and z axes.  The
+     * unit of measurement is in micro-teslas (uT).
+     *
+     * @param x pointer to returned X axis value
+     * @param y pointer to returned Y axis value
+     * @param z pointer to returned Z axis value
+     */
+    void getMagnetometer(float *x, float *y, float *z);
+
+
+  protected:
+    /**
+     * compute a compensated magnetometer axis value, based on the raw
+     * axis value and a per-device, per-axis adjustment coefficient
+     * that was read and stored at init() time.
+     *
+     * @param value the raw axis value to compensate
+     * @param adj the adjustment coefficient 
+     * @return true if successful
+     */
+    float adjustValue(float value, float adj);
+
+    // compensation coefficients (factory set) for this device
+    float m_xCoeff;
+    float m_yCoeff;
+    float m_zCoeff;
+
+    // uncompensated magnetometer readings
+    float m_xData;
+    float m_yData;
+    float m_zData;
+
+  private:
+    mraa::I2c m_i2c;
+    uint8_t m_addr;
+
+  };
+}
+
+

src/mpu9150/jsupm_mpu9150.i

@@ -1,8 +1,25 @@
 %module jsupm_mpu9150
 %include "../upm.i"
+%include "cpointer.i"
+
+%pointer_functions(float, floatp);
 
 %{
     #include "mpu9150.h"
 %}
 
+%include "ak8975.h"
+%{
+    #include "ak8975.h"
+%}
+
+%include "mpu60x0.h"
+%{
+    #include "mpu60x0.h"
+%}
+
 %include "mpu9150.h"
+%{
+    #include "mpu9150.h"
+%}
+

src/mpu9150/mpu60x0.cxx

@@ -0,0 +1,405 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <iostream>
+#include <string.h>
+
+#include "mpu60x0.h"
+
+using namespace upm;
+using namespace std;
+
+
+MPU60X0::MPU60X0(int bus, uint8_t address) :
+  m_i2c(bus), m_gpioIRQ(0)
+{
+  m_addr = address;
+
+  m_accelX = 0.0;
+  m_accelY = 0.0;
+  m_accelZ = 0.0;
+  
+  m_gyroX = 0.0;
+  m_gyroY = 0.0;
+  m_gyroZ = 0.0;
+  
+  m_temp = 0.0;
+
+  m_accelScale = 1.0;
+  m_gyroScale = 1.0;
+
+  mraa_result_t rv;
+  if ( (rv = m_i2c.address(m_addr)) != MRAA_SUCCESS)
+    {
+      cerr << __FUNCTION__ << ": Could not initialize i2c address. " << endl;
+      mraa_result_print(rv);
+      return;
+    }
+}
+
+MPU60X0::~MPU60X0()
+{
+  uninstallISR();
+}
+
+bool MPU60X0::init()
+{
+  // first, take the device out of sleep mode
+  if (!setSleep(false))
+    {
+      cerr << __FUNCTION__ << ": Unable to wake up device" << endl;
+      return false;
+    }
+
+  // set the clock source to use the gyroscope PLL rather than the
+  // internal clock for stability
+  if (!setClockSource(PLL_XG))
+    {
+      cerr << __FUNCTION__ << ": Unable to set clock source" << endl;
+      return false;
+    }
+
+  usleep(5000);
+
+  // enable temperature measurement (default on power up, but let's be sure)
+  enableTemperatureSensor(true);
+
+  // set the gyro and accel scale bits to reasonable values
+  setGyroscopeScale(FS_500);
+  setAccelerometerScale(AFS_2);
+
+  // enable the DLPF
+  setDigitalLowPassFilter(DLPF_94_98);
+
+  // let things stabilize...
+  usleep(100000);
+
+  return true;
+}
+
+
+void MPU60X0::update()
+{
+  // read all of the sensor registers - accel, gyro, and temp
+  uint8_t buffer[14];
+
+  memset(buffer, 0, 14);
+  readRegs(REG_ACCEL_XOUT_H, buffer, 14);
+
+  int16_t ax, ay, az;
+  int16_t temp;
+  int16_t gx, gy, gz;
+
+  ax =  ( (buffer[0] << 8) | buffer[1] );
+  ay =  ( (buffer[2] << 8) | buffer[3] );
+  az =  ( (buffer[4] << 8) | buffer[5] );
+
+  temp = ( (buffer[6] << 8) | buffer[7] );
+
+  gx =  ( (buffer[8] << 8) | buffer[9] );
+  gy =  ( (buffer[10] << 8) | buffer[11] );
+  gz =  ( (buffer[12] << 8) | buffer[13] );
+
+  // now stash them
+  m_accelX = float(ax);
+  m_accelY = float(ay);
+  m_accelZ = float(az);
+
+  m_temp = float(temp);
+
+  m_gyroX = float(gx);
+  m_gyroY = float(gy);
+  m_gyroZ = float(gz);
+}
+
+uint8_t MPU60X0::readReg(uint8_t reg)
+{
+  return m_i2c.readReg(reg);
+}
+
+void MPU60X0::readRegs(uint8_t reg, uint8_t *buf, int len)
+{
+  m_i2c.readBytesReg(reg, buf, len);
+}
+
+bool MPU60X0::writeReg(uint8_t reg, uint8_t val)
+{
+  mraa_result_t rv;
+  if ((rv = m_i2c.writeReg(reg, val)) != MRAA_SUCCESS)
+    {
+      cerr << __FUNCTION__ << ": failed:" << endl;
+      mraa_result_print(rv);
+      return false;
+    } 
+  
+  return true;
+}
+
+bool MPU60X0::setSleep(bool enable)
+{
+  uint8_t reg = readReg(REG_PWR_MGMT_1);
+
+  if (enable)
+    reg |= PWR_SLEEP;
+  else
+    reg &= ~PWR_SLEEP;
+
+  return writeReg(REG_PWR_MGMT_1, reg);
+}
+
+bool MPU60X0::setClockSource(CLKSEL_T clk)
+{
+  uint8_t reg = readReg(REG_PWR_MGMT_1);
+
+  reg &= ~(_CLKSEL_MASK << _CLKSEL_SHIFT);
+
+  reg |= (clk << _CLKSEL_SHIFT);
+
+  return writeReg(REG_PWR_MGMT_1, reg);
+}
+
+bool MPU60X0::setGyroscopeScale(FS_SEL_T scale)
+{
+  uint8_t reg = readReg(REG_GYRO_CONFIG);
+
+  reg &= ~(_FS_SEL_MASK << _FS_SEL_SHIFT);
+
+  reg |= (scale << _FS_SEL_SHIFT);
+
+  if (!writeReg(REG_GYRO_CONFIG, reg))
+    {
+      return false;
+    }
+
+  // store scaling factor
+
+  switch (scale)
+    {
+    case FS_250:
+      m_gyroScale = 131.0;
+      break;
+
+    case FS_500:
+      m_gyroScale = 65.5;
+      break;
+
+    case FS_1000:
+      m_gyroScale = 32.8;
+      break;
+
+    case FS_2000:
+      m_gyroScale = 16.4;
+      break;
+
+    default: // should never occur, but...
+      m_gyroScale = 1.0;        // set a safe, though incorrect value
+      cerr << __FUNCTION__ << ": internal error, unsupported scale" << endl;
+      break;
+    }
+
+  return true;
+}
+
+bool MPU60X0::setAccelerometerScale(AFS_SEL_T scale)
+{
+  uint8_t reg = readReg(REG_ACCEL_CONFIG);
+
+  reg &= ~(_AFS_SEL_MASK << _AFS_SEL_SHIFT);
+
+  reg |= (scale << _AFS_SEL_SHIFT);
+
+  if (!writeReg(REG_ACCEL_CONFIG, reg))
+    {
+      return false;
+    }
+
+  // store scaling factor
+  
+  switch (scale)
+    {
+    case AFS_2:
+      m_accelScale = 16384.0;
+      break;
+
+    case AFS_4:
+      m_accelScale = 8192.0;
+      break;
+
+    case AFS_8:
+      m_accelScale = 4096.0;
+      break;
+
+    case AFS_16:
+      m_accelScale = 2048.0;
+      break;
+
+    default: // should never occur, but...
+      m_accelScale = 1.0;        // set a safe, though incorrect value
+      cerr << __FUNCTION__ << ": internal error, unsupported scale" << endl;
+      break;
+    }
+
+  return true;
+}
+
+bool MPU60X0::setDigitalLowPassFilter(DLPF_CFG_T dlp)
+{
+  uint8_t reg = readReg(REG_CONFIG);
+
+  reg &= ~(_CONFIG_DLPF_MASK << _CONFIG_DLPF_SHIFT);
+
+  reg |= (dlp << _CONFIG_DLPF_SHIFT);
+
+  return writeReg(REG_CONFIG, reg);
+}
+
+bool MPU60X0::setSampleRateDivider(uint8_t div)
+{
+  return writeReg(REG_SMPLRT_DIV, div);
+}
+
+uint8_t MPU60X0::getSampleRateDivider()
+{
+  return readReg(REG_SMPLRT_DIV);
+}
+
+void MPU60X0::getAccelerometer(float *x, float *y, float *z)
+{
+  if (x)
+    *x = m_accelX / m_accelScale;
+
+  if (y)
+    *y = m_accelY / m_accelScale;
+
+  if (z)
+    *z = m_accelZ / m_accelScale;
+}
+
+void MPU60X0::getGyroscope(float *x, float *y, float *z)
+{
+  if (x)
+    *x = m_gyroX / m_gyroScale;
+
+  if (y)
+    *y = m_gyroY / m_gyroScale;
+
+  if (z)
+    *z = m_gyroZ / m_gyroScale;
+}
+
+float MPU60X0::getTemperature()
+{
+  // this equation is taken from the datasheet
+  return (m_temp / 340.0) + 36.53;
+}
+
+bool MPU60X0::enableTemperatureSensor(bool enable)
+{
+  uint8_t reg = readReg(REG_PWR_MGMT_1);
+
+  if (enable)
+    reg &= ~TEMP_DIS;
+  else
+    reg |= TEMP_DIS;
+
+  return writeReg(REG_PWR_MGMT_1, reg);
+}
+
+bool MPU60X0::setExternalSync(EXT_SYNC_SET_T val)
+{
+  uint8_t reg = readReg(REG_CONFIG);
+
+  reg &= ~(_CONFIG_EXT_SYNC_SET_MASK << _CONFIG_EXT_SYNC_SET_SHIFT);
+
+  reg |= (val << _CONFIG_EXT_SYNC_SET_SHIFT);
+
+  return writeReg(REG_CONFIG, reg);
+}
+
+bool MPU60X0::enableI2CBypass(bool enable)
+{
+  uint8_t reg = readReg(REG_INT_PIN_CFG);
+
+  if (enable)
+    reg |= I2C_BYPASS_ENABLE;
+  else
+    reg &= ~I2C_BYPASS_ENABLE;
+
+  return writeReg(REG_INT_PIN_CFG, reg);
+}
+
+bool MPU60X0::setMotionDetectionThreshold(uint8_t thr)
+{
+  return writeReg(REG_MOT_THR, thr);
+}
+
+uint8_t MPU60X0::getInterruptStatus()
+{
+  return readReg(REG_INT_STATUS);
+}
+
+bool MPU60X0::setInterruptEnables(uint8_t enables)
+{
+  return writeReg(REG_INT_ENABLE, enables);
+}
+
+uint8_t MPU60X0::getInterruptEnables()
+{
+  return readReg(REG_INT_ENABLE);
+}
+
+bool MPU60X0::setInterruptPinConfig(uint8_t cfg)
+{
+  return writeReg(REG_INT_PIN_CFG, cfg);
+}
+
+uint8_t MPU60X0::getInterruptPinConfig()
+{
+  return readReg(REG_INT_PIN_CFG);
+}
+
+void MPU60X0::installISR(int gpio, mraa::Edge level, 
+                         void (*isr)(void *), void *arg)
+{
+  // delete any existing ISR and GPIO context
+  uninstallISR();
+
+  // greate gpio context
+  m_gpioIRQ = new mraa::Gpio(gpio);
+
+  m_gpioIRQ->dir(mraa::DIR_IN);
+  m_gpioIRQ->isr(level, isr, arg);
+}
+
+void MPU60X0::uninstallISR()
+{
+  if (m_gpioIRQ)
+    {
+      m_gpioIRQ->isrExit();
+      delete m_gpioIRQ;
+      
+      m_gpioIRQ = 0;
+    }
+}

src/mpu9150/mpu60x0.h

@@ -0,0 +1,934 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <string>
+#include <mraa/i2c.hpp>
+#include <mraa/gpio.hpp>
+
+#define MPU60X0_I2C_BUS 0
+#define MPU60X0_DEFAULT_I2C_ADDR 0x68
+
+namespace upm {
+  
+  /**
+   * @library mpu9150
+   * @sensor mpu60x0
+   * @comname MPU60X0 3-axis Gyroscope and 3-axis Accelerometer
+   * @type accelerometer compass
+   * @man seeed
+   * @con i2c gpio
+   *
+   * @brief API for the MPU60X0 3-axis Gyroscope and 3-axis Accelerometer
+   *
+   * The MPU60X0 devices provide the world’s first integrated 6-axis
+   * motion processor solution that eliminates the package-level
+   * gyroscope and accelerometer cross-axis misalignment associated
+   * with discrete solutions. The devices combine a 3-axis gyroscope
+   * and a 3-axis accelerometer on the same silicon die.
+   *
+   * While not all of the functionality of this device is supported
+   * initially, methods and register definitions are provided that
+   * should allow an end user to implement whatever features are
+   * required.
+   *
+   * @snippet mpu60x0.cxx Interesting
+   */
+  class MPU60X0 {
+  public:
+
+    // NOTE: These enums were composed from both the mpu6050 and
+    // mpu9150 register maps, since this driver was written using an
+    // mpu9150, but we'd like this module to be usable with a
+    // standalone mpu60x0.
+    //
+    // Registers and bitfields marked with an '*' in their
+    // comment indicate registers or bit fields present in the mpu9150
+    // register map, but not in the original mpu6050 register map.  If
+    // using this module on a standalone mpu6050, you should avoid
+    // using those registers or bitfields marked with an *.
+
+    /**
+     * MPU60X0 registers
+     */
+    typedef enum {
+      REG_SELF_TEST_X           = 0x0d,
+      REG_SELF_TEST_Y           = 0x0e,
+      REG_SELF_TEST_Z           = 0x0f,
+      REG_SELF_TEST_A           = 0x10,
+
+      REG_SMPLRT_DIV            = 0x19, // sample rate divider
+
+      REG_CONFIG                = 0x1a,
+      REG_GYRO_CONFIG           = 0x1b,
+      REG_ACCEL_CONFIG          = 0x1c,
+
+      REG_FF_THR                = 0x1d, // *freefall threshold
+      REG_FF_DUR                = 0x1e, // *freefall duration
+
+      REG_MOT_THR               = 0x1f, // motion threshold
+      REG_MOT_DUR               = 0x20, // *motion duration
+
+      REG_ZRMOT_THR             = 0x21, // *zero motion threshhold
+      REG_ZRMOT_DUR             = 0x22, // *zero motion duration
+
+      REG_FIFO_EN               = 0x23,
+
+      REG_I2C_MST_CTRL          = 0x24, // I2C master control
+
+      REG_I2C_SLV0_ADDR         = 0x25, // I2C slave 0
+      REG_I2C_SLV0_REG          = 0x26,
+      REG_I2C_SLV0_CTRL         = 0x27,
+
+      REG_I2C_SLV1_ADDR         = 0x28, // I2C slave 1
+      REG_I2C_SLV1_REG          = 0x29,
+      REG_I2C_SLV1_CTRL         = 0x2a,
+
+      REG_I2C_SLV2_ADDR         = 0x2b, // I2C slave 2
+      REG_I2C_SLV2_REG          = 0x2c,
+      REG_I2C_SLV2_CTRL         = 0x2d,
+
+      REG_I2C_SLV3_ADDR         = 0x2e, // I2C slave 3
+      REG_I2C_SLV3_REG          = 0x2f,
+      REG_I2C_SLV3_CTRL         = 0x30,
+
+      REG_I2C_SLV4_ADDR         = 0x31, // I2C slave 4
+      REG_I2C_SLV4_REG          = 0x32,
+      REG_I2C_SLV4_DO           = 0x33,
+      REG_I2C_SLV4_CTRL         = 0x34,
+      REG_I2C_SLV4_DI           = 0x35,
+
+      REG_I2C_MST_STATUS        = 0x36, // I2C master status
+
+      REG_INT_PIN_CFG           = 0x37, // interrupt pin config/i2c bypass
+      REG_INT_ENABLE            = 0x38,
+
+      // 0x39 reserved
+
+      REG_INT_STATUS            = 0x3a, // interrupt status
+
+      REG_ACCEL_XOUT_H          = 0x3b, // accelerometer outputs
+      REG_ACCEL_XOUT_L          = 0x3c,
+
+      REG_ACCEL_YOUT_H          = 0x3d,
+      REG_ACCEL_YOUT_L          = 0x3e,
+
+      REG_ACCEL_ZOUT_H          = 0x3f,
+      REG_ACCEL_ZOUT_L          = 0x40,
+
+      REG_TEMP_OUT_H            = 0x41, // temperature output
+      REG_TEMP_OUT_L            = 0x42,
+
+      REG_GYRO_XOUT_H           = 0x43, // gyro outputs
+      REG_GYRO_XOUT_L           = 0x44,
+
+      REG_GYRO_YOUT_H           = 0x45,
+      REG_GYRO_YOUT_L           = 0x46,
+
+      REG_GYRO_ZOUT_H           = 0x47,
+      REG_GYRO_ZOUT_L           = 0x48,
+
+      REG_EXT_SENS_DATA_00      = 0x49, // external sensor data
+      REG_EXT_SENS_DATA_01      = 0x4a,
+      REG_EXT_SENS_DATA_02      = 0x4b,
+      REG_EXT_SENS_DATA_03      = 0x4c,
+      REG_EXT_SENS_DATA_04      = 0x4d,
+      REG_EXT_SENS_DATA_05      = 0x4e,
+      REG_EXT_SENS_DATA_06      = 0x4f,
+      REG_EXT_SENS_DATA_07      = 0x50,
+      REG_EXT_SENS_DATA_08      = 0x51,
+      REG_EXT_SENS_DATA_09      = 0x52,
+      REG_EXT_SENS_DATA_10      = 0x53,
+      REG_EXT_SENS_DATA_11      = 0x54,
+      REG_EXT_SENS_DATA_12      = 0x55,
+      REG_EXT_SENS_DATA_13      = 0x56,
+      REG_EXT_SENS_DATA_14      = 0x57,
+      REG_EXT_SENS_DATA_15      = 0x58,
+      REG_EXT_SENS_DATA_16      = 0x59,
+      REG_EXT_SENS_DATA_17      = 0x5a,
+      REG_EXT_SENS_DATA_18      = 0x5b,
+      REG_EXT_SENS_DATA_19      = 0x5c,
+      REG_EXT_SENS_DATA_20      = 0x5d,
+      REG_EXT_SENS_DATA_21      = 0x5e,
+      REG_EXT_SENS_DATA_22      = 0x5f,
+      REG_EXT_SENS_DATA_23      = 0x60,
+
+      REG_MOT_DETECT_STATUS     = 0x61, // *
+
+      // 0x62 reserved
+
+      REG_I2C_SLV0_DO           = 0x63, // I2C slave data outs
+      REG_I2C_SLV1_DO           = 0x64,
+      REG_I2C_SLV2_DO           = 0x65,
+      REG_I2C_SLV3_DO           = 0x66,
+
+      REG_I2C_MST_DELAY_CTRL    = 0x67,
+
+      REG_SIGNAL_PATH_RESET     = 0x68, // signal path resets
+
+      REG_MOT_DETECT_CTRL       = 0x69,
+
+      REG_USER_CTRL             = 0x6a,
+
+      REG_PWR_MGMT_1            = 0x6b, // power management
+      REG_PWR_MGMT_2            = 0x6c,
+
+      // 0x6d-0x71 reserved
+
+      REG_FIFO_COUNTH           = 0x72,
+      REG_FIFO_COUNTL           = 0x73,
+
+      REG_FIFO_R_W              = 0x74,
+
+      REG_WHO_AM_I              = 0x75
+    } MPU60X0_REG_T;
+    
+    /**
+     * CONFIG bits
+     */
+    typedef enum {
+      CONFIG_DLPF_CFG0            = 0x01, // digital low-pass filter config
+      CONFIG_DLPF_CFG1            = 0x02,
+      CONFIG_DLPF_CFG2            = 0x04,
+      _CONFIG_DLPF_SHIFT          = 0,
+      _CONFIG_DLPF_MASK           = 7,
+
+      CONFIG_EXT_SYNC_SET0        = 0x08, // FSYNC pin config
+      CONFIG_EXT_SYNC_SET1        = 0x10,
+      CONFIG_EXT_SYNC_SET2        = 0x20,
+      _CONFIG_EXT_SYNC_SET_SHIFT  = 3,
+      _CONFIG_EXT_SYNC_SET_MASK   = 7
+    } CONFIG_BITS_T;
+
+    /**
+     * CONFIG DLPF_CFG values
+     */
+    typedef enum {
+      DLPF_260_256                = 0, // accel/gyro bandwidth (Hz)
+      DLPF_184_188                = 1,
+      DLPF_94_98                  = 2,
+      DLPF_44_42                  = 3,
+      DLPF_21_20                  = 4,
+      DLPF_10_10                  = 5,
+      DLPF_5_5                    = 6,
+      DLPF_RESERVED               = 7
+    } DLPF_CFG_T;
+
+    /**
+     * CONFIG EXT_SYNC_SET values
+     */
+    typedef enum {
+      EXT_SYNC_DISABLED           = 0,
+      EXT_SYNC_TEMP_OUT           = 1,
+      EXT_SYNC_GYRO_XOUT          = 2,
+      EXT_SYNC_GYRO_YOUT          = 3,
+      EXT_SYNC_GYRO_ZOUT          = 4,
+      EXT_SYNC_ACCEL_XOUT         = 5,
+      EXT_SYNC_ACCEL_YOUT         = 6,
+      EXT_SYNC_ACCEL_ZOUT         = 7
+    } EXT_SYNC_SET_T;
+
+    /**
+     * GYRO_CONFIG bits
+     */
+    typedef enum {
+      // 0x01-0x04 reserved
+      FS_SEL0                          = 0x08, // gyro full scale range
+      FS_SEL1                          = 0x10,
+      _FS_SEL_SHIFT                    = 3,
+      _FS_SEL_MASK                     = 3,
+
+      ZG_ST                            = 0x20, // gyro self test bits
+      YG_ST                            = 0x40,
+      XG_ST                            = 0x80
+    } GRYO_CONFIG_BITS_T;
+
+    /**
+     * GYRO FS_SEL values
+     */
+    typedef enum {
+      FS_250                           = 0, // 250 deg/s, 131 LSB deg/s
+      FS_500                           = 1, // 500 deg/s, 65.5 LSB deg/s
+      FS_1000                          = 2, // 1000 deg/s, 32.8 LSB deg/s
+      FS_2000                          = 3  // 2000 deg/s, 16.4 LSB deg/s
+    } FS_SEL_T;
+
+    /**
+     * ACCEL_CONFIG bits
+     */
+    typedef enum {
+      // 0x01-0x04 reserved
+      AFS_SEL0                         = 0x08, // accel full scale range
+      AFS_SEL1                         = 0x10,
+      _AFS_SEL_SHIFT                   = 3,
+      _AFS_SEL_MASK                    = 3,
+
+      ZA_ST                            = 0x20, // gyro self test bits
+      YA_ST                            = 0x40,
+      XA_ST                            = 0x80
+    } ACCEL_CONFIG_BITS_T;
+
+    /**
+     * ACCEL AFS_SEL (full scaling) values
+     */
+    typedef enum {
+      AFS_2                            = 0, // 2g, 16384 LSB/g
+      AFS_4                            = 1, // 4g, 8192 LSB/g
+      AFS_8                            = 2, // 8g, 4096 LSB/g
+      AFS_16                           = 3  // 16g, 2048 LSB/g
+    } AFS_SEL_T;
+
+    /**
+     * REG_FIFO_EN bits
+     */
+    typedef enum {
+      SLV0_FIFO_EN                     = 0x01,
+      SLV1_FIFO_EN                     = 0x02,
+      SLV2_FIFO_EN                     = 0x04,
+
+      ACCEL_FIFO_EN                    = 0x08,
+
+      ZG_FIFO_EN                       = 0x10,
+      YG_FIFO_EN                       = 0x20,
+      XG_FIFO_EN                       = 0x40,
+
+      TEMP_FIFO_EN                     = 0x80
+    } FIFO_EN_BITS_T;
+
+    /**
+     * REG_I2C_MST_CTRL bits
+     */
+    typedef enum {
+      I2C_MST_CLK0                     = 0x01,
+      I2C_MST_CLK1                     = 0x02,
+      I2C_MST_CLK2                     = 0x04,
+      I2C_MST_CLK3                     = 0x08,
+      _I2C_MST_CLK_SHIFT               = 0,
+      _I2C_MST_CLK_MASK                = 15,
+
+      I2C_MST_P_NSR                    = 0x10,
+
+      SLV_3_FIFO_EN                    = 0x20,
+
+      WAIT_FOR_ES                      = 0x40,
+
+      MULT_MST_EN                      = 0x80
+    } I2C_MST_CTRL_BITS_T;
+
+    /**
+     * I2C_MST_CLK values
+     */
+    typedef enum {
+      MST_CLK_348                      = 0, // 348Khz
+      MST_CLK_333                      = 1,
+      MST_CLK_320                      = 2,
+      MST_CLK_308                      = 3,
+      MST_CLK_296                      = 4,
+      MST_CLK_286                      = 5,
+      MST_CLK_276                      = 6,
+      MST_CLK_267                      = 7,
+      MST_CLK_258                      = 8,
+      MST_CLK_500                      = 9,
+      MST_CLK_471                      = 10,
+      MST_CLK_444                      = 11,
+      MST_CLK_421                      = 12,
+      MST_CLK_400                      = 13,
+      MST_CLK_381                      = 14,
+      MST_CLK_364                      = 15
+    } I2C_MST_CLK_T;
+
+    /**
+     * REG_I2C SLV0-SLV4 _ADDR bits
+     */
+    typedef enum {
+      I2C_SLV_ADDR0                    = 0x01,
+      I2C_SLV_ADDR1                    = 0x02,
+      I2C_SLV_ADDR2                    = 0x04,
+      I2C_SLV_ADDR3                    = 0x08,
+      I2C_SLV_ADDR4                    = 0x10,
+      I2C_SLV_ADDR5                    = 0x20,
+      I2C_SLV_ADDR6                    = 0x40,
+      _I2C_SLV_ADDR_SHIFT              = 0,
+      _I2C_SLV_ADDR_MASK               = 127,
+
+      I2C_SLV_RW                       = 0x80
+    } I2C_SLV_ADDR_BITS_T;
+
+    /**
+     * REG_I2C SLV0-SLV3 _CTRL bits
+     */
+    typedef enum {
+      I2C_SLV_LEN0                     = 0x01,
+      I2C_SLV_LEN1                     = 0x02,
+      I2C_SLV_LEN2                     = 0x04,
+      I2C_SLV_LEN3                     = 0x08,
+      _I2C_SLV_LEN_SHIFT               = 0,
+      _I2C_SLV_LEN_MASK                = 15,
+
+      I2C_SLV_GRP                      = 0x10,
+      I2C_SLV_REG_DIS                  = 0x20,
+      I2C_SLV_BYTE_SW                  = 0x40,
+      I2C_SLV_EN                       = 0x80
+    } I2C_SLV_CTRL_BITS_T;
+
+    /**
+     * REG_I2C_SLV4_CTRL bits, these are different from the SLV0-SLV3
+     * CRTL bits.
+     *
+     * MST_DLY is not enumerated in the register map.  It configures
+     * the reduced access rate of i2c slaves relative to the sample
+     * rate. When a slave’s access rate is decreased relative to the
+     * Sample Rate, the slave is accessed every 
+     * 1 / (1 + I2C_MST_DLY) samples
+     */
+    typedef enum {
+      I2C_MST_DLY0                     = 0x01,
+      I2C_MST_DLY1                     = 0x02,
+      I2C_MST_DLY2                     = 0x04,
+      I2C_MST_DLY3                     = 0x08,
+      I2C_MST_DLY4                     = 0x10,
+      _I2C_MST_DLY_SHIFT               = 0,
+      _I2C_MST_DLY_MASK                = 31,
+
+      I2C_SLV4_REG_DIS                 = 0x20,
+      I2C_SLV4_INT_EN                  = 0x40,
+      I2C_SLV4_EN                      = 0x80
+    } I2C_SLV4_CTRL_BITS_T;
+
+    /**
+     * REG_I2C_MST_STATUS bits
+     */
+    typedef enum {
+      I2C_SLV0_NACK                    = 0x01,
+      I2C_SLV1_NACK                    = 0x02,
+      I2C_SLV2_NACK                    = 0x04,
+      I2C_SLV3_NACK                    = 0x08,
+      I2C_SLV4_NACK                    = 0x10,
+
+      I2C_LOST_ARB                     = 0x20,
+      I2C_SLV4_DONE                    = 0x40,
+      PASS_THROUGH                     = 0x80
+    } I2C_MST_STATUS_BITS_T;
+    
+    /**
+     * REG_INT_PIN_CFG bits
+     */
+    typedef enum {
+      CLKOUT_EN                        = 0x01, // *
+
+      I2C_BYPASS_ENABLE                = 0x02,
+
+      FSYNC_INT_EN                     = 0x04,
+      FSYNC_INT_LEVEL                  = 0x08,
+
+      INT_RD_CLEAR                     = 0x10,
+
+      LATCH_INT_EN                     = 0x20,
+
+      INT_OPEN                         = 0x40,
+      INT_LEVEL                        = 0x80
+    } INT_PIN_CFG_BITS_T;
+    
+    /**
+     * REG_INT_ENABLE bits
+     */
+    typedef enum {
+      DATA_RDY_EN                      = 0x01, // *
+
+      // 0x02, 0x04 reserved
+
+      I2C_MST_INT_EN                   = 0x08,
+
+      FIFO_OFLOW_EN                    = 0x10,
+
+      ZMOT_EN                          = 0x20, // *zero motion
+      MOT_EN                           = 0x40,
+      FF_EN                            = 0x80  // *freefall
+    } INT_ENABLE_BITS_T;
+    
+    /**
+     * REG_INT_STATUS bits
+     */
+    typedef enum {
+      DATA_RDY_INT                     = 0x01,
+
+      // 0x02, 0x04 reserved
+
+      I2C_MST_INT                      = 0x08,
+
+      FIFO_OFLOW_INT                   = 0x10,
+
+      ZMOT_INT                         = 0x20, // *zero motion
+      MOT_INT                          = 0x40,
+      FF_INT                           = 0x80  // *freefall
+    } INT_STATUS_BITS_T;
+    
+    /**
+     * REG_MOT_DETECT_STATUS bits (mpu9150 only)
+     */
+    typedef enum {
+      MOT_ZRMOT                        = 0x01, // *
+
+      // 0x02 reserved
+
+      MOT_ZPOS                         = 0x04, // *
+      MOT_ZNEG                         = 0x08, // *
+
+      MOT_YPOS                         = 0x10, // *
+      MOT_YNEG                         = 0x20, // *
+
+      MOT_XPOS                         = 0x40, // *
+      MOT_XNEG                         = 0x80, // *
+    } MOT_DETECT_STATUS_BITS_T;
+    
+    /**
+     * REG_MST_DELAY_CTRL bits
+     */
+    typedef enum {
+      I2C_SLV0_DLY_EN                  = 0x01,
+      I2C_SLV1_DLY_EN                  = 0x02,
+      I2C_SLV2_DLY_EN                  = 0x04,
+      I2C_SLV3_DLY_EN                  = 0x08,
+      I2C_SLV4_DLY_EN                  = 0x10,
+
+      // 0x20, 0x40, reserved
+
+      DELAY_ES_SHADOW                  = 0x80
+    } MST_DELAY_CTRL_BITS_T;
+    
+    /**
+     * REG_SIGNAL_PATH_RESET bits
+     */
+    typedef enum {
+      TEMP_RESET                       = 0x01,
+      ACCEL_RESET                      = 0x02,
+      GYRO_RESET                       = 0x04
+
+      // 0x08-0x80 reserved
+    } SIGNAL_PATH_RESET_BITS_T;
+    
+    /**
+     * REG_MOT_DETECT_CTRL bits
+     */
+    typedef enum {
+      MOT_COUNT0                       = 0x01, // *
+      MOT_COUNT1                       = 0x02, // *
+      _MOT_COUNT_SHIFT                 = 0,
+      _MOT_COUNT_MASK                  = 3,
+
+      FF_COUNT0                        = 0x04, // *
+      FF_COUNT1                        = 0x08, // *
+      _FF_COUNT_SHIFT                  = 2,
+      _FF_COUNT_MASK                   = 3,
+
+      ACCEL_ON_DELAY0                  = 0x10,
+      ACCEL_ON_DELAY1                  = 0x20,
+      _ACCEL_ON_DELAY_SHIFT            = 4,
+      _ACCEL_ON_DELAY_MASK             = 3
+      // 0x40,0x80 reserved
+    } MOT_DETECT_CTRL_BITS_T;
+    
+    /**
+     * MOT_COUNT or FF_COUNT values (mpu9150 only)
+     */
+    typedef enum {
+      COUNT_0                          = 0, // Reset
+      COUNT_1                          = 1, // counter decrement 1
+      COUNT_2                          = 2, // counter decrement 2
+      COUNT_4                          = 3  // counter decrement 4
+    } MOT_FF_COUNT_T;
+
+    /**
+     * ACCEL_ON_DELAY values
+     */
+    typedef enum {
+      ON_DELAY_0                       = 0, // no delay
+      ON_DELAY_1                       = 1, // add 1ms
+      ON_DELAY_2                       = 2, // add 2ms
+      ON_DELAY_3                       = 3  // add 3ms
+    } ACCEL_ON_DELAY_T;
+
+    /**
+     * REG_USER_CTRL bits
+     */
+    typedef enum {
+      SIG_COND_RESET                   = 0x01,
+      I2C_MST_RESET                    = 0x02,
+      FIFO_RESET                       = 0x04,
+
+      // 0x08 reserved
+
+      I2C_IF_DIS                       = 0x10,
+      I2C_MST_EN                       = 0x20,
+      FIFO_EN                          = 0x40
+
+      /// 0x80 reserved
+    } USER_CTRL_BITS_T;
+    
+    /**
+     * REG_PWR_MGMT_1 bits
+     */
+    typedef enum {
+      CLKSEL0                          = 0x01,
+      CLKSEL1                          = 0x02,
+      CLKSEL2                          = 0x04,
+      _CLKSEL_SHIFT                    = 0,
+      _CLKSEL_MASK                     = 7,
+
+      TEMP_DIS                         = 0x08,
+
+      // 0x10 reserved
+
+      PWR_CYCLE                        = 0x20,
+      PWR_SLEEP                        = 0x40,
+      DEVICE_RESET                     = 0x80
+    } PWR_MGMT_1_BITS_T;
+    
+    /**
+     * CLKSEL values
+     */
+    typedef enum {
+      INT_8MHZ                         = 0, // internal 8Mhz osc
+      PLL_XG                           = 1, // PLL X axis gyro
+      PLL_YG                           = 2, // PLL Y axis gyro
+      PLL_ZG                           = 3, // PLL Z axis gyro
+      PLL_EXT_32KHZ                    = 4, // PLL with external 32.768Khz ref
+      PLL_EXT_19MHZ                    = 5, // PLL with external 19.2Mhz ref
+      // 6 - reserved
+      CLK_STOP                         = 7  // stops clk
+    } CLKSEL_T;
+
+    /**
+     * REG_PWR_MGMT_2 bits
+     */
+    typedef enum {
+      STBY_ZG                          = 0x01,
+      STBY_YG                          = 0x02,
+      STBY_XG                          = 0x04,
+      STBY_ZA                          = 0x08,
+      STBY_YA                          = 0x10,
+      STBY_XA                          = 0x20,
+
+      LP_WAKE_CTRL0                    = 0x40,
+      LP_WAKE_CTRL1                    = 0x80,
+      _LP_WAKE_CTRL_SHIFT              = 6,
+      _LP_WAKE_CTRL_MASK               = 3
+    } PWR_MGMT_2_BITS_T;
+    
+    /**
+     * LP_WAKE_CTRL values
+     */
+    typedef enum {
+      LP_WAKE_1_25                     = 0, // wakeup feq: 1.25hz
+      LP_WAKE_5                        = 1, // 5hz
+      LP_WAKE_20                       = 2, // 20hz
+      LP_WAKE_40                       = 3, // 40hz
+    } LP_WAKE_CRTL_T;
+
+
+    /**
+     * mpu60x0 constructor
+     *
+     * @param bus i2c bus to use
+     * @param address the address for this device
+     */
+    MPU60X0(int bus=MPU60X0_I2C_BUS, uint8_t address=MPU60X0_DEFAULT_I2C_ADDR);
+
+    /**
+     * MPU60X0 Destructor
+     */
+    ~MPU60X0();
+    
+    /**
+     * set up initial values and start operation
+     *
+     * @return true if successful
+     */
+    bool init();
+
+    /**
+     * take a measurement and store the current sensor values
+     * internally.  Note, these user facing registers are only updated
+     * from the internal device sensor values when the i2c serial
+     * traffic is 'idle'.  So, if you are reading the values too fast,
+     * the bus may never be idle, and you will just end up reading
+     * the same values over and over.
+     *
+     * Unfortunately, it is is not clear how long 'idle' actually
+     * means, so if you see this behavior, reduce the rate at which
+     * you are calling update().
+     *
+     */
+    void update();
+
+    /**
+     * read a register
+     *
+     * @param reg the register to read
+     * @return the value of the register
+     */
+    uint8_t readReg(uint8_t reg);
+
+    /**
+     * read contiguous refister into a buffer
+     *
+     * @param reg the register to start reading at
+     * @param buf the buffer to store the results
+     * @param len the number of registers to read
+     * @return the value of the register
+     */
+    void readRegs(uint8_t reg, uint8_t *buf, int len);
+
+    /**
+     * write to a register
+     *
+     * @param reg the register to write to
+     * @param val the value to write
+     * @return true if successful, false otherwise
+     */
+    bool writeReg(uint8_t reg, uint8_t val);
+
+    /**
+     * enable or disable device sleep
+     *
+     * @param enable true to put device to sleep, false to wake up
+     * @return true if successful, false otherwise
+     */
+    bool setSleep(bool enable);
+
+    /**
+     * specify the clock source for the device to use
+     *
+     * @param clk one of the CLKSEL_T values
+     * @return true if successful, false otherwise
+     */
+    bool setClockSource(CLKSEL_T clk);
+
+    /**
+     * set the scaling mode of the gyroscope
+     *
+     * @param scale one of the FS_SEL_T values
+     * @return true if successful, false otherwise
+     */
+    bool setGyroscopeScale(FS_SEL_T scale);
+
+    /**
+     * set the scaling mode of the accelerometer
+     *
+     * @param scale one of the AFS_SEL_T values
+     * @return true if successful, false otherwise
+     */
+    bool setAccelerometerScale(AFS_SEL_T scale);
+
+    /**
+     * set the Low Pass Digital filter.  This enables filtering (if
+     * non-0) of the accelerometer and gyro outputs.
+     *
+     * @param scale one of the DLPF_CFG_T values
+     * @return true if successful, false otherwise
+     */
+    bool setDigitalLowPassFilter(DLPF_CFG_T dlp);
+
+    /**
+     * set the sample rate divider.  This register specifies the
+     * divider from the gyro output rate used to generate the Sample
+     * Rate.  The sensor registor output, FIFO output, DMP sampling
+     * and motion detection are all based on the Sample Rate.
+     *
+     * The Sample Rate is generated by dividing the gyro output rate
+     * by this register:
+     *
+     * Sample Rate = Gyro output rate / (1 + sample rate divider).
+     *
+     * The Gyro output rate is 8Khz when the Digital Low Pass Filter
+     * (DLPF) is 0 or 7 (DLPF_260_256 or DLPF_RESERVED), and 1Khz
+     * otherwise.
+     *
+     * @param scale one of the DLPF_CFG_T values
+     * @return true if successful, false otherwise
+     */
+    bool setSampleRateDivider(uint8_t div);
+
+    /**
+     * get the current Sample Rate divider
+     *
+     * @return the current sample rate divider
+     */
+    uint8_t getSampleRateDivider();
+
+    /**
+     * get the accelerometer values
+     *
+     * @param x the returned x value, if arg is non-NULL
+     * @param y the returned y value, if arg is non-NULL
+     * @param z the returned z value, if arg is non-NULL
+     * @return true if successful, false otherwise
+     */
+    void getAccelerometer(float *x, float *y, float *z);
+
+    /**
+     * get the gyroscope values
+     *
+     * @param x the returned x value, if arg is non-NULL
+     * @param y the returned y value, if arg is non-NULL
+     * @param z the returned z value, if arg is non-NULL
+     * @return true if successful, false otherwise
+     */
+    void getGyroscope(float *x, float *y, float *z);
+
+    /**
+     * get the temperature value
+     *
+     * @return the temperature value in degrees Celcius
+     */
+    float getTemperature();
+
+    /**
+     * enable onboard temperature measurement sensor
+     *
+     * @param enable true to enable temperature sensor, false to disable
+     * @return true if successful, false otherwise
+     */
+    bool enableTemperatureSensor(bool enable);
+
+    /**
+     * configure external sync.  An external signal connected to the
+     * FSYNC pin can be sampled by configuring EXT_SYNC_SET.  Signal
+     * changes to the FSYNC pin are latched so that short strobes may
+     * be captured. The latched FSYNC signal will be sampled at the
+     * Sampling Rate, as defined in register 25. After sampling, the
+     * latch will reset to the current FSYNC signal state.
+     *
+     * The sampled value will be reported in place of the least
+     * significant bit in a sensor data register determined by the
+     * value of EXT_SYNC_SET
+     *
+     * @param val one of the EXT_SYNC_SET_T values
+     * @return true if successful, false otherwise
+     */
+    bool setExternalSync(EXT_SYNC_SET_T val);
+
+    /**
+     * enable I2C Bypass.  Enabling this feature allows devices on the
+     * MPU60X0 auxillary I2C bus to be visible on the MCU's I2C bus.
+     *
+     * @param enable true to I2C bypass
+     * @return true if successful, false otherwise
+     */
+    bool enableI2CBypass(bool enable);
+
+    /**
+     * set the motion detection threshold for interrupt generation.
+     * Motion is detected when the absolute value of any of the
+     * accelerometer measurements exceeds this Motion detection
+     * threshold.
+     *
+     * @param thr threshold
+     * @return true if successful, false otherwise
+     */
+    bool setMotionDetectionThreshold(uint8_t thr);
+
+    /**
+     * return the interrupt status register.
+     *
+     * @return the interrupt status word (see INT_STATUS_BITS_T)
+     */
+    uint8_t getInterruptStatus();
+
+    /**
+     * set the interrupt enables
+     *
+     * @param enables bitmask of INT_ENABLE_BITS_T values to enable
+     * @return true if successful, false otherwise
+     */
+    bool setInterruptEnables(uint8_t enables);
+
+    /**
+     * get the current interrupt enables register
+     *
+     * @return bitmask of INT_ENABLE_BITS_T values
+     */
+    uint8_t getInterruptEnables();
+
+    /**
+     * set the interrupt pin configuration
+     *
+     * @param cfg bitmask of INT_PIN_CFG_BITS_T values
+     * @return true if successful, false otherwise
+     */
+    bool setInterruptPinConfig(uint8_t cfg);
+
+    /**
+     * get the current interrupt pin configuration
+     *
+     * @return bitmask of INT_PIN_CFG_BITS_T values
+     */
+    uint8_t getInterruptPinConfig();
+    
+    /**
+     * install an interrupt handler.
+     *
+     * @param gpio gpio pin to use as interrupt pin
+     * @param level the interrupt trigger level (one of mraa::Edge
+     * values).  Make sure that you have configured the interrupt pin
+     * (setInterruptPinConfig()) properly for whatever level you
+     * choose.
+     * @param isr the interrupt handler, accepting a void * argument
+     * @param arg the argument to pass the the interrupt handler
+     */
+    void installISR(int gpio, mraa::Edge level, void (*isr)(void *), void *arg);
+
+    /**
+     * uninstall a previously installed interrupt handler
+     *
+     */
+    void uninstallISR();
+
+  protected:
+    // uncompensated accelerometer and gyroscope values
+    float m_accelX;
+    float m_accelY;
+    float m_accelZ;
+
+    float m_gyroX;
+    float m_gyroY;
+    float m_gyroZ;
+
+    // uncompensated temperature value
+    float m_temp;
+
+    // accelerometer and gyro scaling factors, depending on their Full
+    // Scale settings.
+    float m_accelScale;
+    float m_gyroScale;
+
+  private:
+    mraa::I2c m_i2c;
+    uint8_t m_addr;
+
+    mraa::Gpio *m_gpioIRQ;
+  };
+}
+
+

src/mpu9150/mpu9150.cxx

@@ -1,9 +1,6 @@
 /*
- * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
- * Copyright (c) 2014 Intel Corporation.
- *
- * Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
- * 8/24/2011 by Jeff Rowberg <jeff@rowberg.net>
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -32,195 +29,74 @@
 #include "mpu9150.h"
 
 using namespace upm;
+using namespace std;
 
-MPU9150::MPU9150 (int bus, int devAddr) {
-    m_name = "MPU9150";
-
-    m_i2cAddr = devAddr;
-    m_bus = bus;
-
-    m_i2Ctx = mraa_i2c_init(m_bus);
-
-    mraa_result_t ret = mraa_i2c_address(m_i2Ctx, m_i2cAddr);
-    if (ret != MRAA_SUCCESS) {
-        fprintf(stderr, "Messed up i2c bus\n");
-    }
-
-    initSensor ();
+MPU9150::MPU9150 (int bus, int address, int magAddress) :
+  m_mag(0), MPU60X0(bus, address)
+{
+  m_magAddress = magAddress;
+  m_i2cBus = bus;
 }
 
-MPU9150::~MPU9150() {
-    mraa_i2c_stop(m_i2Ctx);
+MPU9150::~MPU9150()
+{
+  if (m_mag)
+    delete m_mag;
 }
 
-mraa_result_t
-MPU9150::initSensor () {
-    uint8_t regData = 0x0;
-
-    // setClockSource
-    updateRegBits ( MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
-                    MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
-    // setFullScaleGyroRange
-    updateRegBits ( MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
-                    MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250);
-    // setFullScaleAccelRange
-    updateRegBits ( MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
-                    MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2);
-    // setSleepEnabled
-    i2cReadReg_N (MPU6050_RA_PWR_MGMT_1, 0x1, &regData);
-    regData &= ~(1 << MPU6050_PWR1_SLEEP_BIT);
-    i2cWriteReg (MPU6050_RA_PWR_MGMT_1, regData);
-
-    return MRAA_SUCCESS;
-}
-
-uint8_t
-MPU9150::getDeviceID () {
-    uint8_t regData = 0x0;
-    getRegBits (MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &regData);
-    return regData;
-}
-
-void
-MPU9150::getData () {
-    uint8_t buffer[14];
-
-    for (int i = 0; i < SMOOTH_TIMES; i++) {
-        i2cReadReg_N (MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
-        axisAcceleromter.rawData.axisX  = (((int16_t)buffer[0]) << 8)  | buffer[1];
-        axisAcceleromter.rawData.axisY  = (((int16_t)buffer[2]) << 8)  | buffer[3];
-        axisAcceleromter.rawData.axisZ  = (((int16_t)buffer[4]) << 8)  | buffer[5];
-        axisAcceleromter.sumData.axisX  += (double) axisAcceleromter.rawData.axisX / 16384;
-        axisAcceleromter.sumData.axisY  += (double) axisAcceleromter.rawData.axisY / 16384;
-        axisAcceleromter.sumData.axisZ  += (double) axisAcceleromter.rawData.axisZ / 16384;
-
-        axisGyroscope.rawData.axisX     = (((int16_t)buffer[8]) << 8)  | buffer[9];
-        axisGyroscope.rawData.axisY     = (((int16_t)buffer[10]) << 8) | buffer[11];
-        axisGyroscope.rawData.axisZ     = (((int16_t)buffer[12]) << 8) | buffer[13];
-        axisGyroscope.sumData.axisX     += (double) axisAcceleromter.rawData.axisX * 250 / 32768;
-        axisGyroscope.sumData.axisY     += (double) axisAcceleromter.rawData.axisY * 250 / 32768;
-        axisGyroscope.sumData.axisZ     += (double) axisAcceleromter.rawData.axisZ * 250 / 32768;
-
-        i2cWriteReg (MPU6050_RA_INT_PIN_CFG, 0x02);
-        usleep (10000);
-        m_i2cAddr = MPU9150_RA_MAG_ADDRESS;
-        i2cWriteReg (0x0A, 0x01);
-        usleep (10000);
-        i2cReadReg_N (MPU9150_RA_MAG_XOUT_L, 6, buffer);
-        m_i2cAddr = ADDR;
-
-        axisMagnetomer.rawData.axisX  = (((int16_t)buffer[0]) << 8)  | buffer[1];
-        axisMagnetomer.rawData.axisY  = (((int16_t)buffer[2]) << 8)  | buffer[3];
-        axisMagnetomer.rawData.axisZ  = (((int16_t)buffer[4]) << 8)  | buffer[5];
-        axisMagnetomer.sumData.axisX  += (double) axisMagnetomer.rawData.axisX * 1200 / 4096;
-        axisMagnetomer.sumData.axisY  += (double) axisMagnetomer.rawData.axisY * 1200 / 4096;
-        axisMagnetomer.sumData.axisZ  += (double) axisMagnetomer.rawData.axisZ * 1200 / 4096;
+bool MPU9150::init()
+{
+  // init the gyro/accel component
+  if (!MPU60X0::init())
+    {
+      cerr << __FUNCTION__ << ": Unable to init MPU60X0" << endl;
+      return false;
     }
 
-    axisAcceleromter.data.axisX = axisAcceleromter.sumData.axisX / SMOOTH_TIMES;
-    axisAcceleromter.data.axisY = axisAcceleromter.sumData.axisY / SMOOTH_TIMES;
-    axisAcceleromter.data.axisZ = axisAcceleromter.sumData.axisZ / SMOOTH_TIMES;
-
-    axisGyroscope.data.axisX = axisGyroscope.sumData.axisX / SMOOTH_TIMES;
-    axisGyroscope.data.axisY = axisGyroscope.sumData.axisY / SMOOTH_TIMES;
-    axisGyroscope.data.axisZ = axisGyroscope.sumData.axisZ / SMOOTH_TIMES;
-
-    axisMagnetomer.data.axisX = axisMagnetomer.sumData.axisX / SMOOTH_TIMES;
-    axisMagnetomer.data.axisY = axisMagnetomer.sumData.axisY / SMOOTH_TIMES;
-    axisMagnetomer.data.axisZ = axisMagnetomer.sumData.axisZ / SMOOTH_TIMES;
-}
-
-mraa_result_t
-MPU9150::getAcceleromter (Vector3D * data) {
-    data->axisX = axisAcceleromter.data.axisX;
-    data->axisY = axisAcceleromter.data.axisY;
-    data->axisZ = axisAcceleromter.data.axisZ;
-
-    return MRAA_SUCCESS;
-}
-
-mraa_result_t
-MPU9150::getGyro (Vector3D * data) {
-    data->axisX = axisGyroscope.data.axisX;
-    data->axisY = axisGyroscope.data.axisY;
-    data->axisZ = axisGyroscope.data.axisZ;
-
-    return MRAA_SUCCESS;
-}
-
-mraa_result_t
-MPU9150::getMagnometer (Vector3D * data) {
-    data->axisX = axisMagnetomer.data.axisX;
-    data->axisY = axisMagnetomer.data.axisY;
-    data->axisZ = axisMagnetomer.data.axisZ;
-
-    return MRAA_SUCCESS;
-}
-
-float
-MPU9150::getTemperature () {
-    uint8_t buffer[2];
-    uint16_t tempRaw = 0;
-
-    updateRegBits (MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, 0x1, 0x0);
-    i2cReadReg_N (MPU6050_RA_TEMP_OUT_H, 2, buffer);
-    tempRaw = (((int16_t)buffer[0]) << 8) | buffer[1];
-
-    return (float)tempRaw / 340.0 + 35.0;
-}
-
-/*
- * **************
- *  private area
- * **************
- */
-uint16_t
-MPU9150::i2cReadReg_N (int reg, unsigned int len, uint8_t * buffer) {
-    int readByte = 0;
-    mraa_i2c_address(m_i2Ctx, m_i2cAddr);
-    mraa_i2c_write_byte(m_i2Ctx, reg);
-
-    mraa_i2c_address(m_i2Ctx, m_i2cAddr);
-    readByte = mraa_i2c_read(m_i2Ctx, buffer, len);
-    return readByte;
-}
+  // Now, we need to enable I2C bypass on the MPU60X0 component.  This
+  // will allow us to access the AK8975 Magnetometer on I2C addr 0x0c.
+  if (!enableI2CBypass(true))
+    {
+      cerr << __FUNCTION__ << ": Unable to enable I2C bypass" << endl;
+      return false;
+    }
 
-mraa_result_t
-MPU9150::i2cWriteReg (uint8_t reg, uint8_t value) {
-    mraa_result_t error = MRAA_SUCCESS;
+  // Now that we've done that, create an AK8975 instance and
+  // initialize it.
+  m_mag = new AK8975(m_i2cBus, m_magAddress);
 
-    uint8_t data[2] = { reg, value };
-    error = mraa_i2c_address (m_i2Ctx, m_i2cAddr);
-    error = mraa_i2c_write (m_i2Ctx, data, 2);
+  if (!m_mag->init())
+    {
+      cerr << __FUNCTION__ << ": Unable to init magnetometer" << endl;
+      delete m_mag;
+      m_mag = 0;
+      return false;
+    }
 
-    return error;
+  return true;
 }
 
-int
-MPU9150::updateRegBits (uint8_t reg, uint8_t bitStart, uint8_t length, uint16_t data) {
-    uint8_t regData;
+void MPU9150::update()
+{
+  MPU60X0::update();
 
-    if (i2cReadReg_N (reg, 0x1, &regData) != 0) {
-        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
-        data <<= (bitStart - length + 1); // shift data into correct position
-        data &= mask; // zero all non-important bits in data
-        regData &= ~(mask); // zero all important bits in existing byte
-        regData |= data; // combine data with existing byte
-        return i2cWriteReg (reg, regData);
-    } else {
-        return 0x0;
-    }
+  if (m_mag)
+    m_mag->update();
 }
 
-uint8_t
-MPU9150::getRegBits (uint8_t reg, uint8_t bitStart, uint8_t length, uint8_t * data) {
-    uint8_t count = 0;
-    uint8_t regData;
-    if (i2cReadReg_N (reg, 0x1, &regData) != 0) {
-        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
-        regData &= mask;
-        regData >>= (bitStart - length + 1);
-        *data = regData;
-    }
-    return count;
+void MPU9150::getMagnetometer(float *x, float *y, float *z)
+{
+  float mx, my, mz;
+
+  if (!m_mag)
+    mx = my = mz = 0.0;
+  else
+    m_mag->getMagnetometer(&mx, &my, &mz);
+  
+  if (x)
+    *x = mx;
+  if (y)
+    *y = my;
+  if (z)
+    *z = mz;
 }

src/mpu9150/mpu9150.h

@@ -1,9 +1,6 @@
 /*
- * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
- * Copyright (c) 2014 Intel Corporation.
- *
- * Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
- * 8/24/2011 by Jeff Rowberg <jeff@rowberg.net>
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2015 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -24,188 +21,100 @@
  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
+
 #pragma once
 
-#include <string>
-#include <mraa/i2c.h>
-
-#define MPU6050_ADDRESS_AD0_LOW             0x68 // address pin low (GND), default for InvenSense evaluation board
-#define MPU6050_ADDRESS_AD0_HIGH            0x69 // address pin high (VCC)
-#define ADDR                                MPU6050_ADDRESS_AD0_LOW // device address
-
-// registers address
-#define MPU6050_CLOCK_PLL_XGYRO             0x01
-#define MPU6050_GYRO_FS_250                 0x00
-#define MPU6050_ACCEL_FS_2                  0x00
-#define MPU6050_RA_INT_PIN_CFG              0x37
-
-#define MPU6050_RA_ACCEL_XOUT_H             0x3B
-#define MPU6050_RA_ACCEL_XOUT_L             0x3C
-#define MPU6050_RA_ACCEL_YOUT_H             0x3D
-#define MPU6050_RA_ACCEL_YOUT_L             0x3E
-#define MPU6050_RA_ACCEL_ZOUT_H             0x3F
-#define MPU6050_RA_ACCEL_ZOUT_L             0x40
-#define MPU6050_RA_TEMP_OUT_H               0x41
-#define MPU6050_RA_TEMP_OUT_L               0x42
-#define MPU6050_RA_GYRO_XOUT_H              0x43
-#define MPU6050_RA_GYRO_XOUT_L              0x44
-#define MPU6050_RA_GYRO_YOUT_H              0x45
-#define MPU6050_RA_GYRO_YOUT_L              0x46
-#define MPU6050_RA_GYRO_ZOUT_H              0x47
-#define MPU6050_RA_GYRO_ZOUT_L              0x48
-
-#define MPU6050_RA_CONFIG                   0x1A
-#define MPU6050_CFG_DLPF_CFG_BIT            2
-#define MPU6050_CFG_DLPF_CFG_LENGTH         3
-
-#define MPU6050_RA_GYRO_CONFIG              0x1B
-#define MPU6050_GCONFIG_FS_SEL_BIT          4
-#define MPU6050_GCONFIG_FS_SEL_LENGTH       2
-
-#define MPU6050_RA_ACCEL_CONFIG             0x1C
-#define MPU6050_ACONFIG_AFS_SEL_BIT         4
-#define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
-
-// magnotometer
-#define MPU9150_RA_MAG_ADDRESS              0x0C
-#define MPU9150_RA_MAG_XOUT_L               0x03
-
-#define MPU6050_RA_PWR_MGMT_1               0x6B
-#define MPU6050_PWR1_CLKSEL_BIT             2
-#define MPU6050_PWR1_CLKSEL_LENGTH          3
-#define MPU6050_PWR1_SLEEP_BIT              6
-
-#define MPU6050_RA_INT_PIN_CFG              0x37
-
-// temperature
-#define MPU6050_PWR1_TEMP_DIS_BIT           3
-#define MPU6050_RA_WHO_AM_I                 0x75
-#define MPU6050_WHO_AM_I_BIT                6
-#define MPU6050_WHO_AM_I_LENGTH             6
-
-#define SMOOTH_TIMES                        10.0
-
-#define HIGH               1
-#define LOW                0
+#include "mpu60x0.h"
+#include "ak8975.h"
+
+#define MPU9150_I2C_BUS 0
+#define MPU9150_DEFAULT_I2C_ADDR  MPU60X0_DEFAULT_I2C_ADDR
+
 
 namespace upm {
 
-struct Vector3DRaw {
-    uint16_t axisX;
-    uint16_t axisY;
-    uint16_t axisZ;
-};
-
-struct Vector3D {
-    double axisX;
-    double axisY;
-    double axisZ;
-};
-
-struct AxisData {
-    Vector3DRaw rawData;
-    Vector3D    sumData;
-    Vector3D    data;
-};
-
-/**
- * @brief MPU9150 accelerometer library
- * @defgroup mpu9150 libupm-mpu9150
- * @ingroup seeed i2c accelerometer compass
- */
-/**
- * @library mpu9150
- * @sensor mpu9150
- * @comname MPU9150 Inertial Measurement Unit
- * @altname Grove IMU 9DOF
- * @type accelerometer compass
- * @man seeed
- * @web http://www.seeedstudio.com/wiki/Grove_-_IMU_9DOF_v1.0
- * @con i2c
- *
- * @brief API for MPU9150 chip (Accelerometer, Gyro and Magnometer Sensor)
- *
- * This file defines the MPU9150 interface for libmpu9150
- *
- * @image html mpu9150.jpg
- * @snippet mpu9150.cxx Interesting
- */
-class MPU9150 {
-    public:
-        /**
-         * Instanciates a MPU9150 object
-         *
-         * @param bus number of used bus
-         * @param devAddr addres of used i2c device
-         */
-        MPU9150 (int bus=0, int devAddr=0x68);
-
-        /**
-         * MPU9150 object destructor, basicaly it close i2c connection.
-         */
-        ~MPU9150 ();
-
-        /**
-         * Initiate MPU9150 chips
-         */
-        mraa_result_t initSensor ();
-
-        /**
-         * Get identity of the device
-         */
-        uint8_t getDeviceID ();
-
-        /**
-         * Get the Accelerometer, Gyro and Compass data from the chip and
-         * save it in private section.
-         */
-        void getData ();
-
-        /**
-         * @param data structure with 3 axis (x,y,z)
-         */
-        mraa_result_t getAcceleromter (Vector3D * data);
-
-        /**
-         * @param data structure with 3 axis (x,y,z)
-         */
-        mraa_result_t getGyro (Vector3D * data);
-
-        /**
-         * @param data structure with 3 axis (x,y,z)
-         */
-        mraa_result_t getMagnometer (Vector3D * data);
-
-        /**
-         * Read on die temperature from the chip
-         */
-        float getTemperature ();
-
-        /**
-         * Return name of the component
-         */
-        std::string name()
-        {
-            return m_name;
-        }
-
-    private:
-        std::string m_name;
-
-        int m_i2cAddr;
-        int m_bus;
-        mraa_i2c_context m_i2Ctx;
-
-        AxisData axisMagnetomer;
-        AxisData axisAcceleromter;
-        AxisData axisGyroscope;
-
-        uint16_t i2cReadReg_N (int reg, unsigned int len, uint8_t * buffer);
-        mraa_result_t i2cWriteReg (uint8_t reg, uint8_t value);
-        int updateRegBits (uint8_t reg, uint8_t bitStart,
-                                    uint8_t length, uint16_t data);
-        uint8_t getRegBits (uint8_t reg, uint8_t bitStart,
-                                    uint8_t length, uint8_t * data);
-};
+  /**
+   * @brief MPU9150 accelerometer library
+   * @defgroup mpu9150 libupm-mpu9150
+   * @ingroup seeed i2c gpio accelerometer compass
+   */
+
+  /**
+   * @library mpu9150
+   * @sensor mpu9150
+   * @comname MPU9150 Inertial Measurement Unit
+   * @altname Grove IMU 9DOF
+   * @type accelerometer compass
+   * @man seeed
+   * @web http://www.seeedstudio.com/wiki/Grove_-_IMU_9DOF_v1.0
+   * @con i2c gpio
+   *
+   * @brief API for MPU9150 chip (Accelerometer, Gyro and Magnometer Sensor)
+   *
+   * This file defines the MPU9150 interface for libmpu9150
+   *
+   * @image html mpu9150.jpg
+   * @snippet mpu9150.cxx Interesting
+   */
+
+  class MPU9150: public MPU60X0
+  {
+  public:
+    /**
+     * MPU9150 constructor
+     *
+     * @param bus i2c bus to use
+     * @param address the address for this device
+     * @param magAddress the address of the connected magnetometer
+     */
+    MPU9150 (int bus=MPU9150_I2C_BUS, int address=MPU9150_DEFAULT_I2C_ADDR,
+             int magAddress=AK8975_DEFAULT_I2C_ADDR);
+
+    /**
+     * MPU9150 Destructor
+     */
+    ~MPU9150 ();
+
+    /**
+     * set up initial values and start operation
+     *
+     * @return true if successful
+     */
+    bool init();
+
+    /**
+     * take a measurement and store the current sensor values
+     * internally.  Note, these user facing registers are only updated
+     * from the internal device sensor values when the i2c serial
+     * traffic is 'idle'.  So, if you are reading the values too fast,
+     * the bus may never be idle, and you will just end up reading
+     * the same values over and over.
+     *
+     * Unfortunately, it is is not clear how long 'idle' actually
+     * means, so if you see this behavior, reduce the rate at which
+     * you are calling update().
+     */
+    void update();
+
+    /**
+     * return the compensated values for the x, y, and z axes.  The
+     * unit of measurement is in micro-teslas (uT).
+     *
+     * @param x pointer to returned X axis value
+     * @param y pointer to returned Y axis value
+     * @param z pointer to returned Z axis value
+     */
+    void getMagnetometer(float *x, float *y, float *z);
+
+
+  protected:
+    // magnetometer instance
+    AK8975* m_mag;
+
+
+  private:
+    int m_i2cBus;
+    uint8_t m_magAddress;
+  };
 
 }

src/mpu9150/pyupm_mpu9150.i

@@ -1,11 +1,25 @@
 %module pyupm_mpu9150
 %include "../upm.i"
+%include "cpointer.i"
 
 %include "stdint.i"
 
 %feature("autodoc", "3");
 
+%pointer_functions(float, floatp);
+
+%include "ak8975.h"
+%{
+    #include "ak8975.h"
+%}
+
+%include "mpu60x0.h"
+%{
+    #include "mpu60x0.h"
+%}
+
 %include "mpu9150.h"
 %{
     #include "mpu9150.h"
 %}
+