Code for break using sensor
Dependencies: SEGGER_RTT mbed-dev
Fork of DigitalOut_HelloWorld by
MPU9250.cpp@2:8f53e653f9b1, 2017-12-18 (annotated)
- Committer:
- Anythingconnected
- Date:
- Mon Dec 18 10:18:59 2017 +0000
- Revision:
- 2:8f53e653f9b1
Getting byte by byte read to work
Who changed what in which revision?
User | Revision | Line number | New 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 | } |