![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
s
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of AutoFlight2017_now_copy by
MPU9250/MPU9250.h@1:f31e17341659, 2018-08-28 (annotated)
- Committer:
- TUATBM
- Date:
- Tue Aug 28 07:12:16 2018 +0000
- Revision:
- 1:f31e17341659
- Parent:
- 0:92024886c0be
aaa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TUATBM | 0:92024886c0be | 1 | #ifndef MPU9250_H |
TUATBM | 0:92024886c0be | 2 | #define MPU9250_H |
TUATBM | 0:92024886c0be | 3 | |
TUATBM | 0:92024886c0be | 4 | #include "mbed.h" |
TUATBM | 0:92024886c0be | 5 | #include "math.h" |
TUATBM | 0:92024886c0be | 6 | #include "MPU9250_register.h" |
TUATBM | 0:92024886c0be | 7 | |
TUATBM | 0:92024886c0be | 8 | |
TUATBM | 0:92024886c0be | 9 | #define AK8963_ADDRESS 0x0C<<1 |
TUATBM | 0:92024886c0be | 10 | |
TUATBM | 0:92024886c0be | 11 | // Using the MSENSR-9250 breakout board, ADO is set to 0 |
TUATBM | 0:92024886c0be | 12 | // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 |
TUATBM | 0:92024886c0be | 13 | //mbed uses the eight-bit device address, so shift seven-bit addresses left by one! |
TUATBM | 0:92024886c0be | 14 | #define ADO 0 |
TUATBM | 0:92024886c0be | 15 | #if ADO |
TUATBM | 0:92024886c0be | 16 | #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1 |
TUATBM | 0:92024886c0be | 17 | #else |
TUATBM | 0:92024886c0be | 18 | #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0 |
TUATBM | 0:92024886c0be | 19 | #endif |
TUATBM | 0:92024886c0be | 20 | |
TUATBM | 0:92024886c0be | 21 | #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral |
TUATBM | 0:92024886c0be | 22 | #define Ki 0.0f |
TUATBM | 0:92024886c0be | 23 | |
TUATBM | 0:92024886c0be | 24 | #define IAM_MPU9250 0x71 |
TUATBM | 0:92024886c0be | 25 | #define IAM_AK8963 0x48 |
TUATBM | 0:92024886c0be | 26 | |
TUATBM | 0:92024886c0be | 27 | #define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14 |
TUATBM | 0:92024886c0be | 28 | |
TUATBM | 0:92024886c0be | 29 | const float PI = 3.14159265358979323846f; |
TUATBM | 1:f31e17341659 | 30 | const float LPGAIN_MAG = 0.0; |
TUATBM | 1:f31e17341659 | 31 | |
TUATBM | 0:92024886c0be | 32 | |
TUATBM | 0:92024886c0be | 33 | enum Ascale { |
TUATBM | 0:92024886c0be | 34 | AFS_2G = 0, |
TUATBM | 0:92024886c0be | 35 | AFS_4G, |
TUATBM | 0:92024886c0be | 36 | AFS_8G, |
TUATBM | 0:92024886c0be | 37 | AFS_16G |
TUATBM | 0:92024886c0be | 38 | }; |
TUATBM | 0:92024886c0be | 39 | |
TUATBM | 0:92024886c0be | 40 | enum Gscale { |
TUATBM | 0:92024886c0be | 41 | GFS_250DPS = 0, |
TUATBM | 0:92024886c0be | 42 | GFS_500DPS, |
TUATBM | 0:92024886c0be | 43 | GFS_1000DPS, |
TUATBM | 0:92024886c0be | 44 | GFS_2000DPS |
TUATBM | 0:92024886c0be | 45 | }; |
TUATBM | 0:92024886c0be | 46 | |
TUATBM | 0:92024886c0be | 47 | enum Mscale { |
TUATBM | 0:92024886c0be | 48 | MFS_14BITS = 0, // 0.6 mG per LSB |
TUATBM | 0:92024886c0be | 49 | MFS_16BITS // 0.15 mG per LSB |
TUATBM | 0:92024886c0be | 50 | }; |
TUATBM | 0:92024886c0be | 51 | |
TUATBM | 0:92024886c0be | 52 | |
TUATBM | 0:92024886c0be | 53 | |
TUATBM | 0:92024886c0be | 54 | class MPU9250 { |
TUATBM | 0:92024886c0be | 55 | |
TUATBM | 0:92024886c0be | 56 | public: |
TUATBM | 0:92024886c0be | 57 | //MPU9250(); |
TUATBM | 1:f31e17341659 | 58 | MPU9250(PinName sda, PinName scl, RawSerial* serial_p); |
TUATBM | 0:92024886c0be | 59 | ~MPU9250(); |
TUATBM | 0:92024886c0be | 60 | |
TUATBM | 0:92024886c0be | 61 | bool Initialize(void); |
TUATBM | 0:92024886c0be | 62 | bool sensingAcGyMg(void); |
TUATBM | 0:92024886c0be | 63 | void calculatePostureAngle(float degree[3]); |
TUATBM | 1:f31e17341659 | 64 | float calculateYawByMg(void); |
TUATBM | 0:92024886c0be | 65 | |
TUATBM | 0:92024886c0be | 66 | void MPU9250SelfTest(float * destination); |
TUATBM | 0:92024886c0be | 67 | |
TUATBM | 0:92024886c0be | 68 | void pickupAccel(float accel[3]); |
TUATBM | 0:92024886c0be | 69 | void pickupGyro(float gyro[3]); |
TUATBM | 0:92024886c0be | 70 | void pickupMag(float mag[3]); |
TUATBM | 0:92024886c0be | 71 | float pickupTemp(void); |
TUATBM | 0:92024886c0be | 72 | |
TUATBM | 0:92024886c0be | 73 | void displayAccel(void); |
TUATBM | 0:92024886c0be | 74 | void displayGyro(void); |
TUATBM | 0:92024886c0be | 75 | void displayMag(void); |
TUATBM | 0:92024886c0be | 76 | void displayAngle(void); |
TUATBM | 0:92024886c0be | 77 | void displayQuaternion(void); |
TUATBM | 0:92024886c0be | 78 | void displayTemperature(void); |
TUATBM | 0:92024886c0be | 79 | |
TUATBM | 0:92024886c0be | 80 | void setMagBias(float bias_x, float bias_y, float bias_z); |
TUATBM | 0:92024886c0be | 81 | |
TUATBM | 0:92024886c0be | 82 | private: |
TUATBM | 0:92024886c0be | 83 | //add |
TUATBM | 0:92024886c0be | 84 | I2C *i2c_p; |
TUATBM | 0:92024886c0be | 85 | I2C &i2c; |
TUATBM | 0:92024886c0be | 86 | |
TUATBM | 1:f31e17341659 | 87 | RawSerial* pc_p; |
TUATBM | 0:92024886c0be | 88 | |
TUATBM | 0:92024886c0be | 89 | Timer timer; |
TUATBM | 0:92024886c0be | 90 | |
TUATBM | 0:92024886c0be | 91 | uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G |
TUATBM | 0:92024886c0be | 92 | uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS |
TUATBM | 0:92024886c0be | 93 | uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution |
TUATBM | 0:92024886c0be | 94 | uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR |
TUATBM | 0:92024886c0be | 95 | float aRes, gRes, mRes; // scale resolutions per LSB for the sensors |
TUATBM | 0:92024886c0be | 96 | |
TUATBM | 0:92024886c0be | 97 | int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output |
TUATBM | 0:92024886c0be | 98 | int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output |
TUATBM | 0:92024886c0be | 99 | int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output |
TUATBM | 0:92024886c0be | 100 | float magCalibration[3], magbias[3]; // Factory mag calibration and mag bias |
TUATBM | 0:92024886c0be | 101 | float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer |
TUATBM | 0:92024886c0be | 102 | float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values |
TUATBM | 0:92024886c0be | 103 | int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius |
TUATBM | 0:92024886c0be | 104 | float temperature; |
TUATBM | 0:92024886c0be | 105 | float SelfTest[6]; |
TUATBM | 0:92024886c0be | 106 | |
TUATBM | 0:92024886c0be | 107 | // parameters for 6 DoF sensor fusion calculations |
TUATBM | 0:92024886c0be | 108 | float GyroMeasError; // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 |
TUATBM | 0:92024886c0be | 109 | float beta; // compute beta |
TUATBM | 0:92024886c0be | 110 | float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) |
TUATBM | 0:92024886c0be | 111 | float zeta ; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value |
TUATBM | 0:92024886c0be | 112 | |
TUATBM | 0:92024886c0be | 113 | float pitch, yaw, roll; |
TUATBM | 0:92024886c0be | 114 | float deltat; // integration interval for both filter schemes |
TUATBM | 0:92024886c0be | 115 | int lastUpdate, firstUpdate, Now; // used to calculate integration interval // used to calculate integration interval |
TUATBM | 0:92024886c0be | 116 | float q[4]; // vector to hold quaternion |
TUATBM | 0:92024886c0be | 117 | float eInt[3]; // vector to hold integral error for Mahony method |
TUATBM | 0:92024886c0be | 118 | |
TUATBM | 1:f31e17341659 | 119 | float lpmag[3]; |
TUATBM | 0:92024886c0be | 120 | |
TUATBM | 0:92024886c0be | 121 | void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); |
TUATBM | 0:92024886c0be | 122 | char readByte(uint8_t address, uint8_t subAddress); |
TUATBM | 0:92024886c0be | 123 | void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); |
TUATBM | 0:92024886c0be | 124 | |
TUATBM | 0:92024886c0be | 125 | void initializeValue(void); |
TUATBM | 0:92024886c0be | 126 | |
TUATBM | 0:92024886c0be | 127 | void initMPU9250(void); |
TUATBM | 0:92024886c0be | 128 | void initAK8963(float * destination); |
TUATBM | 0:92024886c0be | 129 | void resetMPU9250(void); |
TUATBM | 0:92024886c0be | 130 | void calibrateMPU9250(float * dest1, float * dest2); |
TUATBM | 0:92024886c0be | 131 | |
TUATBM | 0:92024886c0be | 132 | void getMres(void); |
TUATBM | 0:92024886c0be | 133 | void getGres(void); |
TUATBM | 0:92024886c0be | 134 | void getAres(void); |
TUATBM | 0:92024886c0be | 135 | |
TUATBM | 0:92024886c0be | 136 | void readAccelData(int16_t * destination); |
TUATBM | 0:92024886c0be | 137 | void readGyroData(int16_t * destination); |
TUATBM | 0:92024886c0be | 138 | void readMagData(int16_t * destination); |
TUATBM | 0:92024886c0be | 139 | int16_t readTempData(void); |
TUATBM | 0:92024886c0be | 140 | |
TUATBM | 0:92024886c0be | 141 | uint8_t Whoami_MPU9250(void); |
TUATBM | 0:92024886c0be | 142 | uint8_t Whoami_AK8963(void); |
TUATBM | 0:92024886c0be | 143 | |
TUATBM | 0:92024886c0be | 144 | void sensingAccel(void); |
TUATBM | 0:92024886c0be | 145 | void sensingGyro(void); |
TUATBM | 0:92024886c0be | 146 | void sensingMag(void); |
TUATBM | 0:92024886c0be | 147 | void sensingTemp(void); |
TUATBM | 0:92024886c0be | 148 | |
TUATBM | 0:92024886c0be | 149 | void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
TUATBM | 0:92024886c0be | 150 | void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
TUATBM | 0:92024886c0be | 151 | void translateQuaternionToDeg(float quaternion[4]); |
TUATBM | 0:92024886c0be | 152 | void calibrateDegree(void); |
TUATBM | 0:92024886c0be | 153 | |
TUATBM | 1:f31e17341659 | 154 | void transformCoordinateFromCompassToMPU(); |
TUATBM | 0:92024886c0be | 155 | protected: |
TUATBM | 0:92024886c0be | 156 | |
TUATBM | 0:92024886c0be | 157 | }; |
TUATBM | 0:92024886c0be | 158 | |
TUATBM | 0:92024886c0be | 159 | #endif |