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

Committer:
maetugr
Date:
Thu Nov 19 18:47:27 2015 +0000
Revision:
8:609a2ad4c30e
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 8:609a2ad4c30e 1 // based on http://mbed.org/users/garfieldsg/code/MPU6050/
maetugr 8:609a2ad4c30e 2
maetugr 8:609a2ad4c30e 3 #ifndef MPU6050_H
maetugr 8:609a2ad4c30e 4 #define MPU6050_H
maetugr 8:609a2ad4c30e 5
maetugr 8:609a2ad4c30e 6 #include "mbed.h"
maetugr 8:609a2ad4c30e 7 #include "I2C_Sensor.h"
maetugr 8:609a2ad4c30e 8
maetugr 8:609a2ad4c30e 9 #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
maetugr 8:609a2ad4c30e 10 #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
maetugr 8:609a2ad4c30e 11 #define MPU6050_I2C_ADDRESS 0xD2 // adresses above multiplied by 2
maetugr 8:609a2ad4c30e 12
maetugr 8:609a2ad4c30e 13 // register addresses
maetugr 8:609a2ad4c30e 14 #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
maetugr 8:609a2ad4c30e 15 #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
maetugr 8:609a2ad4c30e 16 #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
maetugr 8:609a2ad4c30e 17 #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
maetugr 8:609a2ad4c30e 18 #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
maetugr 8:609a2ad4c30e 19 #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
maetugr 8:609a2ad4c30e 20 #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
maetugr 8:609a2ad4c30e 21 #define MPU6050_RA_XA_OFFS_L_TC 0x07
maetugr 8:609a2ad4c30e 22 #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
maetugr 8:609a2ad4c30e 23 #define MPU6050_RA_YA_OFFS_L_TC 0x09
maetugr 8:609a2ad4c30e 24 #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
maetugr 8:609a2ad4c30e 25 #define MPU6050_RA_ZA_OFFS_L_TC 0x0B
maetugr 8:609a2ad4c30e 26 #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
maetugr 8:609a2ad4c30e 27 #define MPU6050_RA_XG_OFFS_USRL 0x14
maetugr 8:609a2ad4c30e 28 #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
maetugr 8:609a2ad4c30e 29 #define MPU6050_RA_YG_OFFS_USRL 0x16
maetugr 8:609a2ad4c30e 30 #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
maetugr 8:609a2ad4c30e 31 #define MPU6050_RA_ZG_OFFS_USRL 0x18
maetugr 8:609a2ad4c30e 32 #define MPU6050_RA_SMPLRT_DIV 0x19
maetugr 8:609a2ad4c30e 33 #define MPU6050_RA_CONFIG 0x1A
maetugr 8:609a2ad4c30e 34 #define MPU6050_RA_GYRO_CONFIG 0x1B
maetugr 8:609a2ad4c30e 35 #define MPU6050_RA_ACCEL_CONFIG 0x1C
maetugr 8:609a2ad4c30e 36 #define MPU6050_RA_FF_THR 0x1D
maetugr 8:609a2ad4c30e 37 #define MPU6050_RA_FF_DUR 0x1E
maetugr 8:609a2ad4c30e 38 #define MPU6050_RA_MOT_THR 0x1F
maetugr 8:609a2ad4c30e 39 #define MPU6050_RA_MOT_DUR 0x20
maetugr 8:609a2ad4c30e 40 #define MPU6050_RA_ZRMOT_THR 0x21
maetugr 8:609a2ad4c30e 41 #define MPU6050_RA_ZRMOT_DUR 0x22
maetugr 8:609a2ad4c30e 42 #define MPU6050_RA_FIFO_EN 0x23
maetugr 8:609a2ad4c30e 43 #define MPU6050_RA_I2C_MST_CTRL 0x24
maetugr 8:609a2ad4c30e 44 #define MPU6050_RA_I2C_SLV0_ADDR 0x25
maetugr 8:609a2ad4c30e 45 #define MPU6050_RA_I2C_SLV0_REG 0x26
maetugr 8:609a2ad4c30e 46 #define MPU6050_RA_I2C_SLV0_CTRL 0x27
maetugr 8:609a2ad4c30e 47 #define MPU6050_RA_I2C_SLV1_ADDR 0x28
maetugr 8:609a2ad4c30e 48 #define MPU6050_RA_I2C_SLV1_REG 0x29
maetugr 8:609a2ad4c30e 49 #define MPU6050_RA_I2C_SLV1_CTRL 0x2A
maetugr 8:609a2ad4c30e 50 #define MPU6050_RA_I2C_SLV2_ADDR 0x2B
maetugr 8:609a2ad4c30e 51 #define MPU6050_RA_I2C_SLV2_REG 0x2C
maetugr 8:609a2ad4c30e 52 #define MPU6050_RA_I2C_SLV2_CTRL 0x2D
maetugr 8:609a2ad4c30e 53 #define MPU6050_RA_I2C_SLV3_ADDR 0x2E
maetugr 8:609a2ad4c30e 54 #define MPU6050_RA_I2C_SLV3_REG 0x2F
maetugr 8:609a2ad4c30e 55 #define MPU6050_RA_I2C_SLV3_CTRL 0x30
maetugr 8:609a2ad4c30e 56 #define MPU6050_RA_I2C_SLV4_ADDR 0x31
maetugr 8:609a2ad4c30e 57 #define MPU6050_RA_I2C_SLV4_REG 0x32
maetugr 8:609a2ad4c30e 58 #define MPU6050_RA_I2C_SLV4_DO 0x33
maetugr 8:609a2ad4c30e 59 #define MPU6050_RA_I2C_SLV4_CTRL 0x34
maetugr 8:609a2ad4c30e 60 #define MPU6050_RA_I2C_SLV4_DI 0x35
maetugr 8:609a2ad4c30e 61 #define MPU6050_RA_I2C_MST_STATUS 0x36
maetugr 8:609a2ad4c30e 62 #define MPU6050_RA_INT_PIN_CFG 0x37
maetugr 8:609a2ad4c30e 63 #define MPU6050_RA_INT_ENABLE 0x38
maetugr 8:609a2ad4c30e 64 #define MPU6050_RA_DMP_INT_STATUS 0x39
maetugr 8:609a2ad4c30e 65 #define MPU6050_RA_INT_STATUS 0x3A
maetugr 8:609a2ad4c30e 66 #define MPU6050_RA_ACCEL_XOUT_H 0x3B
maetugr 8:609a2ad4c30e 67 #define MPU6050_RA_ACCEL_XOUT_L 0x3C
maetugr 8:609a2ad4c30e 68 #define MPU6050_RA_ACCEL_YOUT_H 0x3D
maetugr 8:609a2ad4c30e 69 #define MPU6050_RA_ACCEL_YOUT_L 0x3E
maetugr 8:609a2ad4c30e 70 #define MPU6050_RA_ACCEL_ZOUT_H 0x3F
maetugr 8:609a2ad4c30e 71 #define MPU6050_RA_ACCEL_ZOUT_L 0x40
maetugr 8:609a2ad4c30e 72 #define MPU6050_RA_TEMP_OUT_H 0x41
maetugr 8:609a2ad4c30e 73 #define MPU6050_RA_TEMP_OUT_L 0x42
maetugr 8:609a2ad4c30e 74 #define MPU6050_RA_GYRO_XOUT_H 0x43
maetugr 8:609a2ad4c30e 75 #define MPU6050_RA_GYRO_XOUT_L 0x44
maetugr 8:609a2ad4c30e 76 #define MPU6050_RA_GYRO_YOUT_H 0x45
maetugr 8:609a2ad4c30e 77 #define MPU6050_RA_GYRO_YOUT_L 0x46
maetugr 8:609a2ad4c30e 78 #define MPU6050_RA_GYRO_ZOUT_H 0x47
maetugr 8:609a2ad4c30e 79 #define MPU6050_RA_GYRO_ZOUT_L 0x48
maetugr 8:609a2ad4c30e 80 #define MPU6050_RA_EXT_SENS_DATA_00 0x49
maetugr 8:609a2ad4c30e 81 #define MPU6050_RA_EXT_SENS_DATA_01 0x4A
maetugr 8:609a2ad4c30e 82 #define MPU6050_RA_EXT_SENS_DATA_02 0x4B
maetugr 8:609a2ad4c30e 83 #define MPU6050_RA_EXT_SENS_DATA_03 0x4C
maetugr 8:609a2ad4c30e 84 #define MPU6050_RA_EXT_SENS_DATA_04 0x4D
maetugr 8:609a2ad4c30e 85 #define MPU6050_RA_EXT_SENS_DATA_05 0x4E
maetugr 8:609a2ad4c30e 86 #define MPU6050_RA_EXT_SENS_DATA_06 0x4F
maetugr 8:609a2ad4c30e 87 #define MPU6050_RA_EXT_SENS_DATA_07 0x50
maetugr 8:609a2ad4c30e 88 #define MPU6050_RA_EXT_SENS_DATA_08 0x51
maetugr 8:609a2ad4c30e 89 #define MPU6050_RA_EXT_SENS_DATA_09 0x52
maetugr 8:609a2ad4c30e 90 #define MPU6050_RA_EXT_SENS_DATA_10 0x53
maetugr 8:609a2ad4c30e 91 #define MPU6050_RA_EXT_SENS_DATA_11 0x54
maetugr 8:609a2ad4c30e 92 #define MPU6050_RA_EXT_SENS_DATA_12 0x55
maetugr 8:609a2ad4c30e 93 #define MPU6050_RA_EXT_SENS_DATA_13 0x56
maetugr 8:609a2ad4c30e 94 #define MPU6050_RA_EXT_SENS_DATA_14 0x57
maetugr 8:609a2ad4c30e 95 #define MPU6050_RA_EXT_SENS_DATA_15 0x58
maetugr 8:609a2ad4c30e 96 #define MPU6050_RA_EXT_SENS_DATA_16 0x59
maetugr 8:609a2ad4c30e 97 #define MPU6050_RA_EXT_SENS_DATA_17 0x5A
maetugr 8:609a2ad4c30e 98 #define MPU6050_RA_EXT_SENS_DATA_18 0x5B
maetugr 8:609a2ad4c30e 99 #define MPU6050_RA_EXT_SENS_DATA_19 0x5C
maetugr 8:609a2ad4c30e 100 #define MPU6050_RA_EXT_SENS_DATA_20 0x5D
maetugr 8:609a2ad4c30e 101 #define MPU6050_RA_EXT_SENS_DATA_21 0x5E
maetugr 8:609a2ad4c30e 102 #define MPU6050_RA_EXT_SENS_DATA_22 0x5F
maetugr 8:609a2ad4c30e 103 #define MPU6050_RA_EXT_SENS_DATA_23 0x60
maetugr 8:609a2ad4c30e 104 #define MPU6050_RA_MOT_DETECT_STATUS 0x61
maetugr 8:609a2ad4c30e 105 #define MPU6050_RA_I2C_SLV0_DO 0x63
maetugr 8:609a2ad4c30e 106 #define MPU6050_RA_I2C_SLV1_DO 0x64
maetugr 8:609a2ad4c30e 107 #define MPU6050_RA_I2C_SLV2_DO 0x65
maetugr 8:609a2ad4c30e 108 #define MPU6050_RA_I2C_SLV3_DO 0x66
maetugr 8:609a2ad4c30e 109 #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
maetugr 8:609a2ad4c30e 110 #define MPU6050_RA_SIGNAL_PATH_RESET 0x68
maetugr 8:609a2ad4c30e 111 #define MPU6050_RA_MOT_DETECT_CTRL 0x69
maetugr 8:609a2ad4c30e 112 #define MPU6050_RA_USER_CTRL 0x6A
maetugr 8:609a2ad4c30e 113 #define MPU6050_RA_PWR_MGMT_1 0x6B
maetugr 8:609a2ad4c30e 114 #define MPU6050_RA_PWR_MGMT_2 0x6C
maetugr 8:609a2ad4c30e 115 #define MPU6050_RA_BANK_SEL 0x6D
maetugr 8:609a2ad4c30e 116 #define MPU6050_RA_MEM_START_ADDR 0x6E
maetugr 8:609a2ad4c30e 117 #define MPU6050_RA_MEM_R_W 0x6F
maetugr 8:609a2ad4c30e 118 #define MPU6050_RA_DMP_CFG_1 0x70
maetugr 8:609a2ad4c30e 119 #define MPU6050_RA_DMP_CFG_2 0x71
maetugr 8:609a2ad4c30e 120 #define MPU6050_RA_FIFO_COUNTH 0x72
maetugr 8:609a2ad4c30e 121 #define MPU6050_RA_FIFO_COUNTL 0x73
maetugr 8:609a2ad4c30e 122 #define MPU6050_RA_FIFO_R_W 0x74
maetugr 8:609a2ad4c30e 123 #define MPU6050_RA_WHO_AM_I 0x75
maetugr 8:609a2ad4c30e 124
maetugr 8:609a2ad4c30e 125 class MPU6050 : public I2C_Sensor
maetugr 8:609a2ad4c30e 126 {
maetugr 8:609a2ad4c30e 127 public:
maetugr 8:609a2ad4c30e 128 MPU6050(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
maetugr 8:609a2ad4c30e 129 float data_gyro[3]; // where the measured data is saved
maetugr 8:609a2ad4c30e 130 float data_acc[3]; // where the measured data is saved
maetugr 8:609a2ad4c30e 131 virtual void read(); // read all axis from register to array data
maetugr 8:609a2ad4c30e 132 float offset_gyro[3]; // offset that's subtracted from every Gyroscope measurement
maetugr 8:609a2ad4c30e 133 float offset_acc[3]; // offset that's subtracted from every Accelerometer measurement
maetugr 8:609a2ad4c30e 134 void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
maetugr 8:609a2ad4c30e 135 int readTemp(); // read temperature from sensor
maetugr 8:609a2ad4c30e 136
maetugr 8:609a2ad4c30e 137 int raw_gyro[3];
maetugr 8:609a2ad4c30e 138 private:
maetugr 8:609a2ad4c30e 139
maetugr 8:609a2ad4c30e 140 int raw_acc[3];
maetugr 8:609a2ad4c30e 141 void readraw_gyro();
maetugr 8:609a2ad4c30e 142 void readraw_acc();
maetugr 8:609a2ad4c30e 143 };
maetugr 8:609a2ad4c30e 144
maetugr 8:609a2ad4c30e 145 #endif