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