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