Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)
IMU/MPU9250/MPU9250.cpp@5:8ea99e98de73, 2015-09-11 (annotated)
- Committer:
- maetugr
- Date:
- Fri Sep 11 08:43:35 2015 +0000
- Revision:
- 5:8ea99e98de73
- Parent:
- 4:b2efa7f03701
- Child:
- 8:609a2ad4c30e
level mode with 12" props pretty stable indoors; some very small wobles remaining
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:37f0c1e8fa66 | 1 | #include "MPU9250.h" |
maetugr | 0:37f0c1e8fa66 | 2 | |
maetugr | 0:37f0c1e8fa66 | 3 | MPU9250::MPU9250(PinName MOSI, PinName MISO, PinName SCLK, PinName CS) : spi(MOSI, MISO, SCLK), cs(CS) { |
maetugr | 0:37f0c1e8fa66 | 4 | deselect(); // chip must be deselected first |
maetugr | 0:37f0c1e8fa66 | 5 | spi.format(8,0); // setup the spi for standard 8 bit data and SPI-Mode 0 |
maetugr | 0:37f0c1e8fa66 | 6 | spi.frequency(5e6); // with a 5MHz clock rate |
maetugr | 0:37f0c1e8fa66 | 7 | |
maetugr | 0:37f0c1e8fa66 | 8 | /* |
maetugr | 0:37f0c1e8fa66 | 9 | last 3 Bits of|Accelerometer(Fs=1kHz) |Gyroscope |
maetugr | 0:37f0c1e8fa66 | 10 | MPU9250_CONFIG|Bandwidth(Hz)|Delay(ms)|Bandwidth(Hz)|Delay(ms)|Fs(kHz) |
maetugr | 0:37f0c1e8fa66 | 11 | ------------------------------------------------------------------------- |
maetugr | 0:37f0c1e8fa66 | 12 | 0 |260 |0 |256 |0.98 |8 |
maetugr | 0:37f0c1e8fa66 | 13 | 1 |184 |2.0 |188 |1.9 |1 |
maetugr | 0:37f0c1e8fa66 | 14 | 2 |94 |3.0 |98 |2.8 |1 |
maetugr | 0:37f0c1e8fa66 | 15 | 3 |44 |4.9 |42 |4.8 |1 |
maetugr | 0:37f0c1e8fa66 | 16 | 4 |21 |8.5 |20 |8.3 |1 |
maetugr | 0:37f0c1e8fa66 | 17 | 5 |10 |13.8 |10 |13.4 |1 |
maetugr | 0:37f0c1e8fa66 | 18 | 6 |5 |19.0 |5 |18.6 |1 |
maetugr | 0:37f0c1e8fa66 | 19 | */ |
maetugr | 5:8ea99e98de73 | 20 | writeRegister8(MPU9250_CONFIG, 0x03); |
maetugr | 0:37f0c1e8fa66 | 21 | writeRegister8(MPU9250_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps |
maetugr | 0:37f0c1e8fa66 | 22 | writeRegister8(MPU9250_ACCEL_CONFIG, 0x08); // scales accelerometers range to +-4g |
maetugr | 0:37f0c1e8fa66 | 23 | } |
maetugr | 0:37f0c1e8fa66 | 24 | |
maetugr | 0:37f0c1e8fa66 | 25 | uint8_t MPU9250::getWhoami() { |
maetugr | 0:37f0c1e8fa66 | 26 | return readRegister8(MPU9250_WHO_AM_I); |
maetugr | 0:37f0c1e8fa66 | 27 | } |
maetugr | 0:37f0c1e8fa66 | 28 | |
maetugr | 0:37f0c1e8fa66 | 29 | float MPU9250::getTemperature() { |
maetugr | 0:37f0c1e8fa66 | 30 | int16_t data = readRegister16(MPU9250_TEMP_OUT_H); |
maetugr | 0:37f0c1e8fa66 | 31 | return ((data - 21) / 333.87) + 21; // formula from register map p.33 |
maetugr | 0:37f0c1e8fa66 | 32 | } |
maetugr | 0:37f0c1e8fa66 | 33 | |
maetugr | 0:37f0c1e8fa66 | 34 | void MPU9250::readGyro() { |
maetugr | 0:37f0c1e8fa66 | 35 | int16_t rawGyro[3]; |
maetugr | 0:37f0c1e8fa66 | 36 | readRegister48(MPU9250_GYRO_XOUT_H, rawGyro); |
maetugr | 0:37f0c1e8fa66 | 37 | |
maetugr | 0:37f0c1e8fa66 | 38 | int16_t offsetGyro[3] = {-31, -15, -11}; // TODO: make better calibration |
maetugr | 0:37f0c1e8fa66 | 39 | |
maetugr | 0:37f0c1e8fa66 | 40 | for (int i = 0; i < 3; i++) |
maetugr | 4:b2efa7f03701 | 41 | Gyro[i] = (rawGyro[i] - offsetGyro[i]) * 0.07 * 0.87; // subtract offset from calibration and multiply unit factor to get degree per second (datasheet p.10) |
maetugr | 0:37f0c1e8fa66 | 42 | |
maetugr | 0:37f0c1e8fa66 | 43 | |
maetugr | 0:37f0c1e8fa66 | 44 | |
maetugr | 0:37f0c1e8fa66 | 45 | float tmp = Gyro[0]; |
maetugr | 0:37f0c1e8fa66 | 46 | Gyro[0] = -Gyro[1]; |
maetugr | 0:37f0c1e8fa66 | 47 | Gyro[1] = -tmp; |
maetugr | 0:37f0c1e8fa66 | 48 | Gyro[2] = -Gyro[2]; |
maetugr | 0:37f0c1e8fa66 | 49 | } |
maetugr | 0:37f0c1e8fa66 | 50 | |
maetugr | 0:37f0c1e8fa66 | 51 | void MPU9250::readAcc() { |
maetugr | 0:37f0c1e8fa66 | 52 | int16_t rawAcc[3]; |
maetugr | 0:37f0c1e8fa66 | 53 | readRegister48(MPU9250_ACCEL_XOUT_H, rawAcc); |
maetugr | 0:37f0c1e8fa66 | 54 | |
maetugr | 0:37f0c1e8fa66 | 55 | int16_t offsetAcc[3] = {-120, -48, -438}; // TODO: make better calibration |
maetugr | 0:37f0c1e8fa66 | 56 | |
maetugr | 0:37f0c1e8fa66 | 57 | for (int i = 0; i < 3; i++) |
maetugr | 0:37f0c1e8fa66 | 58 | Acc[i] = (rawAcc[i] - offsetAcc[i])/8192.0; // TODO: didn't care about units because IMU-algorithm just uses vector direction |
maetugr | 0:37f0c1e8fa66 | 59 | |
maetugr | 0:37f0c1e8fa66 | 60 | |
maetugr | 0:37f0c1e8fa66 | 61 | |
maetugr | 0:37f0c1e8fa66 | 62 | float tmp = Acc[0]; |
maetugr | 0:37f0c1e8fa66 | 63 | Acc[0] = -Acc[1]; |
maetugr | 0:37f0c1e8fa66 | 64 | Acc[1] = -tmp; |
maetugr | 0:37f0c1e8fa66 | 65 | Acc[2] = -Acc[2]; |
maetugr | 0:37f0c1e8fa66 | 66 | } |
maetugr | 0:37f0c1e8fa66 | 67 | |
maetugr | 0:37f0c1e8fa66 | 68 | // PRIVATE Methods ------------------------------------------------------------------------------------ |
maetugr | 0:37f0c1e8fa66 | 69 | |
maetugr | 0:37f0c1e8fa66 | 70 | |
maetugr | 0:37f0c1e8fa66 | 71 | // SPI Interface -------------------------------------------------------------------------------------- |
maetugr | 0:37f0c1e8fa66 | 72 | uint8_t MPU9250::readRegister8(uint8_t reg) { |
maetugr | 0:37f0c1e8fa66 | 73 | uint8_t result; |
maetugr | 0:37f0c1e8fa66 | 74 | readRegister(reg, &result, 1); |
maetugr | 0:37f0c1e8fa66 | 75 | return result; |
maetugr | 0:37f0c1e8fa66 | 76 | } |
maetugr | 0:37f0c1e8fa66 | 77 | |
maetugr | 0:37f0c1e8fa66 | 78 | uint16_t MPU9250::readRegister16(uint8_t reg) { |
maetugr | 0:37f0c1e8fa66 | 79 | uint8_t result[2]; |
maetugr | 0:37f0c1e8fa66 | 80 | readRegister(reg, result, 2); |
maetugr | 0:37f0c1e8fa66 | 81 | return result[0]<<8 | result[1]; // join 8-Bit pieces to 16-bit short integer |
maetugr | 0:37f0c1e8fa66 | 82 | } |
maetugr | 0:37f0c1e8fa66 | 83 | |
maetugr | 0:37f0c1e8fa66 | 84 | void MPU9250::readRegister48(uint8_t reg, int16_t *buffer) { |
maetugr | 0:37f0c1e8fa66 | 85 | uint8_t result[6]; |
maetugr | 0:37f0c1e8fa66 | 86 | readRegister(reg, result, 6); |
maetugr | 0:37f0c1e8fa66 | 87 | buffer[0] = (int16_t) (result[0] << 8 | result[1]); // join 8-Bit pieces to 16-bit short integers |
maetugr | 0:37f0c1e8fa66 | 88 | buffer[1] = (int16_t) (result[2] << 8 | result[3]); |
maetugr | 0:37f0c1e8fa66 | 89 | buffer[2] = (int16_t) (result[4] << 8 | result[5]); |
maetugr | 0:37f0c1e8fa66 | 90 | } |
maetugr | 0:37f0c1e8fa66 | 91 | |
maetugr | 0:37f0c1e8fa66 | 92 | void MPU9250::writeRegister8(uint8_t reg, uint8_t buffer) { |
maetugr | 0:37f0c1e8fa66 | 93 | writeRegister(reg, &buffer, 1); |
maetugr | 0:37f0c1e8fa66 | 94 | } |
maetugr | 0:37f0c1e8fa66 | 95 | |
maetugr | 0:37f0c1e8fa66 | 96 | void MPU9250::readRegister(uint8_t reg, uint8_t *buffer, int length) { |
maetugr | 0:37f0c1e8fa66 | 97 | select(); |
maetugr | 0:37f0c1e8fa66 | 98 | spi.write(reg | 0x80); // send the register address we want to read and the read flag |
maetugr | 0:37f0c1e8fa66 | 99 | for(int i=0; i<length; i++) // get data |
maetugr | 0:37f0c1e8fa66 | 100 | buffer[i] = spi.write(0x00); |
maetugr | 0:37f0c1e8fa66 | 101 | deselect(); |
maetugr | 0:37f0c1e8fa66 | 102 | } |
maetugr | 0:37f0c1e8fa66 | 103 | |
maetugr | 0:37f0c1e8fa66 | 104 | void MPU9250::writeRegister(uint8_t reg, uint8_t *buffer, int length) { |
maetugr | 0:37f0c1e8fa66 | 105 | select(); |
maetugr | 0:37f0c1e8fa66 | 106 | spi.write(reg & ~0x80); // send the register address we want to write and the write flag |
maetugr | 0:37f0c1e8fa66 | 107 | for(int i=0; i<length; i++) // put data |
maetugr | 0:37f0c1e8fa66 | 108 | spi.write(buffer[i]); |
maetugr | 0:37f0c1e8fa66 | 109 | deselect(); |
maetugr | 0:37f0c1e8fa66 | 110 | } |
maetugr | 0:37f0c1e8fa66 | 111 | |
maetugr | 0:37f0c1e8fa66 | 112 | void MPU9250::select() { cs = 0; } // set Cable Select pin low to start SPI transaction |
maetugr | 0:37f0c1e8fa66 | 113 | void MPU9250::deselect() { cs = 1; } // set Cable Select pin high to stop SPI transaction |