Add basic MPU9250 driver

4 files changed, 169 insertions, 0 deletions

diff --git a/Makefile b/Makefile
index 0a33696..2e42e6e 100644
--- a/Makefile
+++ b/Makefile
@@ -68,6 +68,11 @@ ifneq ($(findstring hdc1080,${drivers}), )
 	COMMON_FLAGS += -DDRIVER_HDC1080
 endif
 
+ifneq ($(findstring mpu9250,${drivers}), )
+	CXX_TARGETS += src/driver/mpu9250.cc
+	COMMON_FLAGS += -DDRIVER_MPU9250
+endif
+
 ifneq ($(findstring mmsimple,${drivers}), )
 	CXX_TARGETS += src/driver/mmsimple.cc
 	COMMON_FLAGS += -DDRIVER_MMSIMPLE
diff --git a/include/driver/mpu9250.h b/include/driver/mpu9250.h
new file mode 100644
index 0000000..e362994
--- /dev/null
+++ b/include/driver/mpu9250.h
@@ -0,0 +1,30 @@
+#ifndef MPU9250_H
+#define MPU9250_H
+
+class MPU9250 {
+	private:
+		MPU9250(const MPU9250 &copy);
+		unsigned char const address = 0x68;
+		unsigned char txbuf[3];
+		unsigned char rxbuf[8];
+		signed int getWordReg(unsigned char const regBase);
+
+	public:
+		MPU9250() {}
+		void init();
+		int getAccelX() { return getWordReg(59); }
+		int getAccelY() { return getWordReg(61); }
+		int getAccelZ() { return getWordReg(63); }
+		int getGyroX() { return getWordReg(67); }
+		int getGyroY() { return getWordReg(69); }
+		int getGyroZ() { return getWordReg(71); }
+		void getMagnet(int *x, int *y, int *z);
+		void setAccelEnable(bool x, bool y, bool z);
+		void setGyroEnable(bool x, bool y, bool z);
+		void setPower(bool sleep, bool gyroStandby);
+		float getTemperature();
+};
+
+extern MPU9250 mpu9250;
+
+#endif
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;