From 386be4780922d8c6107163d044c557c59178200c Mon Sep 17 00:00:00 2001 From: Daniel Friesel Date: Mon, 21 Oct 2019 16:39:32 +0200 Subject: Add basic MPU9250 driver --- Makefile | 5 ++ include/driver/mpu9250.h | 30 ++++++++++++ src/app/i2cdetect/main.cc | 20 ++++++++ src/driver/mpu9250.cc | 114 ++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 169 insertions(+) create mode 100644 include/driver/mpu9250.h create mode 100644 src/driver/mpu9250.cc 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 ©); + 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; -- cgit v1.2.3