PLANET-Q MPU9250 Library
Dependents: IZU2020_AVIONICS IZU2020_AVIONICS
PQMPU9250.h@6:a7d11c40a920, 2019-11-24 (annotated)
- Committer:
- tanahashi
- Date:
- Sun Nov 24 06:08:54 2019 +0000
- Revision:
- 6:a7d11c40a920
- Parent:
- 5:cf516bc2bdda
- Child:
- 7:da63be95c693
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tanahashi | 0:c5ccb9d0afba | 1 | #ifndef PQMPU9250_H |
tanahashi | 0:c5ccb9d0afba | 2 | #define PQMPU9250_H |
tanahashi | 0:c5ccb9d0afba | 3 | |
tanahashi | 0:c5ccb9d0afba | 4 | #define MPU9250_ADDR_HIGH 0b1101001<<1 |
tanahashi | 0:c5ccb9d0afba | 5 | #define MPU9250_ADDR_LOW 0b1101000<<1 |
tanahashi | 0:c5ccb9d0afba | 6 | #define AK8963_ADDR 0b0001100<<1 |
tanahashi | 0:c5ccb9d0afba | 7 | #define WIA 0x00 |
tanahashi | 0:c5ccb9d0afba | 8 | #define ST1 0x02 |
tanahashi | 0:c5ccb9d0afba | 9 | #define HXL 0x03 |
tanahashi | 0:c5ccb9d0afba | 10 | #define CNTL1 0x0A |
tanahashi | 0:c5ccb9d0afba | 11 | #define GYRO_CONFIG 0x1B |
tanahashi | 0:c5ccb9d0afba | 12 | #define ACCEL_CONFIG 0x1C |
tanahashi | 0:c5ccb9d0afba | 13 | #define INT_PIN_CFG 0x37 |
tanahashi | 0:c5ccb9d0afba | 14 | #define ACCEL_XOUT_H 0x3B |
tanahashi | 0:c5ccb9d0afba | 15 | #define GYRO_XOUT_H 0x43 |
tanahashi | 0:c5ccb9d0afba | 16 | #define PWR_MGMT_1 0x6B |
tanahashi | 0:c5ccb9d0afba | 17 | #define WHO_AM_I 0x75 |
tanahashi | 0:c5ccb9d0afba | 18 | #define ACCEL_LSB 0.0000610 |
tanahashi | 0:c5ccb9d0afba | 19 | #define GYRO_LSB 0.00763 |
tanahashi | 0:c5ccb9d0afba | 20 | #define MAG_LSB 0.150 |
tanahashi | 0:c5ccb9d0afba | 21 | |
tanahashi | 0:c5ccb9d0afba | 22 | typedef enum { |
tanahashi | 0:c5ccb9d0afba | 23 | AD0_HIGH = MPU9250_ADDR_HIGH, |
tanahashi | 0:c5ccb9d0afba | 24 | AD0_LOW = MPU9250_ADDR_LOW |
tanahashi | 0:c5ccb9d0afba | 25 | } AD0_t; |
tanahashi | 0:c5ccb9d0afba | 26 | |
tanahashi | 0:c5ccb9d0afba | 27 | typedef enum { |
tanahashi | 0:c5ccb9d0afba | 28 | _2G = 0b00000000, |
tanahashi | 0:c5ccb9d0afba | 29 | _4G = 0b00001000, |
tanahashi | 0:c5ccb9d0afba | 30 | _8G = 0b00010000, |
tanahashi | 0:c5ccb9d0afba | 31 | _16G = 0b00011000 |
tanahashi | 0:c5ccb9d0afba | 32 | } AccelConfig_t; |
tanahashi | 0:c5ccb9d0afba | 33 | |
tanahashi | 0:c5ccb9d0afba | 34 | typedef enum { |
tanahashi | 0:c5ccb9d0afba | 35 | _250DPS = 0b00000000, |
tanahashi | 0:c5ccb9d0afba | 36 | _500DPS = 0b00001000, |
tanahashi | 0:c5ccb9d0afba | 37 | _1000DPS = 0b00010000, |
tanahashi | 0:c5ccb9d0afba | 38 | _2000DPS = 0b00011000 |
tanahashi | 0:c5ccb9d0afba | 39 | } GyroConfig_t; |
tanahashi | 0:c5ccb9d0afba | 40 | |
tanahashi | 0:c5ccb9d0afba | 41 | typedef enum { |
tanahashi | 0:c5ccb9d0afba | 42 | _16B_8HZ = 0b00010010, |
tanahashi | 0:c5ccb9d0afba | 43 | _16B_100HZ = 0b00010110, |
tanahashi | 0:c5ccb9d0afba | 44 | _14B_8HZ = 0b00000010, |
tanahashi | 0:c5ccb9d0afba | 45 | _14B_100HZ = 0b00000100 |
tanahashi | 0:c5ccb9d0afba | 46 | } MagConfig_t; |
tanahashi | 0:c5ccb9d0afba | 47 | |
tanahashi | 5:cf516bc2bdda | 48 | /** |
tanahashi | 3:9042b3662a14 | 49 | * 9軸センサMPU9250のライブラリ |
tanahashi | 5:cf516bc2bdda | 50 | * @code |
tanahashi | 5:cf516bc2bdda | 51 | #include "mbed.h" |
tanahashi | 5:cf516bc2bdda | 52 | #include "PQMPU9250.h" |
tanahashi | 5:cf516bc2bdda | 53 | |
tanahashi | 5:cf516bc2bdda | 54 | Serial pc(USBTX, USBRX, 115200); |
tanahashi | 5:cf516bc2bdda | 55 | I2C i2c(p9, p10); |
tanahashi | 5:cf516bc2bdda | 56 | |
tanahashi | 5:cf516bc2bdda | 57 | MPU9250 mpu(i2c, AD0_HIGH); |
tanahashi | 5:cf516bc2bdda | 58 | |
tanahashi | 5:cf516bc2bdda | 59 | float accel_offset[] = {0, 0, 0}; |
tanahashi | 5:cf516bc2bdda | 60 | float gyro_offset[] = {0, 0, 0}; |
tanahashi | 5:cf516bc2bdda | 61 | float mag_offset[] = {0, 0, 0}; |
tanahashi | 5:cf516bc2bdda | 62 | float accel[3]; |
tanahashi | 5:cf516bc2bdda | 63 | float gyro[3]; |
tanahashi | 5:cf516bc2bdda | 64 | float mag[3]; |
tanahashi | 5:cf516bc2bdda | 65 | |
tanahashi | 5:cf516bc2bdda | 66 | int main() { |
tanahashi | 5:cf516bc2bdda | 67 | mpu.begin(); |
tanahashi | 5:cf516bc2bdda | 68 | mpu.set(_2G, _250DPS, _16B_8HZ); |
tanahashi | 5:cf516bc2bdda | 69 | mpu.offset(accel_offset, gyro_offset, mag_offset); |
tanahashi | 5:cf516bc2bdda | 70 | if(!mpu.test()){ |
tanahashi | 5:cf516bc2bdda | 71 | pc.printf("[ FAIL ] MPU9250 cannot be reached.\r\n"); |
tanahashi | 5:cf516bc2bdda | 72 | return 0; |
tanahashi | 5:cf516bc2bdda | 73 | } |
tanahashi | 5:cf516bc2bdda | 74 | if(!mpu.test_AK8963()){ |
tanahashi | 5:cf516bc2bdda | 75 | pc.printf("[ FAIL ] AK8963 cannot be reached.\r\n"); |
tanahashi | 5:cf516bc2bdda | 76 | return 0; |
tanahashi | 5:cf516bc2bdda | 77 | } |
tanahashi | 5:cf516bc2bdda | 78 | while(1) { |
tanahashi | 5:cf516bc2bdda | 79 | mpu.read(accel, gyro, mag); |
tanahashi | 5:cf516bc2bdda | 80 | pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], mag[0], mag[1], mag[2]); |
tanahashi | 5:cf516bc2bdda | 81 | } |
tanahashi | 5:cf516bc2bdda | 82 | } |
tanahashi | 6:a7d11c40a920 | 83 | * @endcode |
tanahashi | 3:9042b3662a14 | 84 | */ |
tanahashi | 0:c5ccb9d0afba | 85 | class MPU9250 |
tanahashi | 0:c5ccb9d0afba | 86 | { |
tanahashi | 0:c5ccb9d0afba | 87 | private: |
tanahashi | 5:cf516bc2bdda | 88 | I2C *_i2c; |
tanahashi | 5:cf516bc2bdda | 89 | int _addr; |
tanahashi | 0:c5ccb9d0afba | 90 | char cmd[2]; |
tanahashi | 0:c5ccb9d0afba | 91 | char buff[8]; |
tanahashi | 0:c5ccb9d0afba | 92 | float accel_LSB; |
tanahashi | 0:c5ccb9d0afba | 93 | float gyro_LSB; |
tanahashi | 0:c5ccb9d0afba | 94 | float mag_LSB; |
tanahashi | 0:c5ccb9d0afba | 95 | float accel_offset[3]; |
tanahashi | 0:c5ccb9d0afba | 96 | float gyro_offset[3]; |
tanahashi | 0:c5ccb9d0afba | 97 | float mag_offset[3]; |
tanahashi | 0:c5ccb9d0afba | 98 | public: |
tanahashi | 5:cf516bc2bdda | 99 | |
tanahashi | 0:c5ccb9d0afba | 100 | /** |
tanahashi | 0:c5ccb9d0afba | 101 | * @param i2c I2Cのインスタンスへの参照 |
tanahashi | 0:c5ccb9d0afba | 102 | * @param AD0 AD0ピンのH/Lレベル |
tanahashi | 0:c5ccb9d0afba | 103 | */ |
tanahashi | 0:c5ccb9d0afba | 104 | MPU9250(I2C &i2c, AD0_t AD0); |
tanahashi | 0:c5ccb9d0afba | 105 | |
tanahashi | 0:c5ccb9d0afba | 106 | /** |
tanahashi | 0:c5ccb9d0afba | 107 | * センサ動作開始 |
tanahashi | 0:c5ccb9d0afba | 108 | */ |
tanahashi | 0:c5ccb9d0afba | 109 | void begin(); |
tanahashi | 0:c5ccb9d0afba | 110 | |
tanahashi | 0:c5ccb9d0afba | 111 | /** |
tanahashi | 0:c5ccb9d0afba | 112 | * センサ通信テスト |
tanahashi | 5:cf516bc2bdda | 113 | * @retval true 通信成功 |
tanahashi | 5:cf516bc2bdda | 114 | * @retval false 通信失敗 |
tanahashi | 0:c5ccb9d0afba | 115 | */ |
tanahashi | 0:c5ccb9d0afba | 116 | bool test(); |
tanahashi | 0:c5ccb9d0afba | 117 | |
tanahashi | 4:13d220528a23 | 118 | /** |
tanahashi | 5:cf516bc2bdda | 119 | * 磁気センサAK8963通信テスト |
tanahashi | 5:cf516bc2bdda | 120 | * @retval true 通信成功 |
tanahashi | 5:cf516bc2bdda | 121 | * @retval false 通信失敗 |
tanahashi | 4:13d220528a23 | 122 | */ |
tanahashi | 0:c5ccb9d0afba | 123 | bool test_AK8963(); |
tanahashi | 0:c5ccb9d0afba | 124 | |
tanahashi | 0:c5ccb9d0afba | 125 | /** |
tanahashi | 0:c5ccb9d0afba | 126 | * センサ設定 |
tanahashi | 0:c5ccb9d0afba | 127 | * @param accel_config 加速度の測定レンジ |
tanahashi | 5:cf516bc2bdda | 128 | * @param gyro_config 角速度の測定レンジ |
tanahashi | 5:cf516bc2bdda | 129 | * @param mag_config 磁気センサの分解能/データレート |
tanahashi | 0:c5ccb9d0afba | 130 | */ |
tanahashi | 0:c5ccb9d0afba | 131 | void set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config); |
tanahashi | 0:c5ccb9d0afba | 132 | |
tanahashi | 0:c5ccb9d0afba | 133 | /** |
tanahashi | 0:c5ccb9d0afba | 134 | * 加速度センサ設定 |
tanahashi | 0:c5ccb9d0afba | 135 | * @param accel_config 角速度センサの測定レンジ |
tanahashi | 0:c5ccb9d0afba | 136 | */ |
tanahashi | 0:c5ccb9d0afba | 137 | void set_accel(AccelConfig_t accel_config); |
tanahashi | 4:13d220528a23 | 138 | |
tanahashi | 4:13d220528a23 | 139 | /** |
tanahashi | 5:cf516bc2bdda | 140 | * 角速度センサ設定 |
tanahashi | 5:cf516bc2bdda | 141 | * @param gyro_config 角速度の測定レンジ |
tanahashi | 4:13d220528a23 | 142 | */ |
tanahashi | 0:c5ccb9d0afba | 143 | void set_gyro(GyroConfig_t gyro_config); |
tanahashi | 4:13d220528a23 | 144 | |
tanahashi | 4:13d220528a23 | 145 | /** |
tanahashi | 5:cf516bc2bdda | 146 | * 磁気センサ設定 |
tanahashi | 5:cf516bc2bdda | 147 | * @param mag_config 磁気センサの分解能/データレート |
tanahashi | 4:13d220528a23 | 148 | */ |
tanahashi | 0:c5ccb9d0afba | 149 | void set_mag(MagConfig_t mag_config); |
tanahashi | 4:13d220528a23 | 150 | |
tanahashi | 4:13d220528a23 | 151 | /** |
tanahashi | 5:cf516bc2bdda | 152 | * ゼロ点補正 |
tanahashi | 5:cf516bc2bdda | 153 | * @param accel 加速度のオフセット配列 |
tanahashi | 5:cf516bc2bdda | 154 | * @param gyro 角速度のオフセット配列 |
tanahashi | 5:cf516bc2bdda | 155 | * @param mag 磁気のオフセット配列 |
tanahashi | 4:13d220528a23 | 156 | */ |
tanahashi | 0:c5ccb9d0afba | 157 | void offset(float *accel, float *gyro, float *mag); |
tanahashi | 4:13d220528a23 | 158 | |
tanahashi | 4:13d220528a23 | 159 | /** |
tanahashi | 5:cf516bc2bdda | 160 | * 加速度のゼロ点補正 |
tanahashi | 5:cf516bc2bdda | 161 | * @param accel 加速度のオフセット配列 |
tanahashi | 4:13d220528a23 | 162 | */ |
tanahashi | 0:c5ccb9d0afba | 163 | void offset_accel(float *accel); |
tanahashi | 4:13d220528a23 | 164 | |
tanahashi | 4:13d220528a23 | 165 | /** |
tanahashi | 5:cf516bc2bdda | 166 | * 角速度のゼロ点補正 |
tanahashi | 5:cf516bc2bdda | 167 | * @param gyro 角速度のオフセット配列 |
tanahashi | 4:13d220528a23 | 168 | */ |
tanahashi | 0:c5ccb9d0afba | 169 | void offset_gyro(float *gyro); |
tanahashi | 4:13d220528a23 | 170 | |
tanahashi | 4:13d220528a23 | 171 | /** |
tanahashi | 5:cf516bc2bdda | 172 | * 磁気のゼロ点補正 |
tanahashi | 5:cf516bc2bdda | 173 | * @param mag 磁気のオフセット配列 |
tanahashi | 4:13d220528a23 | 174 | */ |
tanahashi | 0:c5ccb9d0afba | 175 | void offset_mag(float *mag); |
tanahashi | 4:13d220528a23 | 176 | |
tanahashi | 4:13d220528a23 | 177 | /** |
tanahashi | 5:cf516bc2bdda | 178 | * 測定値の読み取り |
tanahashi | 5:cf516bc2bdda | 179 | * @param accel 加速度を格納する配列 |
tanahashi | 5:cf516bc2bdda | 180 | * @param gyro 角速度を格納する配列 |
tanahashi | 5:cf516bc2bdda | 181 | * @param mag 磁気を格納する配列 |
tanahashi | 4:13d220528a23 | 182 | */ |
tanahashi | 0:c5ccb9d0afba | 183 | void read(float *accel, float *gyro, float *mag); |
tanahashi | 4:13d220528a23 | 184 | |
tanahashi | 4:13d220528a23 | 185 | /** |
tanahashi | 5:cf516bc2bdda | 186 | * 加速度測定値の読み取り |
tanahashi | 5:cf516bc2bdda | 187 | * @param accel 加速度を格納する配列 |
tanahashi | 4:13d220528a23 | 188 | */ |
tanahashi | 0:c5ccb9d0afba | 189 | void read_accel(float *accel); |
tanahashi | 4:13d220528a23 | 190 | |
tanahashi | 4:13d220528a23 | 191 | /** |
tanahashi | 5:cf516bc2bdda | 192 | * 角速度測定値の読み取り |
tanahashi | 5:cf516bc2bdda | 193 | * @param gyro 角速度を格納する配列 |
tanahashi | 4:13d220528a23 | 194 | */ |
tanahashi | 0:c5ccb9d0afba | 195 | void read_gyro(float *gyro); |
tanahashi | 4:13d220528a23 | 196 | |
tanahashi | 4:13d220528a23 | 197 | /** |
tanahashi | 5:cf516bc2bdda | 198 | * 磁気測定値の読み取り |
tanahashi | 5:cf516bc2bdda | 199 | * @param mag 磁気を格納する配列 |
tanahashi | 4:13d220528a23 | 200 | */ |
tanahashi | 0:c5ccb9d0afba | 201 | void read_mag(float *mag); |
tanahashi | 0:c5ccb9d0afba | 202 | }; |
tanahashi | 0:c5ccb9d0afba | 203 | |
tanahashi | 0:c5ccb9d0afba | 204 | #endif |