Matthias Grob / Mbed 2 deprecated FlyBed3

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)

Dependencies:   mbed

IMU/MPU9250/MPU9250.cpp@3:3709be130495, 2015-09-10 (annotated)

Committer:
maetugr
Date:
Thu Sep 10 17:10:11 2015 +0000
Revision:
3:3709be130495
Parent:
0:37f0c1e8fa66
Child:
4:b2efa7f03701
test of auto leveling mode without battery on bed; try tuning madgwick now to rely more on gyro

Who changed what in which revision?

UserRevisionLine numberNew 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 3:3709be130495 20 writeRegister8(MPU9250_CONFIG, 0x00);
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 0:37f0c1e8fa66 41 Gyro[i] = (rawGyro[i] - offsetGyro[i])*0.07; // 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