anyThing Connected Team / BREAK_SENSOR_LED

Code for break using sensor

Dependencies:   SEGGER_RTT mbed-dev

Fork of DigitalOut_HelloWorld by mbed official

MPU9250.cpp@3:c058c2f600bb, 2017-12-18 (annotated)

Committer:
Anythingconnected
Date:
Mon Dec 18 10:41:06 2017 +0000
Revision:
3:c058c2f600bb
Parent:
2:8f53e653f9b1 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anythingconnected 2:8f53e653f9b1 1 //InvenSense MPU-9250
Anythingconnected 2:8f53e653f9b1 2 //Driver for the nrf51822 mcu
Anythingconnected 2:8f53e653f9b1 3 //maximum I2C speed is 400kHz
Anythingconnected 2:8f53e653f9b1 4
Anythingconnected 2:8f53e653f9b1 5 //TODO don't forget tap/double-tap functionality and calibration
Anythingconnected 2:8f53e653f9b1 6
Anythingconnected 2:8f53e653f9b1 7 #include "MPU9250.h"
Anythingconnected 2:8f53e653f9b1 8 #include "mbed.h"
Anythingconnected 2:8f53e653f9b1 9
Anythingconnected 2:8f53e653f9b1 10 bool MPU9250::readRegister(int addr, uint8_t* data, int length = 1) {
Anythingconnected 2:8f53e653f9b1 11 if (i2c.write(deviceAddress,(char *)&addr, 1, true)) {
Anythingconnected 2:8f53e653f9b1 12 if (i2c.read(deviceAddress,(char *)data, length, false)) {
Anythingconnected 2:8f53e653f9b1 13 return true;
Anythingconnected 2:8f53e653f9b1 14 }
Anythingconnected 2:8f53e653f9b1 15 }
Anythingconnected 2:8f53e653f9b1 16
Anythingconnected 2:8f53e653f9b1 17 return false;
Anythingconnected 2:8f53e653f9b1 18 }
Anythingconnected 2:8f53e653f9b1 19
Anythingconnected 2:8f53e653f9b1 20 bool MPU9250::writeRegister(int addr, uint8_t data) {
Anythingconnected 2:8f53e653f9b1 21 if (i2c.write(deviceAddress,(char *)&addr, 1, true)) {
Anythingconnected 2:8f53e653f9b1 22 bool ack = i2c.write(data);
Anythingconnected 2:8f53e653f9b1 23 i2c.stop();
Anythingconnected 2:8f53e653f9b1 24 return ack;
Anythingconnected 2:8f53e653f9b1 25 }
Anythingconnected 2:8f53e653f9b1 26
Anythingconnected 2:8f53e653f9b1 27 return false;
Anythingconnected 2:8f53e653f9b1 28 }
Anythingconnected 2:8f53e653f9b1 29
Anythingconnected 2:8f53e653f9b1 30 bool MPU9250::readMagRegister(int addr, uint8_t* data, int length = 1) {
Anythingconnected 2:8f53e653f9b1 31 if (i2c.write(MPU9250_REG_ID,(char *)&addr, 1, true)) {
Anythingconnected 2:8f53e653f9b1 32 if (i2c.read(MPU9250_REG_ID,(char *)data, length, false)) {
Anythingconnected 2:8f53e653f9b1 33 return true;
Anythingconnected 2:8f53e653f9b1 34 }
Anythingconnected 2:8f53e653f9b1 35 }
Anythingconnected 2:8f53e653f9b1 36
Anythingconnected 2:8f53e653f9b1 37 i2c.stop();
Anythingconnected 2:8f53e653f9b1 38
Anythingconnected 2:8f53e653f9b1 39 return false;
Anythingconnected 2:8f53e653f9b1 40 }
Anythingconnected 2:8f53e653f9b1 41
Anythingconnected 2:8f53e653f9b1 42 bool MPU9250::writeMagRegister(int addr, uint8_t data) {
Anythingconnected 2:8f53e653f9b1 43 if (i2c.write(MPU9250_REG_ID,(char *)&addr, 1, true)) {
Anythingconnected 2:8f53e653f9b1 44 bool ack = i2c.write(data);
Anythingconnected 2:8f53e653f9b1 45 i2c.stop();
Anythingconnected 2:8f53e653f9b1 46 return ack;
Anythingconnected 2:8f53e653f9b1 47 }
Anythingconnected 2:8f53e653f9b1 48
Anythingconnected 2:8f53e653f9b1 49 return false;
Anythingconnected 2:8f53e653f9b1 50 }
Anythingconnected 2:8f53e653f9b1 51
Anythingconnected 2:8f53e653f9b1 52 MPU9250::MPU9250(int SCLPin, int SDAPin, int addr) : i2c((PinName)SDAPin,(PinName)SCLPin) {
Anythingconnected 2:8f53e653f9b1 53 deviceAddress = addr;
Anythingconnected 2:8f53e653f9b1 54 //magnetometer has it's own non-changeable address defined in: MPU9250_REG_ID
Anythingconnected 2:8f53e653f9b1 55 }
Anythingconnected 2:8f53e653f9b1 56
Anythingconnected 2:8f53e653f9b1 57 bool MPU9250::init() {
Anythingconnected 2:8f53e653f9b1 58 //configure interrupts and disable FSYNC pin interrupt (not connected)
Anythingconnected 2:8f53e653f9b1 59 // ACTL 0 = INT pin active high
Anythingconnected 2:8f53e653f9b1 60 // OPEN 0 = INT pin is push-pull
Anythingconnected 2:8f53e653f9b1 61 // LATCH_INT_EN 1 = keep INT high until cleared
Anythingconnected 2:8f53e653f9b1 62 // INT_ANYRD_2CLEAR 1 = INT is cleared if any read operation is performed
Anythingconnected 2:8f53e653f9b1 63 // ACTL_FSYNC 0 = FSYNC is active high
Anythingconnected 2:8f53e653f9b1 64 // FSYNC_INT_MODE_EN 0 = disable FSYNC input
Anythingconnected 2:8f53e653f9b1 65 // BYPASS_EN 1 = set I2C master pins to bypass when I2C master is disabled
Anythingconnected 2:8f53e653f9b1 66 // RESERVED = keep at 0
Anythingconnected 2:8f53e653f9b1 67 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)
Anythingconnected 2:8f53e653f9b1 68
Anythingconnected 2:8f53e653f9b1 69 //configure gyroscope range
Anythingconnected 2:8f53e653f9b1 70 writeRegister(MPU9250_REG_GYROCFG,MPU9250_GYROSCALE_2000);
Anythingconnected 2:8f53e653f9b1 71
Anythingconnected 2:8f53e653f9b1 72 //configure accelerometer range
Anythingconnected 2:8f53e653f9b1 73 writeRegister(MPU9250_REG_ACCELCFG,MPU9250_ACCSCALE_8);
Anythingconnected 2:8f53e653f9b1 74
Anythingconnected 2:8f53e653f9b1 75 //read first magnetometer data
Anythingconnected 2:8f53e653f9b1 76 writeMagRegister(MPU9250_MAG_REG_CNTL, 0x01); //magnetometer single-measurement
Anythingconnected 2:8f53e653f9b1 77
Anythingconnected 2:8f53e653f9b1 78 //set interrupt conditions and enable interrupts
Anythingconnected 2:8f53e653f9b1 79 writeRegister(MPU9250_REG_WOMT, 0b00000011); //Set wake-on-motion threshold, range is 0mg to 1020mg (LSB = 4mg)
Anythingconnected 2:8f53e653f9b1 80 writeRegister(MPU9250_REG_INTEN, 0b01000000); //Enable wake-on-motion interrupt
Anythingconnected 2:8f53e653f9b1 81
Anythingconnected 2:8f53e653f9b1 82
Anythingconnected 2:8f53e653f9b1 83 return wakeup();
Anythingconnected 2:8f53e653f9b1 84 }
Anythingconnected 2:8f53e653f9b1 85
Anythingconnected 2:8f53e653f9b1 86 MPU9250_GYRO_DATA MPU9250::readGyro() {
Anythingconnected 2:8f53e653f9b1 87 MPU9250_GYRO_DATA data;
Anythingconnected 2:8f53e653f9b1 88 uint8_t* dataBytePtr = (uint8_t *)&data;
Anythingconnected 2:8f53e653f9b1 89
Anythingconnected 2:8f53e653f9b1 90 data.isValid = readRegister(MPU9250_REG_GYRO_X_H, dataBytePtr, 6);
Anythingconnected 2:8f53e653f9b1 91
Anythingconnected 2:8f53e653f9b1 92 return data;
Anythingconnected 2:8f53e653f9b1 93 }
Anythingconnected 2:8f53e653f9b1 94
Anythingconnected 2:8f53e653f9b1 95 MPU9250_ACCEL_DATA MPU9250::readAccel() {
Anythingconnected 2:8f53e653f9b1 96 MPU9250_ACCEL_DATA data;
Anythingconnected 2:8f53e653f9b1 97 uint8_t* dataBytePtr = (uint8_t *)&data;
Anythingconnected 2:8f53e653f9b1 98
Anythingconnected 2:8f53e653f9b1 99 data.isValid = readRegister(MPU9250_REG_ACCEL_X_H, dataBytePtr, 6);
Anythingconnected 2:8f53e653f9b1 100
Anythingconnected 2:8f53e653f9b1 101 return data;
Anythingconnected 2:8f53e653f9b1 102 }
Anythingconnected 2:8f53e653f9b1 103
Anythingconnected 2:8f53e653f9b1 104 MPU9250_ACCEL_DATA MPU9250::readAccelNew() {
Anythingconnected 2:8f53e653f9b1 105 //MPU9250_ACCEL_DATA data;
Anythingconnected 2:8f53e653f9b1 106 //uint8_t* dataBytePtr = (uint8_t *)&data;
Anythingconnected 2:8f53e653f9b1 107
Anythingconnected 2:8f53e653f9b1 108 //data.isValid = readRegister(MPU9250_REG_ACCEL_X_H, dataBytePtr, 6);
Anythingconnected 2:8f53e653f9b1 109
Anythingconnected 2:8f53e653f9b1 110 //return data;
Anythingconnected 2:8f53e653f9b1 111
Anythingconnected 2:8f53e653f9b1 112 //EXAMPLE>
Anythingconnected 2:8f53e653f9b1 113 MPU9250_ACCEL_DATA data;
Anythingconnected 2:8f53e653f9b1 114 uint8_t oneByte;
Anythingconnected 2:8f53e653f9b1 115
Anythingconnected 2:8f53e653f9b1 116 //read one byte at a time and transfer it to the struct ourselves instead of working with pointers
Anythingconnected 2:8f53e653f9b1 117 //addr = register address
Anythingconnected 2:8f53e653f9b1 118
Anythingconnected 2:8f53e653f9b1 119 //#define MPU9250_REG_ACCEL_X_H 0x3B
Anythingconnected 2:8f53e653f9b1 120 i2c.write(deviceAddress,MPU9250_REG_ACCEL_X_H, 1, true);
Anythingconnected 2:8f53e653f9b1 121 i2c.read(deviceAddress, oneByte, 1, false);
Anythingconnected 2:8f53e653f9b1 122 data.x = (oneByte << 8); //high byte
Anythingconnected 2:8f53e653f9b1 123
Anythingconnected 2:8f53e653f9b1 124 i2c.write(deviceAddress,MPU9250_REG_ACCEL_X_L, 1, true);
Anythingconnected 2:8f53e653f9b1 125 i2c.read(deviceAddress, oneByte, 1, false);
Anythingconnected 2:8f53e653f9b1 126 data.x = data.x | oneByte; //low byte
Anythingconnected 2:8f53e653f9b1 127
Anythingconnected 2:8f53e653f9b1 128 i2c.stop();
Anythingconnected 2:8f53e653f9b1 129 return data;
Anythingconnected 2:8f53e653f9b1 130 //EXAMPLE<
Anythingconnected 2:8f53e653f9b1 131 }
Anythingconnected 2:8f53e653f9b1 132
Anythingconnected 2:8f53e653f9b1 133 MPU9250_TEMP_DATA MPU9250::readTemp() {
Anythingconnected 2:8f53e653f9b1 134 MPU9250_TEMP_DATA data;
Anythingconnected 2:8f53e653f9b1 135 uint8_t* dataBytePtr = (uint8_t *)&data;
Anythingconnected 2:8f53e653f9b1 136
Anythingconnected 2:8f53e653f9b1 137 data.isValid = readRegister(MPU9250_REG_TEMP_H, dataBytePtr, 2);
Anythingconnected 2:8f53e653f9b1 138
Anythingconnected 2:8f53e653f9b1 139 //TODO conv from sensor to celsius: ((sensoroutput-roomtempoffset)/tempsensitivity)+21
Anythingconnected 2:8f53e653f9b1 140
Anythingconnected 2:8f53e653f9b1 141 return data;
Anythingconnected 2:8f53e653f9b1 142 }
Anythingconnected 2:8f53e653f9b1 143
Anythingconnected 2:8f53e653f9b1 144 MPU9250_MAG_DATA MPU9250::readMag() {
Anythingconnected 2:8f53e653f9b1 145 MPU9250_MAG_DATA data;
Anythingconnected 2:8f53e653f9b1 146 uint8_t magDataReady;
Anythingconnected 2:8f53e653f9b1 147 uint8_t* dataBytePtr = (uint8_t *)&data;
Anythingconnected 2:8f53e653f9b1 148
Anythingconnected 2:8f53e653f9b1 149 //wait for magnetometer reading to be finished
Anythingconnected 2:8f53e653f9b1 150 do { readMagRegister(MPU9250_MAG_REG_STATUS,&magDataReady); }
Anythingconnected 2:8f53e653f9b1 151 while (!(magDataReady&0x01));
Anythingconnected 2:8f53e653f9b1 152
Anythingconnected 2:8f53e653f9b1 153 //read magnetometer data:
Anythingconnected 2:8f53e653f9b1 154 data.isValid = readMagRegister(MPU9250_MAG_REG_X_L, dataBytePtr, 6);
Anythingconnected 2:8f53e653f9b1 155
Anythingconnected 2:8f53e653f9b1 156 //high and low nibble need to be reversed because of register map order
Anythingconnected 2:8f53e653f9b1 157 data.x = SHORT_SWAP_BYTE(data.x);
Anythingconnected 2:8f53e653f9b1 158 data.y = SHORT_SWAP_BYTE(data.y);
Anythingconnected 2:8f53e653f9b1 159 data.z = SHORT_SWAP_BYTE(data.z);
Anythingconnected 2:8f53e653f9b1 160
Anythingconnected 2:8f53e653f9b1 161 return data;
Anythingconnected 2:8f53e653f9b1 162 }
Anythingconnected 2:8f53e653f9b1 163
Anythingconnected 2:8f53e653f9b1 164 bool MPU9250::wakeup() {
Anythingconnected 2:8f53e653f9b1 165 return writeRegister(MPU9250_REG_PWR1,0);
Anythingconnected 2:8f53e653f9b1 166 }
Anythingconnected 2:8f53e653f9b1 167
Anythingconnected 2:8f53e653f9b1 168 bool MPU9250::standby() {
Anythingconnected 2:8f53e653f9b1 169 //turns off sensing but keeps the gyroscope powered
Anythingconnected 2:8f53e653f9b1 170 return writeRegister(MPU9250_REG_PWR1, 0b00010000);
Anythingconnected 2:8f53e653f9b1 171 }
Anythingconnected 2:8f53e653f9b1 172
Anythingconnected 2:8f53e653f9b1 173 bool MPU9250::sleep() {
Anythingconnected 2:8f53e653f9b1 174 //TODO
Anythingconnected 2:8f53e653f9b1 175 return writeRegister(MPU9250_REG_PWR1, 0b01000000);
Anythingconnected 2:8f53e653f9b1 176 }