Diff: MPU9250.cpp

Revision:

2:8f53e653f9b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.cpp	Mon Dec 18 10:18:59 2017 +0000
@@ -0,0 +1,176 @@
+//InvenSense MPU-9250
+//Driver for the nrf51822 mcu
+//maximum I2C speed is 400kHz
+
+//TODO don't forget tap/double-tap functionality and calibration
+
+#include "MPU9250.h"
+#include "mbed.h"
+
+bool MPU9250::readRegister(int addr, uint8_t* data, int length = 1) {
+    if (i2c.write(deviceAddress,(char *)&addr, 1, true)) {
+        if (i2c.read(deviceAddress,(char *)data, length, false)) {
+            return true;
+        }
+    }
+    
+    return false;
+}
+
+bool MPU9250::writeRegister(int addr, uint8_t data) {
+    if (i2c.write(deviceAddress,(char *)&addr, 1, true)) {
+        bool ack = i2c.write(data);
+        i2c.stop();
+        return ack;
+    }
+        
+    return false;
+}
+
+bool MPU9250::readMagRegister(int addr, uint8_t* data, int length = 1) {
+    if (i2c.write(MPU9250_REG_ID,(char *)&addr, 1, true)) {
+        if (i2c.read(MPU9250_REG_ID,(char *)data, length, false)) {
+            return true;
+        }
+    }
+    
+    i2c.stop();
+    
+    return false;
+}
+
+bool MPU9250::writeMagRegister(int addr, uint8_t data) {
+    if (i2c.write(MPU9250_REG_ID,(char *)&addr, 1, true)) {
+        bool ack = i2c.write(data);
+        i2c.stop();
+        return ack;
+    }
+    
+    return false;
+}
+
+MPU9250::MPU9250(int SCLPin, int SDAPin, int addr) : i2c((PinName)SDAPin,(PinName)SCLPin) {
+    deviceAddress = addr;
+    //magnetometer has it's own non-changeable address defined in: MPU9250_REG_ID   
+}
+        
+bool MPU9250::init() {
+    //configure interrupts and disable FSYNC pin interrupt (not connected)
+    // ACTL 0 = INT pin active high
+    // OPEN 0 = INT pin is push-pull
+    // LATCH_INT_EN 1 = keep INT high until cleared
+    // INT_ANYRD_2CLEAR 1 = INT is cleared if any read operation is performed
+    // ACTL_FSYNC 0 = FSYNC is active high
+    // FSYNC_INT_MODE_EN 0 = disable FSYNC input
+    // BYPASS_EN 1 = set I2C master pins to bypass when I2C master is disabled
+    // RESERVED = keep at 0
+    writeRegister(MPU9250_REG_INTCFG, 0b00110010); //(flags MSB to LSB: ACTL,OPEN,LATCH_INT_EN,INT_ANYRD_2CLEAR,ACTL_FSYNC,FSYNC_INT_MODE_EN,BYPASS_EN,RESERVED)
+
+    //configure gyroscope range
+    writeRegister(MPU9250_REG_GYROCFG,MPU9250_GYROSCALE_2000);
+    
+    //configure accelerometer range
+    writeRegister(MPU9250_REG_ACCELCFG,MPU9250_ACCSCALE_8);
+    
+    //read first magnetometer data
+    writeMagRegister(MPU9250_MAG_REG_CNTL, 0x01); //magnetometer single-measurement
+    
+    //set interrupt conditions and enable interrupts
+    writeRegister(MPU9250_REG_WOMT,  0b00000011); //Set wake-on-motion threshold, range is 0mg to 1020mg (LSB = 4mg)
+    writeRegister(MPU9250_REG_INTEN, 0b01000000); //Enable wake-on-motion interrupt
+    
+    
+    return wakeup();
+}
+
+MPU9250_GYRO_DATA MPU9250::readGyro() {
+    MPU9250_GYRO_DATA data;
+    uint8_t* dataBytePtr = (uint8_t *)&data;
+
+    data.isValid = readRegister(MPU9250_REG_GYRO_X_H, dataBytePtr, 6);
+
+    return data;
+}
+
+MPU9250_ACCEL_DATA MPU9250::readAccel() {
+    MPU9250_ACCEL_DATA data;
+    uint8_t* dataBytePtr = (uint8_t *)&data;
+
+    data.isValid = readRegister(MPU9250_REG_ACCEL_X_H, dataBytePtr, 6);
+
+    return data;
+}
+
+MPU9250_ACCEL_DATA MPU9250::readAccelNew() {
+    //MPU9250_ACCEL_DATA data;
+    //uint8_t* dataBytePtr = (uint8_t *)&data;
+
+    //data.isValid = readRegister(MPU9250_REG_ACCEL_X_H, dataBytePtr, 6);
+
+    //return data;
+
+    //EXAMPLE>
+    MPU9250_ACCEL_DATA data; 
+    uint8_t oneByte;
+    
+    //read one byte at a time and transfer it to the struct ourselves instead of working with pointers
+    //addr = register address
+    
+    //#define MPU9250_REG_ACCEL_X_H 0x3B
+    i2c.write(deviceAddress,MPU9250_REG_ACCEL_X_H, 1, true);
+    i2c.read(deviceAddress, oneByte, 1, false);
+    data.x = (oneByte << 8); //high byte
+    
+    i2c.write(deviceAddress,MPU9250_REG_ACCEL_X_L, 1, true);
+    i2c.read(deviceAddress, oneByte, 1, false);
+    data.x = data.x | oneByte; //low byte    
+    
+    i2c.stop();
+    return data;
+    //EXAMPLE<
+}
+
+MPU9250_TEMP_DATA MPU9250::readTemp() {
+    MPU9250_TEMP_DATA data;
+    uint8_t* dataBytePtr = (uint8_t *)&data;
+
+    data.isValid = readRegister(MPU9250_REG_TEMP_H, dataBytePtr, 2);
+    
+    //TODO conv from sensor to celsius: ((sensoroutput-roomtempoffset)/tempsensitivity)+21
+    
+    return data;
+}
+
+MPU9250_MAG_DATA MPU9250::readMag() {
+    MPU9250_MAG_DATA data;
+    uint8_t magDataReady;
+    uint8_t* dataBytePtr = (uint8_t *)&data;
+    
+    //wait for magnetometer reading to be finished
+    do { readMagRegister(MPU9250_MAG_REG_STATUS,&magDataReady); }
+    while (!(magDataReady&0x01));
+    
+    //read magnetometer data:
+    data.isValid = readMagRegister(MPU9250_MAG_REG_X_L, dataBytePtr, 6);
+    
+    //high and low nibble need to be reversed because of register map order
+    data.x = SHORT_SWAP_BYTE(data.x);
+    data.y = SHORT_SWAP_BYTE(data.y);
+    data.z = SHORT_SWAP_BYTE(data.z);
+    
+    return data;
+}
+
+bool MPU9250::wakeup() {
+    return writeRegister(MPU9250_REG_PWR1,0);
+}
+
+bool MPU9250::standby() {
+    //turns off sensing but keeps the gyroscope powered
+    return writeRegister(MPU9250_REG_PWR1, 0b00010000);
+}
+
+bool MPU9250::sleep() {
+    //TODO
+    return writeRegister(MPU9250_REG_PWR1, 0b01000000);
+}
\ No newline at end of file