IZU2020 / PQMPU9250

PLANET-Q MPU9250 Library

Dependents:   IZU2020_AVIONICS IZU2020_AVIONICS

PQMPU9250.cpp@0:c5ccb9d0afba, 2019-11-24 (annotated)

Committer:
tanahashi
Date:
Sun Nov 24 04:30:50 2019 +0000
Revision:
0:c5ccb9d0afba
Child:
7:da63be95c693
test

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 0:c5ccb9d0afba 13 cmd[0] = PWR_MGMT_1;
tanahashi 0:c5ccb9d0afba 14 cmd[1] = 0x00;
tanahashi 0:c5ccb9d0afba 15 _i2c->write(_addr, cmd, 2);
tanahashi 0:c5ccb9d0afba 16 cmd[0] = 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 0:c5ccb9d0afba 22 bool MPU9250::test(){
tanahashi 0:c5ccb9d0afba 23 cmd[0] = WHO_AM_I;
tanahashi 0:c5ccb9d0afba 24 _i2c->write(_addr, cmd, 1);
tanahashi 0:c5ccb9d0afba 25 _i2c->read(_addr, buff, 1);
tanahashi 0:c5ccb9d0afba 26 if (buff[0] == 0x71) {
tanahashi 0:c5ccb9d0afba 27 return true;
tanahashi 0:c5ccb9d0afba 28 } else {
tanahashi 0:c5ccb9d0afba 29 return false;
tanahashi 0:c5ccb9d0afba 30 }
tanahashi 0:c5ccb9d0afba 31 }
tanahashi 0:c5ccb9d0afba 32
tanahashi 0:c5ccb9d0afba 33 bool MPU9250::test_AK8963()
tanahashi 0:c5ccb9d0afba 34 {
tanahashi 0:c5ccb9d0afba 35 cmd[0] = WIA;
tanahashi 0:c5ccb9d0afba 36 _i2c->write(AK8963_ADDR, cmd, 1);
tanahashi 0:c5ccb9d0afba 37 _i2c->read(AK8963_ADDR, buff, 1);
tanahashi 0:c5ccb9d0afba 38 if (buff[0] == 0x48) {
tanahashi 0:c5ccb9d0afba 39 return true;
tanahashi 0:c5ccb9d0afba 40 } else {
tanahashi 0:c5ccb9d0afba 41 return false;
tanahashi 0:c5ccb9d0afba 42 }
tanahashi 0:c5ccb9d0afba 43 }
tanahashi 0:c5ccb9d0afba 44
tanahashi 0:c5ccb9d0afba 45 void MPU9250::set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config){
tanahashi 0:c5ccb9d0afba 46 set_accel(accel_config);
tanahashi 0:c5ccb9d0afba 47 set_gyro(gyro_config);
tanahashi 0:c5ccb9d0afba 48 set_mag(mag_config);
tanahashi 0:c5ccb9d0afba 49 }
tanahashi 0:c5ccb9d0afba 50
tanahashi 0:c5ccb9d0afba 51 void MPU9250::set_accel(AccelConfig_t accel_config)
tanahashi 0:c5ccb9d0afba 52 {
tanahashi 0:c5ccb9d0afba 53 cmd[0] = ACCEL_CONFIG;
tanahashi 0:c5ccb9d0afba 54 cmd[1] = accel_config;
tanahashi 0:c5ccb9d0afba 55 _i2c->write(_addr, cmd, 2);
tanahashi 0:c5ccb9d0afba 56 if (accel_config == _2G) {
tanahashi 0:c5ccb9d0afba 57 accel_LSB = ACCEL_LSB;
tanahashi 0:c5ccb9d0afba 58 } else if (accel_config == _4G) {
tanahashi 0:c5ccb9d0afba 59 accel_LSB = ACCEL_LSB * 2;
tanahashi 0:c5ccb9d0afba 60 } else if (accel_config == _8G) {
tanahashi 0:c5ccb9d0afba 61 accel_LSB = ACCEL_LSB * 4;
tanahashi 0:c5ccb9d0afba 62 } else if (accel_config == _16G) {
tanahashi 0:c5ccb9d0afba 63 accel_LSB = ACCEL_LSB * 8;
tanahashi 0:c5ccb9d0afba 64 }
tanahashi 0:c5ccb9d0afba 65 }
tanahashi 0:c5ccb9d0afba 66
tanahashi 0:c5ccb9d0afba 67 void MPU9250::set_gyro(GyroConfig_t gyro_config)
tanahashi 0:c5ccb9d0afba 68 {
tanahashi 0:c5ccb9d0afba 69 cmd[0] = GYRO_CONFIG;
tanahashi 0:c5ccb9d0afba 70 cmd[1] = gyro_config;
tanahashi 0:c5ccb9d0afba 71 _i2c->write(_addr, cmd, 2);
tanahashi 0:c5ccb9d0afba 72 if (gyro_config == _250DPS) {
tanahashi 0:c5ccb9d0afba 73 gyro_LSB = GYRO_LSB;
tanahashi 0:c5ccb9d0afba 74 } else if (gyro_config == _500DPS) {
tanahashi 0:c5ccb9d0afba 75 gyro_LSB = GYRO_LSB * 2;
tanahashi 0:c5ccb9d0afba 76 } else if (gyro_config == _1000DPS) {
tanahashi 0:c5ccb9d0afba 77 gyro_LSB = GYRO_LSB * 4;
tanahashi 0:c5ccb9d0afba 78 } else if (gyro_config == _2000DPS) {
tanahashi 0:c5ccb9d0afba 79 gyro_LSB = GYRO_LSB * 8;
tanahashi 0:c5ccb9d0afba 80 }
tanahashi 0:c5ccb9d0afba 81 }
tanahashi 0:c5ccb9d0afba 82
tanahashi 0:c5ccb9d0afba 83 void MPU9250::set_mag(MagConfig_t mag_config)
tanahashi 0:c5ccb9d0afba 84 {
tanahashi 0:c5ccb9d0afba 85 cmd[0] = CNTL1;
tanahashi 0:c5ccb9d0afba 86 cmd[1] = mag_config;
tanahashi 0:c5ccb9d0afba 87 _i2c->write(AK8963_ADDR, cmd, 2);
tanahashi 0:c5ccb9d0afba 88 mag_LSB = MAG_LSB;
tanahashi 0:c5ccb9d0afba 89 }
tanahashi 0:c5ccb9d0afba 90
tanahashi 0:c5ccb9d0afba 91 void MPU9250::offset(float *accel, float *gyro, float *mag)
tanahashi 0:c5ccb9d0afba 92 {
tanahashi 0:c5ccb9d0afba 93 offset_accel(accel);
tanahashi 0:c5ccb9d0afba 94 offset_gyro(gyro);
tanahashi 0:c5ccb9d0afba 95 offset_mag(mag);
tanahashi 0:c5ccb9d0afba 96 }
tanahashi 0:c5ccb9d0afba 97
tanahashi 0:c5ccb9d0afba 98 void MPU9250::offset_accel(float *accel)
tanahashi 0:c5ccb9d0afba 99 {
tanahashi 0:c5ccb9d0afba 100 for (int i = 0; i < 3; i++) {
tanahashi 0:c5ccb9d0afba 101 accel_offset[i] = accel[i];
tanahashi 0:c5ccb9d0afba 102 }
tanahashi 0:c5ccb9d0afba 103 }
tanahashi 0:c5ccb9d0afba 104
tanahashi 0:c5ccb9d0afba 105 void MPU9250::offset_gyro(float *gyro)
tanahashi 0:c5ccb9d0afba 106 {
tanahashi 0:c5ccb9d0afba 107 for (int i = 0; i < 3; i++) {
tanahashi 0:c5ccb9d0afba 108 gyro_offset[i] = gyro[i];
tanahashi 0:c5ccb9d0afba 109 }
tanahashi 0:c5ccb9d0afba 110 }
tanahashi 0:c5ccb9d0afba 111
tanahashi 0:c5ccb9d0afba 112 void MPU9250::offset_mag(float *mag)
tanahashi 0:c5ccb9d0afba 113 {
tanahashi 0:c5ccb9d0afba 114 for (int i = 0; i < 3; i++) {
tanahashi 0:c5ccb9d0afba 115 mag_offset[i] = mag[i];
tanahashi 0:c5ccb9d0afba 116 }
tanahashi 0:c5ccb9d0afba 117 }
tanahashi 0:c5ccb9d0afba 118
tanahashi 0:c5ccb9d0afba 119 void MPU9250::read(float *accel, float *gyro, float *mag)
tanahashi 0:c5ccb9d0afba 120 {
tanahashi 0:c5ccb9d0afba 121 read_accel(accel);
tanahashi 0:c5ccb9d0afba 122 read_gyro(gyro);
tanahashi 0:c5ccb9d0afba 123 read_mag(mag);
tanahashi 0:c5ccb9d0afba 124 }
tanahashi 0:c5ccb9d0afba 125
tanahashi 0:c5ccb9d0afba 126 void MPU9250::read_accel(float *accel)
tanahashi 0:c5ccb9d0afba 127 {
tanahashi 0:c5ccb9d0afba 128 cmd[0] = ACCEL_XOUT_H;
tanahashi 0:c5ccb9d0afba 129 _i2c->write(_addr, cmd, 1);
tanahashi 0:c5ccb9d0afba 130 _i2c->read(_addr, buff, 6);
tanahashi 0:c5ccb9d0afba 131 accel[0] = (short)(buff[0] << 8 | buff[1]) * accel_LSB - accel_offset[0];
tanahashi 0:c5ccb9d0afba 132 accel[1] = (short)(buff[2] << 8 | buff[3]) * accel_LSB - accel_offset[1];
tanahashi 0:c5ccb9d0afba 133 accel[2] = (short)(buff[4] << 8 | buff[5]) * accel_LSB - accel_offset[2];
tanahashi 0:c5ccb9d0afba 134 }
tanahashi 0:c5ccb9d0afba 135
tanahashi 0:c5ccb9d0afba 136 void MPU9250::read_gyro(float *gyro)
tanahashi 0:c5ccb9d0afba 137 {
tanahashi 0:c5ccb9d0afba 138 cmd[0] = GYRO_XOUT_H;
tanahashi 0:c5ccb9d0afba 139 _i2c->write(_addr, cmd, 1);
tanahashi 0:c5ccb9d0afba 140 _i2c->read(_addr, buff, 6);
tanahashi 0:c5ccb9d0afba 141 gyro[0] = (short)(buff[0] << 8 | buff[1]) * gyro_LSB - gyro_offset[0];
tanahashi 0:c5ccb9d0afba 142 gyro[1] = (short)(buff[2] << 8 | buff[3]) * gyro_LSB - gyro_offset[1];
tanahashi 0:c5ccb9d0afba 143 gyro[2] = (short)(buff[4] << 8 | buff[5]) * gyro_LSB - gyro_offset[2];
tanahashi 0:c5ccb9d0afba 144 }
tanahashi 0:c5ccb9d0afba 145
tanahashi 0:c5ccb9d0afba 146 void MPU9250::read_mag(float *mag)
tanahashi 0:c5ccb9d0afba 147 {
tanahashi 0:c5ccb9d0afba 148 cmd[0] = HXL;
tanahashi 0:c5ccb9d0afba 149 _i2c->write(AK8963_ADDR, cmd, 1);
tanahashi 0:c5ccb9d0afba 150 _i2c->read(AK8963_ADDR, buff, 7);
tanahashi 0:c5ccb9d0afba 151 mag[0] = (short)(buff[0] | buff[1] << 8) * mag_LSB - mag_offset[0];
tanahashi 0:c5ccb9d0afba 152 mag[1] = (short)(buff[2] | buff[3] << 8) * mag_LSB - mag_offset[1];
tanahashi 0:c5ccb9d0afba 153 mag[2] = (short)(buff[4] | buff[5] << 8) * mag_LSB - mag_offset[2];
tanahashi 0:c5ccb9d0afba 154 }