Code for break using sensor
Dependencies: SEGGER_RTT mbed-dev
Fork of DigitalOut_HelloWorld by
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