Add basic MPU9250 driver

2 files changed, 134 insertions, 0 deletions

diff --git a/src/app/i2cdetect/main.cc b/src/app/i2cdetect/main.cc
index e74ce5b..e235369 100644
--- a/src/app/i2cdetect/main.cc
+++ b/src/app/i2cdetect/main.cc
@@ -28,6 +28,9 @@
 #ifdef DRIVER_HDC1080
 #include "driver/hdc1080.h"
 #endif
+#ifdef DRIVER_MPU9250
+#include "driver/mpu9250.h"
+#endif
 #ifdef DRIVER_MMSIMPLE
 #include "driver/mmsimple.h"
 #endif
@@ -88,6 +91,20 @@ void loop(void)
 	kout << "HDC1080 temperature " << hdc1080.getTemp() << " degC" << endl;
 	kout << "HDC1080 humidity " << hdc1080.getRH() << " %H" << endl;
 #endif
+#ifdef DRIVER_MPU9250
+	int mx, my, mz;
+	kout << "Temperature: " << mpu9250.getTemperature() << endl;
+	kout << "Accel X " << mpu9250.getAccelX() << endl;
+	kout << "Accel Y " << mpu9250.getAccelX() << endl;
+	kout << "Accel Z " << mpu9250.getAccelX() << endl;
+	kout << "Gyro X " << mpu9250.getGyroX() << endl;
+	kout << "Gyro Y " << mpu9250.getGyroX() << endl;
+	kout << "Gyro Z " << mpu9250.getGyroX() << endl;
+	mpu9250.getMagnet(&mx, &my, &mz);
+	kout << "Magnet X " << mx << endl;
+	kout << "Magnet Y " << mx << endl;
+	kout << "Magnet Z " << mx << endl;
+#endif
 #ifdef DRIVER_MAX44009
 	kout.printf_float(max44009.getLux());
 	kout << endl;
@@ -131,6 +148,9 @@ int main(void)
 		kout << "[!] invalid HDC1080 manufacturer ID: " << hex << hdc1080.getManufacturerID() << endl;
 	}
 #endif
+#ifdef DRIVER_MPU9250
+	mpu9250.init();
+#endif
 
 	for (unsigned char i = 0; i < sizeof(i2c_status)/sizeof(unsigned int); i++) {
 		i2c_status[i] = 0;
diff --git a/src/driver/mpu9250.cc b/src/driver/mpu9250.cc
new file mode 100644
index 0000000..c1ac53d
--- /dev/null
+++ b/src/driver/mpu9250.cc
@@ -0,0 +1,114 @@
+#include "driver/mpu9250.h"
+#include "arch.h"
+#if defined(MULTIPASS_ARCH_HAS_I2C) && !defined(DRIVER_SOFTI2C)
+#include "driver/i2c.h"
+#else
+#include "driver/soft_i2c.h"
+#endif
+
+void MPU9250::init()
+{
+	// reset to default
+	txbuf[0] = 107;
+	txbuf[1] = 0x80;
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+	arch.delay_ms(1);
+
+	// enable I2C passthrough to magnetometer
+	txbuf[0] = 55;
+	txbuf[1] = 1<<1;
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+
+	// reset magnetometer
+	txbuf[0] = 0x0b;
+	txbuf[1] = 1;
+	i2c.xmit(0x0c, 2, txbuf, 0, rxbuf);
+	arch.delay_ms(1);
+
+	// put magnetometer into continuous measurement mode
+	txbuf[0] = 0x0a;
+	txbuf[1] = 2;
+	i2c.xmit(0x0c, 2, txbuf, 0, rxbuf);
+}
+
+void MPU9250::setGyroEnable(bool x, bool y, bool z)
+{
+	txbuf[0] = 108;
+	i2c.xmit(address, 1, txbuf, 1, txbuf + 1); // not a typo
+
+	txbuf[1] &= 0x07;
+
+	if (!x) {
+		txbuf[1] |= 4;
+	}
+	if (!y) {
+		txbuf[1] |= 2;
+	}
+	if (!z) {
+		txbuf[1] |= 1;
+	}
+
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+}
+
+void MPU9250::setAccelEnable(bool x, bool y, bool z)
+{
+	txbuf[0] = 108;
+	i2c.xmit(address, 1, txbuf, 1, txbuf + 1); // not a typo
+
+	txbuf[1] &= 0xf8;
+
+	if (!x) {
+		txbuf[1] |= 1<<5;
+	}
+	if (!y) {
+		txbuf[1] |= 1<<4;
+	}
+	if (!z) {
+		txbuf[1] |= 1<<3;
+	}
+
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+}
+
+void MPU9250::setPower(bool sleep, bool gyroStandby)
+{
+	txbuf[0] = 107;
+	if (sleep) {
+		txbuf[1] = 1<<6;
+	}
+	else if (gyroStandby) {
+		txbuf[1] = 1<<4;
+	}
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+}
+
+signed int MPU9250::getWordReg(unsigned char const regBase)
+{
+	txbuf[0] = regBase;
+	i2c.xmit(address, 1, txbuf, 2, rxbuf);
+
+	return ((signed int)rxbuf[0] << 8) + rxbuf[1];
+}
+
+float MPU9250::getTemperature()
+{
+	return (float)getWordReg(65) / 333.87 + 21;
+}
+
+void MPU9250::getMagnet(int *x, int *y, int *z)
+{
+	txbuf[0] = 0x02;
+	i2c.xmit(0x0c, 0, txbuf, 8, rxbuf);
+
+	if ((rxbuf[0] & 0x01) && !(rxbuf[7] & 0x08)) {
+		*x = ((signed int)rxbuf[2] << 8) + rxbuf[1];
+		*y = ((signed int)rxbuf[4] << 8) + rxbuf[3];
+		*z = ((signed int)rxbuf[6] << 8) + rxbuf[5];
+	}
+	else {
+		*x = *y = *z = 0;
+	}
+}
+
+MPU9250 mpu9250;