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@8:609a2ad4c30e, 2015-11-19 (annotated)

Committer:
maetugr
Date:
Thu Nov 19 18:47:27 2015 +0000
Revision:
8:609a2ad4c30e
Parent:
5:8ea99e98de73 
made I2C-Sensors working in parallel, added rolling buffer for PID derivative, played with the PID and frequency parameters in main

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 8:609a2ad4c30e 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 8:609a2ad4c30e 38 int16_t offsetGyro[3] = {-31, -16, -12}; // 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