IZU2020 / PQMPU9250

PLANET-Q MPU9250 Library

Dependents:   IZU2020_AVIONICS IZU2020_AVIONICS

PQMPU9250.cpp@7:da63be95c693, 2019-12-17 (annotated)

Committer:
tanahashi
Date:
Tue Dec 17 14:08:27 2019 +0000
Revision:
7:da63be95c693
Parent:
0:c5ccb9d0afba 
fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanahashi 0:c5ccb9d0afba 1 #include "mbed.h"
tanahashi 0:c5ccb9d0afba 2 #include "PQMPU9250.h"
tanahashi 0:c5ccb9d0afba 3
tanahashi 0:c5ccb9d0afba 4 MPU9250::MPU9250(I2C &i2c, AD0_t AD0)
tanahashi 0:c5ccb9d0afba 5 {
tanahashi 0:c5ccb9d0afba 6 _i2c = &i2c;
tanahashi 0:c5ccb9d0afba 7 _addr = AD0;
tanahashi 0:c5ccb9d0afba 8 _i2c->frequency(400000);
tanahashi 0:c5ccb9d0afba 9 }
tanahashi 0:c5ccb9d0afba 10
tanahashi 0:c5ccb9d0afba 11 void MPU9250::begin()
tanahashi 0:c5ccb9d0afba 12 {
tanahashi 7:da63be95c693 13 cmd[0] = MPU9250_PWR_MGMT_1;
tanahashi 0:c5ccb9d0afba 14 cmd[1] = 0x00;
tanahashi 0:c5ccb9d0afba 15 _i2c->write(_addr, cmd, 2);
tanahashi 7:da63be95c693 16 cmd[0] = MPU9250_INT_PIN_CFG;
tanahashi 0:c5ccb9d0afba 17 cmd[1] = 0x02;
tanahashi 0:c5ccb9d0afba 18 _i2c->write(_addr, cmd, 2);
tanahashi 0:c5ccb9d0afba 19 set(_2G, _250DPS, _16B_8HZ);
tanahashi 0:c5ccb9d0afba 20 }
tanahashi 0:c5ccb9d0afba 21
tanahashi 7:da63be95c693 22 bool MPU9250::test()
tanahashi 7:da63be95c693 23 {
tanahashi 7:da63be95c693 24 cmd[0] = MPU9250_WHO_AM_I;
tanahashi 0:c5ccb9d0afba 25 _i2c->write(_addr, cmd, 1);
tanahashi 0:c5ccb9d0afba 26 _i2c->read(_addr, buff, 1);
tanahashi 0:c5ccb9d0afba 27 if (buff[0] == 0x71) {
tanahashi 0:c5ccb9d0afba 28 return true;
tanahashi 0:c5ccb9d0afba 29 } else {
tanahashi 0:c5ccb9d0afba 30 return false;
tanahashi 0:c5ccb9d0afba 31 }
tanahashi 0:c5ccb9d0afba 32 }
tanahashi 0:c5ccb9d0afba 33
tanahashi 0:c5ccb9d0afba 34 bool MPU9250::test_AK8963()
tanahashi 0:c5ccb9d0afba 35 {
tanahashi 7:da63be95c693 36 cmd[0] = MPU9250_WIA;
tanahashi 7:da63be95c693 37 _i2c->write(MPU9250_AK8963_ADDR, cmd, 1);
tanahashi 7:da63be95c693 38 _i2c->read(MPU9250_AK8963_ADDR, buff, 1);
tanahashi 0:c5ccb9d0afba 39 if (buff[0] == 0x48) {
tanahashi 0:c5ccb9d0afba 40 return true;
tanahashi 0:c5ccb9d0afba 41 } else {
tanahashi 0:c5ccb9d0afba 42 return false;
tanahashi 0:c5ccb9d0afba 43 }
tanahashi 0:c5ccb9d0afba 44 }
tanahashi 0:c5ccb9d0afba 45
tanahashi 7:da63be95c693 46 void MPU9250::set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config)
tanahashi 7:da63be95c693 47 {
tanahashi 0:c5ccb9d0afba 48 set_accel(accel_config);
tanahashi 0:c5ccb9d0afba 49 set_gyro(gyro_config);
tanahashi 0:c5ccb9d0afba 50 set_mag(mag_config);
tanahashi 0:c5ccb9d0afba 51 }
tanahashi 0:c5ccb9d0afba 52
tanahashi 0:c5ccb9d0afba 53 void MPU9250::set_accel(AccelConfig_t accel_config)
tanahashi 0:c5ccb9d0afba 54 {
tanahashi 7:da63be95c693 55 cmd[0] = MPU9250_ACCEL_CONFIG;
tanahashi 0:c5ccb9d0afba 56 cmd[1] = accel_config;
tanahashi 0:c5ccb9d0afba 57 _i2c->write(_addr, cmd, 2);
tanahashi 0:c5ccb9d0afba 58 if (accel_config == _2G) {
tanahashi 7:da63be95c693 59 accel_LSB = MPU9250_ACCEL_LSB;
tanahashi 0:c5ccb9d0afba 60 } else if (accel_config == _4G) {
tanahashi 7:da63be95c693 61 accel_LSB = MPU9250_ACCEL_LSB * 2;
tanahashi 0:c5ccb9d0afba 62 } else if (accel_config == _8G) {
tanahashi 7:da63be95c693 63 accel_LSB = MPU9250_ACCEL_LSB * 4;
tanahashi 0:c5ccb9d0afba 64 } else if (accel_config == _16G) {
tanahashi 7:da63be95c693 65 accel_LSB = MPU9250_ACCEL_LSB * 8;
tanahashi 0:c5ccb9d0afba 66 }
tanahashi 0:c5ccb9d0afba 67 }
tanahashi 0:c5ccb9d0afba 68
tanahashi 0:c5ccb9d0afba 69 void MPU9250::set_gyro(GyroConfig_t gyro_config)
tanahashi 0:c5ccb9d0afba 70 {
tanahashi 7:da63be95c693 71 cmd[0] = MPU9250_GYRO_CONFIG;
tanahashi 0:c5ccb9d0afba 72 cmd[1] = gyro_config;
tanahashi 0:c5ccb9d0afba 73 _i2c->write(_addr, cmd, 2);
tanahashi 0:c5ccb9d0afba 74 if (gyro_config == _250DPS) {
tanahashi 7:da63be95c693 75 gyro_LSB = MPU9250_GYRO_LSB;
tanahashi 0:c5ccb9d0afba 76 } else if (gyro_config == _500DPS) {
tanahashi 7:da63be95c693 77 gyro_LSB = MPU9250_GYRO_LSB * 2;
tanahashi 0:c5ccb9d0afba 78 } else if (gyro_config == _1000DPS) {
tanahashi 7:da63be95c693 79 gyro_LSB = MPU9250_GYRO_LSB * 4;
tanahashi 0:c5ccb9d0afba 80 } else if (gyro_config == _2000DPS) {
tanahashi 7:da63be95c693 81 gyro_LSB = MPU9250_GYRO_LSB * 8;
tanahashi 0:c5ccb9d0afba 82 }
tanahashi 0:c5ccb9d0afba 83 }
tanahashi 0:c5ccb9d0afba 84
tanahashi 0:c5ccb9d0afba 85 void MPU9250::set_mag(MagConfig_t mag_config)
tanahashi 0:c5ccb9d0afba 86 {
tanahashi 7:da63be95c693 87 cmd[0] = MPU9250_CNTL1;
tanahashi 0:c5ccb9d0afba 88 cmd[1] = mag_config;
tanahashi 7:da63be95c693 89 _i2c->write(MPU9250_AK8963_ADDR, cmd, 2);
tanahashi 7:da63be95c693 90 mag_LSB = MPU9250_MAG_LSB;
tanahashi 0:c5ccb9d0afba 91 }
tanahashi 0:c5ccb9d0afba 92
tanahashi 0:c5ccb9d0afba 93 void MPU9250::offset(float *accel, float *gyro, float *mag)
tanahashi 0:c5ccb9d0afba 94 {
tanahashi 0:c5ccb9d0afba 95 offset_accel(accel);
tanahashi 0:c5ccb9d0afba 96 offset_gyro(gyro);
tanahashi 0:c5ccb9d0afba 97 offset_mag(mag);
tanahashi 0:c5ccb9d0afba 98 }
tanahashi 0:c5ccb9d0afba 99
tanahashi 0:c5ccb9d0afba 100 void MPU9250::offset_accel(float *accel)
tanahashi 0:c5ccb9d0afba 101 {
tanahashi 0:c5ccb9d0afba 102 for (int i = 0; i < 3; i++) {
tanahashi 0:c5ccb9d0afba 103 accel_offset[i] = accel[i];
tanahashi 0:c5ccb9d0afba 104 }
tanahashi 0:c5ccb9d0afba 105 }
tanahashi 0:c5ccb9d0afba 106
tanahashi 0:c5ccb9d0afba 107 void MPU9250::offset_gyro(float *gyro)
tanahashi 0:c5ccb9d0afba 108 {
tanahashi 0:c5ccb9d0afba 109 for (int i = 0; i < 3; i++) {
tanahashi 0:c5ccb9d0afba 110 gyro_offset[i] = gyro[i];
tanahashi 0:c5ccb9d0afba 111 }
tanahashi 0:c5ccb9d0afba 112 }
tanahashi 0:c5ccb9d0afba 113
tanahashi 0:c5ccb9d0afba 114 void MPU9250::offset_mag(float *mag)
tanahashi 0:c5ccb9d0afba 115 {
tanahashi 0:c5ccb9d0afba 116 for (int i = 0; i < 3; i++) {
tanahashi 0:c5ccb9d0afba 117 mag_offset[i] = mag[i];
tanahashi 0:c5ccb9d0afba 118 }
tanahashi 0:c5ccb9d0afba 119 }
tanahashi 0:c5ccb9d0afba 120
tanahashi 0:c5ccb9d0afba 121 void MPU9250::read(float *accel, float *gyro, float *mag)
tanahashi 0:c5ccb9d0afba 122 {
tanahashi 0:c5ccb9d0afba 123 read_accel(accel);
tanahashi 0:c5ccb9d0afba 124 read_gyro(gyro);
tanahashi 0:c5ccb9d0afba 125 read_mag(mag);
tanahashi 0:c5ccb9d0afba 126 }
tanahashi 0:c5ccb9d0afba 127
tanahashi 0:c5ccb9d0afba 128 void MPU9250::read_accel(float *accel)
tanahashi 0:c5ccb9d0afba 129 {
tanahashi 7:da63be95c693 130 cmd[0] = MPU9250_ACCEL_XOUT_H;
tanahashi 0:c5ccb9d0afba 131 _i2c->write(_addr, cmd, 1);
tanahashi 0:c5ccb9d0afba 132 _i2c->read(_addr, buff, 6);
tanahashi 0:c5ccb9d0afba 133 accel[0] = (short)(buff[0] << 8 | buff[1]) * accel_LSB - accel_offset[0];
tanahashi 0:c5ccb9d0afba 134 accel[1] = (short)(buff[2] << 8 | buff[3]) * accel_LSB - accel_offset[1];
tanahashi 0:c5ccb9d0afba 135 accel[2] = (short)(buff[4] << 8 | buff[5]) * accel_LSB - accel_offset[2];
tanahashi 0:c5ccb9d0afba 136 }
tanahashi 0:c5ccb9d0afba 137
tanahashi 0:c5ccb9d0afba 138 void MPU9250::read_gyro(float *gyro)
tanahashi 0:c5ccb9d0afba 139 {
tanahashi 7:da63be95c693 140 cmd[0] = MPU9250_GYRO_XOUT_H;
tanahashi 0:c5ccb9d0afba 141 _i2c->write(_addr, cmd, 1);
tanahashi 0:c5ccb9d0afba 142 _i2c->read(_addr, buff, 6);
tanahashi 0:c5ccb9d0afba 143 gyro[0] = (short)(buff[0] << 8 | buff[1]) * gyro_LSB - gyro_offset[0];
tanahashi 0:c5ccb9d0afba 144 gyro[1] = (short)(buff[2] << 8 | buff[3]) * gyro_LSB - gyro_offset[1];
tanahashi 0:c5ccb9d0afba 145 gyro[2] = (short)(buff[4] << 8 | buff[5]) * gyro_LSB - gyro_offset[2];
tanahashi 0:c5ccb9d0afba 146 }
tanahashi 0:c5ccb9d0afba 147
tanahashi 0:c5ccb9d0afba 148 void MPU9250::read_mag(float *mag)
tanahashi 0:c5ccb9d0afba 149 {
tanahashi 7:da63be95c693 150 cmd[0] = MPU9250_HXL;
tanahashi 7:da63be95c693 151 _i2c->write(MPU9250_AK8963_ADDR, cmd, 1);
tanahashi 7:da63be95c693 152 _i2c->read(MPU9250_AK8963_ADDR, buff, 7);
tanahashi 0:c5ccb9d0afba 153 mag[0] = (short)(buff[0] | buff[1] << 8) * mag_LSB - mag_offset[0];
tanahashi 0:c5ccb9d0afba 154 mag[1] = (short)(buff[2] | buff[3] << 8) * mag_LSB - mag_offset[1];
tanahashi 0:c5ccb9d0afba 155 mag[2] = (short)(buff[4] | buff[5] << 8) * mag_LSB - mag_offset[2];
tanahashi 0:c5ccb9d0afba 156 }