diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/app/i2cdetect/main.cc | 20 | ||||
| -rw-r--r-- | src/driver/mpu9250.cc | 114 |
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; |