Bender Robotics / Mbed 2 deprecated PLAUCI_full

posilani dat

Dependencies:   FatFileSystemCpp mbed PowerControl USBHostLite

mpu6000.cpp@26:5674b8978551, 2017-05-23 (annotated)

Committer:
PavelKumpan
Date:
Tue May 23 18:42:14 2017 +0000
Revision:
26:5674b8978551
Parent:
20:66ecb2f0e307 
Recreated communication protocol.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
legwinskij 18:7acae34b518d 1 // MPU6000 MBED library developed for swimmers
legwinskij 18:7acae34b518d 2
legwinskij 18:7acae34b518d 3 #include "mbed.h"
legwinskij 18:7acae34b518d 4 #include "mpu6000.h"
legwinskij 18:7acae34b518d 5
legwinskij 18:7acae34b518d 6 // Constructor
legwinskij 18:7acae34b518d 7 MPU6000::MPU6000(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
legwinskij 18:7acae34b518d 8
legwinskij 18:7acae34b518d 9 // Function that writes data to register
legwinskij 18:7acae34b518d 10 void MPU6000::regWrite(uint8_t ADDR, uint8_t DATA)
legwinskij 18:7acae34b518d 11 {
legwinskij 18:7acae34b518d 12 // CS low -> start communicating
legwinskij 18:7acae34b518d 13 cs = 0;
legwinskij 18:7acae34b518d 14 // Send register address first
legwinskij 18:7acae34b518d 15 spi.write(ADDR);
legwinskij 18:7acae34b518d 16 // Send value to be written
legwinskij 18:7acae34b518d 17 spi.write(DATA);
legwinskij 18:7acae34b518d 18 // Wait a bit, since chip select is only software defined
legwinskij 18:7acae34b518d 19 wait_us(1);
legwinskij 18:7acae34b518d 20 // CS high -> stop communicating
legwinskij 18:7acae34b518d 21 cs = 1;
legwinskij 18:7acae34b518d 22 }
legwinskij 18:7acae34b518d 23
legwinskij 18:7acae34b518d 24 // Function that reads data from register
legwinskij 18:7acae34b518d 25 uint8_t MPU6000::regRead(uint8_t ADDR)
legwinskij 18:7acae34b518d 26 {
legwinskij 18:7acae34b518d 27 // CS low -> start communicating
legwinskij 18:7acae34b518d 28 cs = 0;
legwinskij 18:7acae34b518d 29 // Write register address to read from ored with read flag
legwinskij 18:7acae34b518d 30 spi.write((ADDR | READ));
legwinskij 18:7acae34b518d 31 // Write dummy data to get respond clocked out
legwinskij 18:7acae34b518d 32 int DATA = spi.write(0x00);
legwinskij 18:7acae34b518d 33 // Wait a bit
legwinskij 18:7acae34b518d 34 wait_us(1);
legwinskij 18:7acae34b518d 35 // CS high -> stop communicating
legwinskij 18:7acae34b518d 36 cs = 1;
legwinskij 18:7acae34b518d 37 // Return what has been read
legwinskij 18:7acae34b518d 38 return DATA;
legwinskij 18:7acae34b518d 39 }
legwinskij 18:7acae34b518d 40
legwinskij 18:7acae34b518d 41 // Initializes MPU6000
legwinskij 20:66ecb2f0e307 42 uint8_t MPU6000::init()
legwinskij 18:7acae34b518d 43 {
legwinskij 18:7acae34b518d 44 // Set chipselect high
legwinskij 18:7acae34b518d 45 cs = 1;
legwinskij 18:7acae34b518d 46 // Set spi bit length and format (use mode 3)
legwinskij 18:7acae34b518d 47 spi.format(8, 3);
legwinskij 18:7acae34b518d 48 // Set spi frquency to 1MHz (1MHz is max of MPU6000)
legwinskij 18:7acae34b518d 49 spi.frequency(1000000);
legwinskij 18:7acae34b518d 50 // Disable I2C first
legwinskij 18:7acae34b518d 51 regWrite(USER_CTRL, 0x10);
legwinskij 18:7acae34b518d 52 // Reboot
legwinskij 18:7acae34b518d 53 regWrite(PWR_MGMT_1, 0x80);
legwinskij 18:7acae34b518d 54 // Wait a bit for MPU to reset
legwinskij 18:7acae34b518d 55 wait_ms(150);
legwinskij 20:66ecb2f0e307 56 // Read WHOAMI register
legwinskij 20:66ecb2f0e307 57 uint8_t name = regRead(WHO_AM_I);
legwinskij 18:7acae34b518d 58 // Select gyro pll for clk (manufacturers recommendation)
legwinskij 18:7acae34b518d 59 regWrite(PWR_MGMT_1, 0x03);
legwinskij 18:7acae34b518d 60 // Disable I2C again (after reset registers clear themselves)
legwinskij 18:7acae34b518d 61 regWrite(USER_CTRL, 0x10);
legwinskij 18:7acae34b518d 62 // Set gyro full scale range to 500 dps (degrees per second)
legwinskij 18:7acae34b518d 63 regWrite(GYRO_CONFIG, 0x10);
legwinskij 18:7acae34b518d 64 // Set gyro full scale range to +- 4g
legwinskij 18:7acae34b518d 65 regWrite(ACCEL_CONFIG, 0x08);
legwinskij 18:7acae34b518d 66 // Wait a bit
legwinskij 18:7acae34b518d 67 wait_ms (150);
legwinskij 20:66ecb2f0e307 68 // Return name
legwinskij 20:66ecb2f0e307 69 return name;
legwinskij 18:7acae34b518d 70 }
legwinskij 18:7acae34b518d 71
legwinskij 18:7acae34b518d 72 // Reads current ACC values and stores them to provided buffer
legwinskij 18:7acae34b518d 73 void MPU6000::valRead(bool device, short int &x, short int &y, short int &z)
legwinskij 18:7acae34b518d 74 {
legwinskij 18:7acae34b518d 75 uint8_t bufHigh, bufLow;
legwinskij 18:7acae34b518d 76 // Switch based on selected device
legwinskij 18:7acae34b518d 77 switch(device)
legwinskij 18:7acae34b518d 78 {
legwinskij 18:7acae34b518d 79 // Handle accelerometer selection
legwinskij 18:7acae34b518d 80 case ACCEL:
legwinskij 18:7acae34b518d 81 {
legwinskij 18:7acae34b518d 82 // Wait a bit
legwinskij 18:7acae34b518d 83 wait_us(5);
legwinskij 18:7acae34b518d 84 // CS low -> start communicating
legwinskij 18:7acae34b518d 85 cs = 0;
legwinskij 18:7acae34b518d 86 // Write ACC_X high byte address, rest of values will come until we remain clocking -> burst mode
legwinskij 18:7acae34b518d 87 spi.write(ACCEL_XOUT_H | READ);
legwinskij 18:7acae34b518d 88 // ACC_X high byte
legwinskij 18:7acae34b518d 89 bufHigh = spi.write(0x00);
legwinskij 18:7acae34b518d 90 // ACC_X low byte
legwinskij 18:7acae34b518d 91 bufLow = spi.write(0x00);
legwinskij 18:7acae34b518d 92 // Merge ACC_X and add it to buffer
legwinskij 18:7acae34b518d 93 x = (short int)((bufHigh << 8) | bufLow);
legwinskij 18:7acae34b518d 94 // ACC_Y high byte
legwinskij 18:7acae34b518d 95 bufHigh = spi.write(0x00);
legwinskij 18:7acae34b518d 96 // ACC_Y low byte
legwinskij 18:7acae34b518d 97 bufLow = spi.write(0x00);
legwinskij 18:7acae34b518d 98 // Merge ACC_Y and add it to buffer
legwinskij 18:7acae34b518d 99 y = (short int)((bufHigh << 8) | bufLow);
legwinskij 18:7acae34b518d 100 // ACC_Z high byte
legwinskij 18:7acae34b518d 101 bufHigh = spi.write(0x00);
legwinskij 18:7acae34b518d 102 // ACC_Y low byte
legwinskij 18:7acae34b518d 103 bufLow = spi.write(0x00);
legwinskij 18:7acae34b518d 104 // Merge ACC_Z and add it to buffer
legwinskij 18:7acae34b518d 105 z = (short int)((bufHigh << 8) | bufLow);
legwinskij 18:7acae34b518d 106 // CS high -> stop communicating
legwinskij 18:7acae34b518d 107 cs = 1;
legwinskij 18:7acae34b518d 108 break;
legwinskij 18:7acae34b518d 109 }
legwinskij 18:7acae34b518d 110 // Handle gyro selection
legwinskij 18:7acae34b518d 111 case GYRO:
legwinskij 18:7acae34b518d 112 {
legwinskij 18:7acae34b518d 113 // Wait a bit
legwinskij 18:7acae34b518d 114 wait_us(5);
legwinskij 18:7acae34b518d 115 // CS low -> start communicating
legwinskij 18:7acae34b518d 116 cs = 0;
legwinskij 18:7acae34b518d 117 // Write GYRO_X high byte address, rest of values will come until we remain clocking -> burst mode
legwinskij 18:7acae34b518d 118 spi.write(GYRO_XOUT_H | READ);
legwinskij 18:7acae34b518d 119 // GYRO_X high byte
legwinskij 18:7acae34b518d 120 bufHigh = spi.write(0x00);
legwinskij 18:7acae34b518d 121 // GYRO_X low byte
legwinskij 18:7acae34b518d 122 bufLow = spi.write(0x00);
legwinskij 18:7acae34b518d 123 // Merge GYRO_X and add it to buffer
legwinskij 18:7acae34b518d 124 x = (short int)((bufHigh << 8) | bufLow);
legwinskij 18:7acae34b518d 125 // GYRO_Y high byte
legwinskij 18:7acae34b518d 126 bufHigh = spi.write(0x00);
legwinskij 18:7acae34b518d 127 // GYRO_Y low byte
legwinskij 18:7acae34b518d 128 bufLow = spi.write(0x00);
legwinskij 18:7acae34b518d 129 // Merge GYRO_Y and add it to buffer
legwinskij 18:7acae34b518d 130 y = (short int)((bufHigh << 8) | bufLow);
legwinskij 18:7acae34b518d 131 // GYRO_Z high byte
legwinskij 18:7acae34b518d 132 bufHigh = spi.write(0x00);
legwinskij 18:7acae34b518d 133 // GYRO_Z low byte
legwinskij 18:7acae34b518d 134 bufLow = spi.write(0x00);
legwinskij 18:7acae34b518d 135 // Merge GYRO_Z and add it to buffer
legwinskij 18:7acae34b518d 136 z = (short int)((bufHigh << 8) | bufLow);
legwinskij 18:7acae34b518d 137 // CS high -> stop communicating
legwinskij 18:7acae34b518d 138 cs = 1;
legwinskij 18:7acae34b518d 139 break;
legwinskij 18:7acae34b518d 140 }
legwinskij 18:7acae34b518d 141 }
legwinskij 18:7acae34b518d 142 }