anyThing Connected Team / MPU9250

MPU library edited by kevin

Dependents:   Sensor_tap_BLE

MPU9250.h@0:264ee5acfe00, 2018-03-02 (annotated)

Committer:
MarijnJ
Date:
Fri Mar 02 10:29:51 2018 +0000
Revision:
0:264ee5acfe00
bla

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarijnJ 0:264ee5acfe00 1 #ifndef MPU9250_H
MarijnJ 0:264ee5acfe00 2 #define MPU9250_H
MarijnJ 0:264ee5acfe00 3
MarijnJ 0:264ee5acfe00 4 #include "mbed.h"
MarijnJ 0:264ee5acfe00 5 #include "math.h"
MarijnJ 0:264ee5acfe00 6 #include "MPU9250-common.h"
MarijnJ 0:264ee5acfe00 7
MarijnJ 0:264ee5acfe00 8
MarijnJ 0:264ee5acfe00 9 // Set initial input parameters
MarijnJ 0:264ee5acfe00 10 enum Ascale {
MarijnJ 0:264ee5acfe00 11 AFS_2G = 0,
MarijnJ 0:264ee5acfe00 12 AFS_4G = 1,
MarijnJ 0:264ee5acfe00 13 AFS_8G = 2,
MarijnJ 0:264ee5acfe00 14 AFS_16G = 3
MarijnJ 0:264ee5acfe00 15 };
MarijnJ 0:264ee5acfe00 16
MarijnJ 0:264ee5acfe00 17 enum Gscale {
MarijnJ 0:264ee5acfe00 18 GFS_250DPS = 0,
MarijnJ 0:264ee5acfe00 19 GFS_500DPS = 1,
MarijnJ 0:264ee5acfe00 20 GFS_1000DPS = 2,
MarijnJ 0:264ee5acfe00 21 GFS_2000DPS = 3
MarijnJ 0:264ee5acfe00 22 };
MarijnJ 0:264ee5acfe00 23
MarijnJ 0:264ee5acfe00 24 enum Mscale {
MarijnJ 0:264ee5acfe00 25 MFS_14BITS = 0, // 0.6 mG per LSB
MarijnJ 0:264ee5acfe00 26 MFS_16BITS // 0.15 mG per LSB
MarijnJ 0:264ee5acfe00 27 };
MarijnJ 0:264ee5acfe00 28
MarijnJ 0:264ee5acfe00 29
MarijnJ 0:264ee5acfe00 30
MarijnJ 0:264ee5acfe00 31 class MPU9250 {
MarijnJ 0:264ee5acfe00 32 public:
MarijnJ 0:264ee5acfe00 33
MarijnJ 0:264ee5acfe00 34 // The sufficientMeasurements boolean is used to indicate that the sensor has
MarijnJ 0:264ee5acfe00 35 // gathered enough measurements to do future sensor measurements reliably.
MarijnJ 0:264ee5acfe00 36 // After roughly 10 seconds of full speed measuring, it should be accurate
MarijnJ 0:264ee5acfe00 37 // enough (with the filter updates), so this can then be set to true.
MarijnJ 0:264ee5acfe00 38 bool sufficientMeasurements;
MarijnJ 0:264ee5acfe00 39
MarijnJ 0:264ee5acfe00 40 I2C *i2c;
MarijnJ 0:264ee5acfe00 41 float beta, zeta;
MarijnJ 0:264ee5acfe00 42 float deltat; // integration interval for both filter schemes
MarijnJ 0:264ee5acfe00 43 float q[4]; // vector to hold quaternion
MarijnJ 0:264ee5acfe00 44 float eInt[3]; // vector to hold integral error for Mahony method
MarijnJ 0:264ee5acfe00 45
MarijnJ 0:264ee5acfe00 46 const uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
MarijnJ 0:264ee5acfe00 47 const uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
MarijnJ 0:264ee5acfe00 48 const uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
MarijnJ 0:264ee5acfe00 49 const uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
MarijnJ 0:264ee5acfe00 50
MarijnJ 0:264ee5acfe00 51 float aRes, gRes, mRes; // Resolution scales for accelerometer, gyro, magnetometer
MarijnJ 0:264ee5acfe00 52
MarijnJ 0:264ee5acfe00 53 float accelCalibration[3];
MarijnJ 0:264ee5acfe00 54 float accelBias[3];
MarijnJ 0:264ee5acfe00 55
MarijnJ 0:264ee5acfe00 56 float gyroCalibration[3];
MarijnJ 0:264ee5acfe00 57 float gyroBias[3];
MarijnJ 0:264ee5acfe00 58
MarijnJ 0:264ee5acfe00 59 float magCalibration[3];
MarijnJ 0:264ee5acfe00 60 float magBias[3];
MarijnJ 0:264ee5acfe00 61
MarijnJ 0:264ee5acfe00 62
MarijnJ 0:264ee5acfe00 63 MPU9250(I2C *i2cConnection) : i2c(i2cConnection), Ascale(AFS_8G),
MarijnJ 0:264ee5acfe00 64 Gscale(GFS_1000DPS), Mscale(MFS_16BITS), Mmode(0x06) {
MarijnJ 0:264ee5acfe00 65 // Gyroscope measurement error in rads/s (start at 60 deg/s),
MarijnJ 0:264ee5acfe00 66 // then reduce after ~10 s to 3
MarijnJ 0:264ee5acfe00 67 float GyroMeasError = PI * (60.0f / 180.0f);
MarijnJ 0:264ee5acfe00 68 beta = sqrt(3.0f / 4.0f) * GyroMeasError;
MarijnJ 0:264ee5acfe00 69
MarijnJ 0:264ee5acfe00 70 // Gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
MarijnJ 0:264ee5acfe00 71 float GyroMeasDrift = PI * (1.0f / 180.0f);
MarijnJ 0:264ee5acfe00 72
MarijnJ 0:264ee5acfe00 73 // Other free parameter zeta in the Madgwick scheme usually set to a small or zero value
MarijnJ 0:264ee5acfe00 74 zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
MarijnJ 0:264ee5acfe00 75
MarijnJ 0:264ee5acfe00 76 // Integration interval for both filter schemes
MarijnJ 0:264ee5acfe00 77 deltat = 0.0f;
MarijnJ 0:264ee5acfe00 78
MarijnJ 0:264ee5acfe00 79 q[0] = 1.0f;
MarijnJ 0:264ee5acfe00 80 q[1] = q[2] = q[3] = 0.0f;
MarijnJ 0:264ee5acfe00 81
MarijnJ 0:264ee5acfe00 82 eInt[0] = eInt[1] = eInt[2] = 0.0f;
MarijnJ 0:264ee5acfe00 83
MarijnJ 0:264ee5acfe00 84 // Get resolution scales
MarijnJ 0:264ee5acfe00 85 aRes = getAres();
MarijnJ 0:264ee5acfe00 86 gRes = getGres();
MarijnJ 0:264ee5acfe00 87 mRes = getMres();
MarijnJ 0:264ee5acfe00 88
MarijnJ 0:264ee5acfe00 89 // Set initial calibration stuff all to 0.0f
MarijnJ 0:264ee5acfe00 90 accelCalibration[0] = accelCalibration[1] = accelCalibration[2] = 0.0f;
MarijnJ 0:264ee5acfe00 91 accelBias[0] = accelBias[1] = accelBias[2] = 0.0f;
MarijnJ 0:264ee5acfe00 92 gyroCalibration[0] = gyroCalibration[1] = gyroCalibration[2] = 0.0f;
MarijnJ 0:264ee5acfe00 93 gyroBias[0] = gyroBias[1] = gyroBias[2] = 0.0f;
MarijnJ 0:264ee5acfe00 94 magCalibration[0] = magCalibration[1] = magCalibration[2] = 0.0f;
MarijnJ 0:264ee5acfe00 95
MarijnJ 0:264ee5acfe00 96 // User environmental xyz-axes correction in milliGauss, should be automatically calculated
MarijnJ 0:264ee5acfe00 97 magBias[0] = 470.0f;
MarijnJ 0:264ee5acfe00 98 magBias[1] = 120.0f;
MarijnJ 0:264ee5acfe00 99 magBias[2] = 125.0f;
MarijnJ 0:264ee5acfe00 100
MarijnJ 0:264ee5acfe00 101 // We haven't done enough measurements to get accurate values yet!
MarijnJ 0:264ee5acfe00 102 sufficientMeasurements = false;
MarijnJ 0:264ee5acfe00 103 }
MarijnJ 0:264ee5acfe00 104
MarijnJ 0:264ee5acfe00 105 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
MarijnJ 0:264ee5acfe00 106 char data_write[2];
MarijnJ 0:264ee5acfe00 107 data_write[0] = subAddress;
MarijnJ 0:264ee5acfe00 108 data_write[1] = data;
MarijnJ 0:264ee5acfe00 109 i2c->write(address, data_write, 2, 0);
MarijnJ 0:264ee5acfe00 110 }
MarijnJ 0:264ee5acfe00 111
MarijnJ 0:264ee5acfe00 112 char readByte(uint8_t address, uint8_t subAddress) {
MarijnJ 0:264ee5acfe00 113 // `data` will store the register data
MarijnJ 0:264ee5acfe00 114 char data[1];
MarijnJ 0:264ee5acfe00 115 char data_write[1];
MarijnJ 0:264ee5acfe00 116 data_write[0] = subAddress;
MarijnJ 0:264ee5acfe00 117 i2c->write(address, data_write, 1, 1); // no stop
MarijnJ 0:264ee5acfe00 118 i2c->read(address, data, 1, 0);
MarijnJ 0:264ee5acfe00 119 return data[0];
MarijnJ 0:264ee5acfe00 120 }
MarijnJ 0:264ee5acfe00 121
MarijnJ 0:264ee5acfe00 122 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest) {
MarijnJ 0:264ee5acfe00 123 char data[14];
MarijnJ 0:264ee5acfe00 124 char data_write[1];
MarijnJ 0:264ee5acfe00 125 data_write[0] = subAddress;
MarijnJ 0:264ee5acfe00 126 i2c->write(address, data_write, 1, 1); // no stop
MarijnJ 0:264ee5acfe00 127 i2c->read(address, data, count, 0);
MarijnJ 0:264ee5acfe00 128
MarijnJ 0:264ee5acfe00 129 for (int ii = 0; ii < count; ii++) {
MarijnJ 0:264ee5acfe00 130 dest[ii] = data[ii];
MarijnJ 0:264ee5acfe00 131 }
MarijnJ 0:264ee5acfe00 132 }
MarijnJ 0:264ee5acfe00 133
MarijnJ 0:264ee5acfe00 134 float getMres() {
MarijnJ 0:264ee5acfe00 135 switch (Mscale) {
MarijnJ 0:264ee5acfe00 136 // Possible magnetometer scales (and their register bit settings) are:
MarijnJ 0:264ee5acfe00 137 // 14 bit resolution (0) and 16 bit resolution (1)
MarijnJ 0:264ee5acfe00 138 case MFS_14BITS:
MarijnJ 0:264ee5acfe00 139 return 10.0 * 4219.0 / 8190.0; // Proper scale to return milliGauss
MarijnJ 0:264ee5acfe00 140 case MFS_16BITS:
MarijnJ 0:264ee5acfe00 141 return 10.0 * 4219.0 / 32760.0; // Proper scale to return milliGauss
MarijnJ 0:264ee5acfe00 142 }
MarijnJ 0:264ee5acfe00 143
MarijnJ 0:264ee5acfe00 144 return -1.0f;
MarijnJ 0:264ee5acfe00 145 }
MarijnJ 0:264ee5acfe00 146
MarijnJ 0:264ee5acfe00 147
MarijnJ 0:264ee5acfe00 148 float getGres() {
MarijnJ 0:264ee5acfe00 149 switch (Gscale) {
MarijnJ 0:264ee5acfe00 150 // Possible gyro scales (and their register bit settings) are:
MarijnJ 0:264ee5acfe00 151 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
MarijnJ 0:264ee5acfe00 152 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
MarijnJ 0:264ee5acfe00 153 case GFS_250DPS:
MarijnJ 0:264ee5acfe00 154 return 250.0/32768.0;
MarijnJ 0:264ee5acfe00 155 case GFS_500DPS:
MarijnJ 0:264ee5acfe00 156 return 500.0/32768.0;
MarijnJ 0:264ee5acfe00 157 case GFS_1000DPS:
MarijnJ 0:264ee5acfe00 158 return 1000.0/32768.0;
MarijnJ 0:264ee5acfe00 159 case GFS_2000DPS:
MarijnJ 0:264ee5acfe00 160 return 2000.0/32768.0;
MarijnJ 0:264ee5acfe00 161 }
MarijnJ 0:264ee5acfe00 162
MarijnJ 0:264ee5acfe00 163 return -1.0f;
MarijnJ 0:264ee5acfe00 164 }
MarijnJ 0:264ee5acfe00 165
MarijnJ 0:264ee5acfe00 166
MarijnJ 0:264ee5acfe00 167 float getAres() {
MarijnJ 0:264ee5acfe00 168 switch (Ascale) {
MarijnJ 0:264ee5acfe00 169 // Possible accelerometer scales (and their register bit settings) are:
MarijnJ 0:264ee5acfe00 170 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
MarijnJ 0:264ee5acfe00 171 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
MarijnJ 0:264ee5acfe00 172 case AFS_2G:
MarijnJ 0:264ee5acfe00 173 return 2.0 / 32768.0;
MarijnJ 0:264ee5acfe00 174 case AFS_4G:
MarijnJ 0:264ee5acfe00 175 return 4.0 / 32768.0;
MarijnJ 0:264ee5acfe00 176 case AFS_8G:
MarijnJ 0:264ee5acfe00 177 return 8.0 / 32768.0;
MarijnJ 0:264ee5acfe00 178 case AFS_16G:
MarijnJ 0:264ee5acfe00 179 return 16.0 / 32768.0;
MarijnJ 0:264ee5acfe00 180 }
MarijnJ 0:264ee5acfe00 181
MarijnJ 0:264ee5acfe00 182 return -1.0f;
MarijnJ 0:264ee5acfe00 183 }
MarijnJ 0:264ee5acfe00 184
MarijnJ 0:264ee5acfe00 185 uint8_t hasNewData() {
MarijnJ 0:264ee5acfe00 186 return readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01;
MarijnJ 0:264ee5acfe00 187 }
MarijnJ 0:264ee5acfe00 188
MarijnJ 0:264ee5acfe00 189 uint8_t getWhoAmI() {
MarijnJ 0:264ee5acfe00 190 return readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
MarijnJ 0:264ee5acfe00 191 }
MarijnJ 0:264ee5acfe00 192
MarijnJ 0:264ee5acfe00 193
MarijnJ 0:264ee5acfe00 194 void readAccelData(float *ax, float *ay, float *az) {
MarijnJ 0:264ee5acfe00 195 // x/y/z accel register data stored here
MarijnJ 0:264ee5acfe00 196 uint8_t rawData[6];
MarijnJ 0:264ee5acfe00 197
MarijnJ 0:264ee5acfe00 198 // Read the six raw data registers into data array
MarijnJ 0:264ee5acfe00 199 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
MarijnJ 0:264ee5acfe00 200
MarijnJ 0:264ee5acfe00 201 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 202 int16_t axTemp = (int16_t) (((int16_t) rawData[0] << 8) | rawData[1]);
MarijnJ 0:264ee5acfe00 203 int16_t ayTemp = (int16_t) (((int16_t) rawData[2] << 8) | rawData[3]);
MarijnJ 0:264ee5acfe00 204 int16_t azTemp = (int16_t) (((int16_t) rawData[4] << 8) | rawData[5]);
MarijnJ 0:264ee5acfe00 205
MarijnJ 0:264ee5acfe00 206 // "Return" ax, ay and az in actual g's, depending on resolution
MarijnJ 0:264ee5acfe00 207 *ax = (float) axTemp * aRes - accelBias[0];
MarijnJ 0:264ee5acfe00 208 *ay = (float) ayTemp * aRes - accelBias[1];
MarijnJ 0:264ee5acfe00 209 *az = (float) azTemp * aRes - accelBias[2];
MarijnJ 0:264ee5acfe00 210 }
MarijnJ 0:264ee5acfe00 211
MarijnJ 0:264ee5acfe00 212 void readGyroData(float *gx, float *gy, float *gz) {
MarijnJ 0:264ee5acfe00 213 // x/y/z gyro register data stored here
MarijnJ 0:264ee5acfe00 214 uint8_t rawData[6];
MarijnJ 0:264ee5acfe00 215
MarijnJ 0:264ee5acfe00 216 // Read the six raw data registers sequentially into data array
MarijnJ 0:264ee5acfe00 217 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
MarijnJ 0:264ee5acfe00 218
MarijnJ 0:264ee5acfe00 219 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 220 int16_t gxTemp = (int16_t) (((int16_t) rawData[0] << 8) | rawData[1]);
MarijnJ 0:264ee5acfe00 221 int16_t gyTemp = (int16_t) (((int16_t) rawData[2] << 8) | rawData[3]);
MarijnJ 0:264ee5acfe00 222 int16_t gzTemp = (int16_t) (((int16_t) rawData[4] << 8) | rawData[5]);
MarijnJ 0:264ee5acfe00 223
MarijnJ 0:264ee5acfe00 224 // "Return" gx, gy and gz in actual deg/s, depending on scale
MarijnJ 0:264ee5acfe00 225 *gx = (float) gxTemp * gRes - gyroBias[0];
MarijnJ 0:264ee5acfe00 226 *gy = (float) gyTemp * gRes - gyroBias[1];
MarijnJ 0:264ee5acfe00 227 *gz = (float) gzTemp * gRes - gyroBias[2];
MarijnJ 0:264ee5acfe00 228 }
MarijnJ 0:264ee5acfe00 229
MarijnJ 0:264ee5acfe00 230 void readMagData(float *mx, float *my, float *mz) {
MarijnJ 0:264ee5acfe00 231 // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
MarijnJ 0:264ee5acfe00 232 uint8_t rawData[7];
MarijnJ 0:264ee5acfe00 233
MarijnJ 0:264ee5acfe00 234 // Wait for magnetometer data ready bit to be set
MarijnJ 0:264ee5acfe00 235 if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) {
MarijnJ 0:264ee5acfe00 236 // Read the six raw data and ST2 registers sequentially into data array
MarijnJ 0:264ee5acfe00 237 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);
MarijnJ 0:264ee5acfe00 238 uint8_t c = rawData[6]; // End data read by reading ST2 register
MarijnJ 0:264ee5acfe00 239
MarijnJ 0:264ee5acfe00 240 // Check if magnetic sensor overflow set, if not then report data
MarijnJ 0:264ee5acfe00 241 if (!(c & 0x08)) {
MarijnJ 0:264ee5acfe00 242 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 243 // Data stored as little Endian
MarijnJ 0:264ee5acfe00 244 int16_t mxTemp = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]);
MarijnJ 0:264ee5acfe00 245 int16_t myTemp = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
MarijnJ 0:264ee5acfe00 246 int16_t mzTemp = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
MarijnJ 0:264ee5acfe00 247
MarijnJ 0:264ee5acfe00 248 // Calculate the magnetometer values in milliGauss. Include factory
MarijnJ 0:264ee5acfe00 249 // calibration per data sheet and user environmental corrections
MarijnJ 0:264ee5acfe00 250 // "Return" gx, gy and gz in actual deg/s, depending on scale
MarijnJ 0:264ee5acfe00 251 *mx = (float) mxTemp * mRes * magCalibration[0] - magBias[0];
MarijnJ 0:264ee5acfe00 252 *my = (float) myTemp * mRes * magCalibration[1] - magBias[1];
MarijnJ 0:264ee5acfe00 253 *mz = (float) mzTemp * mRes * magCalibration[2] - magBias[2];
MarijnJ 0:264ee5acfe00 254 }
MarijnJ 0:264ee5acfe00 255 }
MarijnJ 0:264ee5acfe00 256 }
MarijnJ 0:264ee5acfe00 257
MarijnJ 0:264ee5acfe00 258 int16_t readTempData() {
MarijnJ 0:264ee5acfe00 259 // x/y/z gyro register data stored here
MarijnJ 0:264ee5acfe00 260 uint8_t rawData[2];
MarijnJ 0:264ee5acfe00 261
MarijnJ 0:264ee5acfe00 262 // Read the two raw data registers sequentially into data array
MarijnJ 0:264ee5acfe00 263 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);
MarijnJ 0:264ee5acfe00 264
MarijnJ 0:264ee5acfe00 265 // Turn the MSB and LSB into a 16-bit value
MarijnJ 0:264ee5acfe00 266 return (int16_t) (((int16_t) rawData[0]) << 8 | rawData[1]);
MarijnJ 0:264ee5acfe00 267 }
MarijnJ 0:264ee5acfe00 268
MarijnJ 0:264ee5acfe00 269 float getTemperature() {
MarijnJ 0:264ee5acfe00 270 int16_t tempData = readTempData();
MarijnJ 0:264ee5acfe00 271 return ((float) tempData) / 333.87f + 21.0f; // In Celsius
MarijnJ 0:264ee5acfe00 272 }
MarijnJ 0:264ee5acfe00 273
MarijnJ 0:264ee5acfe00 274
MarijnJ 0:264ee5acfe00 275 void getYawPitchRoll(float *yaw, float *pitch, float *roll, float declination) {
MarijnJ 0:264ee5acfe00 276 // Define output variables from updated quaternion---these are
MarijnJ 0:264ee5acfe00 277 // Tait-Bryan angles, commonly used in aircraft orientation. In
MarijnJ 0:264ee5acfe00 278 // this coordinate system, the positive z-axis is down toward Earth.
MarijnJ 0:264ee5acfe00 279 // Yaw is the angle between Sensor x-axis and Earth magnetic North
MarijnJ 0:264ee5acfe00 280 // (or true North if corrected for local declination, looking down
MarijnJ 0:264ee5acfe00 281 // on the sensor positive yaw is counterclockwise.
MarijnJ 0:264ee5acfe00 282 // Pitch is angle between sensor x-axis and Earth ground plane,
MarijnJ 0:264ee5acfe00 283 // toward the Earth is positive, up toward the sky is negative.
MarijnJ 0:264ee5acfe00 284 // Roll is angle between sensor y-axis and Earth ground plane,
MarijnJ 0:264ee5acfe00 285 // y-axis up is positive roll. These arise from the definition of
MarijnJ 0:264ee5acfe00 286 // the homogeneous rotation matrix constructed from quaternions.
MarijnJ 0:264ee5acfe00 287 // Tait-Bryan angles as well as Euler angles are non-commutative;
MarijnJ 0:264ee5acfe00 288 // that is, the get the correct orientation the rotations must be
MarijnJ 0:264ee5acfe00 289 // applied in the correct order which for this configuration is
MarijnJ 0:264ee5acfe00 290 // yaw, pitch, and then roll.
MarijnJ 0:264ee5acfe00 291 // For more see
MarijnJ 0:264ee5acfe00 292 // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
MarijnJ 0:264ee5acfe00 293 // which has additional links.
MarijnJ 0:264ee5acfe00 294 *yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]),
MarijnJ 0:264ee5acfe00 295 q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
MarijnJ 0:264ee5acfe00 296 *pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
MarijnJ 0:264ee5acfe00 297 *roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
MarijnJ 0:264ee5acfe00 298 q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
MarijnJ 0:264ee5acfe00 299
MarijnJ 0:264ee5acfe00 300 // Quaternion q is in deg, so convert it to radian
MarijnJ 0:264ee5acfe00 301 *pitch = RAD2DEG(*pitch);
MarijnJ 0:264ee5acfe00 302 *yaw = RAD2DEG(*yaw)- declination;
MarijnJ 0:264ee5acfe00 303 *roll = RAD2DEG(*roll);
MarijnJ 0:264ee5acfe00 304 }
MarijnJ 0:264ee5acfe00 305
MarijnJ 0:264ee5acfe00 306
MarijnJ 0:264ee5acfe00 307 void resetMPU9250() {
MarijnJ 0:264ee5acfe00 308 // Write a one to bit 7 reset bit; toggle reset device
MarijnJ 0:264ee5acfe00 309 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
MarijnJ 0:264ee5acfe00 310 wait(0.1);
MarijnJ 0:264ee5acfe00 311 }
MarijnJ 0:264ee5acfe00 312
MarijnJ 0:264ee5acfe00 313 void initAK8963() {
MarijnJ 0:264ee5acfe00 314 // First extract the factory calibration for each magnetometer axis
MarijnJ 0:264ee5acfe00 315 // x/y/z gyro calibration data stored here
MarijnJ 0:264ee5acfe00 316 uint8_t rawData[3];
MarijnJ 0:264ee5acfe00 317
MarijnJ 0:264ee5acfe00 318 // Power down magnetometer
MarijnJ 0:264ee5acfe00 319 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
MarijnJ 0:264ee5acfe00 320 wait(0.01);
MarijnJ 0:264ee5acfe00 321
MarijnJ 0:264ee5acfe00 322 // Enter Fuse ROM access mode
MarijnJ 0:264ee5acfe00 323 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F);
MarijnJ 0:264ee5acfe00 324 wait(0.01);
MarijnJ 0:264ee5acfe00 325
MarijnJ 0:264ee5acfe00 326 // Read the x-, y-, and z-axis calibration values
MarijnJ 0:264ee5acfe00 327 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);
MarijnJ 0:264ee5acfe00 328
MarijnJ 0:264ee5acfe00 329 // Return x-axis sensitivity adjustment values, etc.
MarijnJ 0:264ee5acfe00 330 magCalibration[0] = (float)(rawData[0] - 128) / 256.0f + 1.0f;
MarijnJ 0:264ee5acfe00 331 magCalibration[1] = (float)(rawData[1] - 128) / 256.0f + 1.0f;
MarijnJ 0:264ee5acfe00 332 magCalibration[2] = (float)(rawData[2] - 128) / 256.0f + 1.0f;
MarijnJ 0:264ee5acfe00 333
MarijnJ 0:264ee5acfe00 334 // Power down magnetometer
MarijnJ 0:264ee5acfe00 335 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
MarijnJ 0:264ee5acfe00 336 wait(0.01);
MarijnJ 0:264ee5acfe00 337
MarijnJ 0:264ee5acfe00 338 // Configure the magnetometer for continuous read and highest resolution
MarijnJ 0:264ee5acfe00 339 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
MarijnJ 0:264ee5acfe00 340 // and enable continuous mode data acquisition Mmode (bits [3:0]),
MarijnJ 0:264ee5acfe00 341 // 0010 for 8 Hz and 0110 for 100 Hz sample rates
MarijnJ 0:264ee5acfe00 342 // Set magnetometer data resolution and sample ODR
MarijnJ 0:264ee5acfe00 343 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode);
MarijnJ 0:264ee5acfe00 344 wait(0.01);
MarijnJ 0:264ee5acfe00 345 }
MarijnJ 0:264ee5acfe00 346
MarijnJ 0:264ee5acfe00 347
MarijnJ 0:264ee5acfe00 348 void initMPU9250() {
MarijnJ 0:264ee5acfe00 349 // Initialize MPU9250 device
MarijnJ 0:264ee5acfe00 350 // Wake up device
MarijnJ 0:264ee5acfe00 351 // Clear sleep mode bit (6), enable all sensors
MarijnJ 0:264ee5acfe00 352 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
MarijnJ 0:264ee5acfe00 353
MarijnJ 0:264ee5acfe00 354 // Delay 100 ms for PLL to get established on x-axis gyro
MarijnJ 0:264ee5acfe00 355 // Should check for PLL ready interrupt
MarijnJ 0:264ee5acfe00 356 wait(0.1);
MarijnJ 0:264ee5acfe00 357
MarijnJ 0:264ee5acfe00 358 // Get stable time source
MarijnJ 0:264ee5acfe00 359 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
MarijnJ 0:264ee5acfe00 360 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
MarijnJ 0:264ee5acfe00 361
MarijnJ 0:264ee5acfe00 362 // Configure Gyro and Accelerometer
MarijnJ 0:264ee5acfe00 363 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
MarijnJ 0:264ee5acfe00 364 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
MarijnJ 0:264ee5acfe00 365 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
MarijnJ 0:264ee5acfe00 366 writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
MarijnJ 0:264ee5acfe00 367
MarijnJ 0:264ee5acfe00 368 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
MarijnJ 0:264ee5acfe00 369 // Use a 200 Hz rate; the same rate set in CONFIG above
MarijnJ 0:264ee5acfe00 370 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);
MarijnJ 0:264ee5acfe00 371
MarijnJ 0:264ee5acfe00 372 // Set gyroscope full scale range
MarijnJ 0:264ee5acfe00 373 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are
MarijnJ 0:264ee5acfe00 374 // left-shifted into positions 4:3
MarijnJ 0:264ee5acfe00 375 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
MarijnJ 0:264ee5acfe00 376
MarijnJ 0:264ee5acfe00 377 // Clear self-test bits [7:5]
MarijnJ 0:264ee5acfe00 378 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0);
MarijnJ 0:264ee5acfe00 379
MarijnJ 0:264ee5acfe00 380 // Clear AFS bits [4:3]
MarijnJ 0:264ee5acfe00 381 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18);
MarijnJ 0:264ee5acfe00 382
MarijnJ 0:264ee5acfe00 383 // Set full scale range for the gyro
MarijnJ 0:264ee5acfe00 384 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3);
MarijnJ 0:264ee5acfe00 385
MarijnJ 0:264ee5acfe00 386 // Set accelerometer configuration
MarijnJ 0:264ee5acfe00 387 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
MarijnJ 0:264ee5acfe00 388
MarijnJ 0:264ee5acfe00 389 // Clear self-test bits [7:5]
MarijnJ 0:264ee5acfe00 390 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0);
MarijnJ 0:264ee5acfe00 391
MarijnJ 0:264ee5acfe00 392 // Clear AFS bits [4:3]
MarijnJ 0:264ee5acfe00 393 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18);
MarijnJ 0:264ee5acfe00 394
MarijnJ 0:264ee5acfe00 395 // Set full scale range for the accelerometer
MarijnJ 0:264ee5acfe00 396 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3);
MarijnJ 0:264ee5acfe00 397
MarijnJ 0:264ee5acfe00 398 // Set accelerometer sample rate configuration
MarijnJ 0:264ee5acfe00 399 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing
MarijnJ 0:264ee5acfe00 400 // 1 for accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
MarijnJ 0:264ee5acfe00 401 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
MarijnJ 0:264ee5acfe00 402
MarijnJ 0:264ee5acfe00 403 // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
MarijnJ 0:264ee5acfe00 404 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F);
MarijnJ 0:264ee5acfe00 405
MarijnJ 0:264ee5acfe00 406 // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
MarijnJ 0:264ee5acfe00 407 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03);
MarijnJ 0:264ee5acfe00 408
MarijnJ 0:264ee5acfe00 409 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
MarijnJ 0:264ee5acfe00 410 // but all these rates are further reduced by a factor of 5 to 200 Hz
MarijnJ 0:264ee5acfe00 411 // because of the SMPLRT_DIV setting
MarijnJ 0:264ee5acfe00 412
MarijnJ 0:264ee5acfe00 413 // Configure Interrupts and Bypass Enable
MarijnJ 0:264ee5acfe00 414 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS,
MarijnJ 0:264ee5acfe00 415 // enable I2C_BYPASS_EN so additional chips can join the I2C bus and all
MarijnJ 0:264ee5acfe00 416 // can be controlled by the Arduino as master
MarijnJ 0:264ee5acfe00 417 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
MarijnJ 0:264ee5acfe00 418
MarijnJ 0:264ee5acfe00 419 // Enable data ready (bit 0) interrupt
MarijnJ 0:264ee5acfe00 420 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);
MarijnJ 0:264ee5acfe00 421 }
MarijnJ 0:264ee5acfe00 422
MarijnJ 0:264ee5acfe00 423 // Function which accumulates gyro and accelerometer data after device
MarijnJ 0:264ee5acfe00 424 // initialization. It calculates the average of the at-rest readings and then
MarijnJ 0:264ee5acfe00 425 // loads the resulting offsets into accelerometer and gyro bias registers.
MarijnJ 0:264ee5acfe00 426 void calibrateMPU9250() {
MarijnJ 0:264ee5acfe00 427 // Data array to hold accelerometer and gyro x, y, z, data
MarijnJ 0:264ee5acfe00 428 uint8_t data[12];
MarijnJ 0:264ee5acfe00 429 uint16_t ii, packet_count, fifo_count;
MarijnJ 0:264ee5acfe00 430 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
MarijnJ 0:264ee5acfe00 431
MarijnJ 0:264ee5acfe00 432 // Reset device, reset all registers, clear gyro and accelerometer bias registers
MarijnJ 0:264ee5acfe00 433 // Write a one to bit 7 reset bit; toggle reset device
MarijnJ 0:264ee5acfe00 434 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
MarijnJ 0:264ee5acfe00 435 wait(0.1);
MarijnJ 0:264ee5acfe00 436
MarijnJ 0:264ee5acfe00 437 // Get stable time source
MarijnJ 0:264ee5acfe00 438 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
MarijnJ 0:264ee5acfe00 439 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
MarijnJ 0:264ee5acfe00 440 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
MarijnJ 0:264ee5acfe00 441 wait(0.2);
MarijnJ 0:264ee5acfe00 442
MarijnJ 0:264ee5acfe00 443 // Configure device for bias calculation
MarijnJ 0:264ee5acfe00 444 // Disable all interrupts
MarijnJ 0:264ee5acfe00 445 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);
MarijnJ 0:264ee5acfe00 446
MarijnJ 0:264ee5acfe00 447 // Disable FIFO
MarijnJ 0:264ee5acfe00 448 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);
MarijnJ 0:264ee5acfe00 449
MarijnJ 0:264ee5acfe00 450 // Turn on internal clock source
MarijnJ 0:264ee5acfe00 451 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
MarijnJ 0:264ee5acfe00 452
MarijnJ 0:264ee5acfe00 453 // Disable I2C master
MarijnJ 0:264ee5acfe00 454 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00);
MarijnJ 0:264ee5acfe00 455
MarijnJ 0:264ee5acfe00 456 // Disable FIFO and I2C master modes
MarijnJ 0:264ee5acfe00 457 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);
MarijnJ 0:264ee5acfe00 458
MarijnJ 0:264ee5acfe00 459 // Reset FIFO and DMP
MarijnJ 0:264ee5acfe00 460 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);
MarijnJ 0:264ee5acfe00 461 wait(0.015);
MarijnJ 0:264ee5acfe00 462
MarijnJ 0:264ee5acfe00 463 // Configure MPU9250 gyro and accelerometer for bias calculation
MarijnJ 0:264ee5acfe00 464 // Set low-pass filter to 188 Hz
MarijnJ 0:264ee5acfe00 465 writeByte(MPU9250_ADDRESS, CONFIG, 0x01);
MarijnJ 0:264ee5acfe00 466
MarijnJ 0:264ee5acfe00 467 // Set sample rate to 1 kHz
MarijnJ 0:264ee5acfe00 468 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);
MarijnJ 0:264ee5acfe00 469
MarijnJ 0:264ee5acfe00 470 // Set gyro full-scale to 250 degrees per second, maximum sensitivity
MarijnJ 0:264ee5acfe00 471 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
MarijnJ 0:264ee5acfe00 472
MarijnJ 0:264ee5acfe00 473 // Set accelerometer full-scale to 2 g, maximum sensitivity
MarijnJ 0:264ee5acfe00 474 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
MarijnJ 0:264ee5acfe00 475
MarijnJ 0:264ee5acfe00 476 // = 131 LSB/degrees/sec
MarijnJ 0:264ee5acfe00 477 uint16_t gyrosensitivity = 131;
MarijnJ 0:264ee5acfe00 478
MarijnJ 0:264ee5acfe00 479 // = 16384 LSB/g
MarijnJ 0:264ee5acfe00 480 uint16_t accelsensitivity = 16384;
MarijnJ 0:264ee5acfe00 481
MarijnJ 0:264ee5acfe00 482 // Configure FIFO to capture accelerometer and gyro data for bias calculation
MarijnJ 0:264ee5acfe00 483 // Enable FIFO
MarijnJ 0:264ee5acfe00 484 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);
MarijnJ 0:264ee5acfe00 485
MarijnJ 0:264ee5acfe00 486 // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
MarijnJ 0:264ee5acfe00 487 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);
MarijnJ 0:264ee5acfe00 488
MarijnJ 0:264ee5acfe00 489 // accumulate 40 samples in 80 milliseconds = 480 bytes
MarijnJ 0:264ee5acfe00 490 wait(0.04);
MarijnJ 0:264ee5acfe00 491
MarijnJ 0:264ee5acfe00 492 // At end of sample accumulation, turn off FIFO sensor read
MarijnJ 0:264ee5acfe00 493 // Disable gyro and accelerometer sensors for FIFO
MarijnJ 0:264ee5acfe00 494 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);
MarijnJ 0:264ee5acfe00 495
MarijnJ 0:264ee5acfe00 496 // read FIFO sample count
MarijnJ 0:264ee5acfe00 497 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]);
MarijnJ 0:264ee5acfe00 498 fifo_count = ((uint16_t) data[0] << 8) | data[1];
MarijnJ 0:264ee5acfe00 499
MarijnJ 0:264ee5acfe00 500 // How many sets of full gyro and accelerometer data for averaging
MarijnJ 0:264ee5acfe00 501 packet_count = fifo_count/12;
MarijnJ 0:264ee5acfe00 502
MarijnJ 0:264ee5acfe00 503 for (ii = 0; ii < packet_count; ii++) {
MarijnJ 0:264ee5acfe00 504 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
MarijnJ 0:264ee5acfe00 505
MarijnJ 0:264ee5acfe00 506 // read data for averaging
MarijnJ 0:264ee5acfe00 507 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]);
MarijnJ 0:264ee5acfe00 508
MarijnJ 0:264ee5acfe00 509 // Form signed 16-bit integer for each sample in FIFO
MarijnJ 0:264ee5acfe00 510 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]);
MarijnJ 0:264ee5acfe00 511 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]);
MarijnJ 0:264ee5acfe00 512 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]);
MarijnJ 0:264ee5acfe00 513 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7]);
MarijnJ 0:264ee5acfe00 514 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9]);
MarijnJ 0:264ee5acfe00 515 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]);
MarijnJ 0:264ee5acfe00 516
MarijnJ 0:264ee5acfe00 517 // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
MarijnJ 0:264ee5acfe00 518 accel_bias[0] += (int32_t) accel_temp[0];
MarijnJ 0:264ee5acfe00 519 accel_bias[1] += (int32_t) accel_temp[1];
MarijnJ 0:264ee5acfe00 520 accel_bias[2] += (int32_t) accel_temp[2];
MarijnJ 0:264ee5acfe00 521 gyro_bias[0] += (int32_t) gyro_temp[0];
MarijnJ 0:264ee5acfe00 522 gyro_bias[1] += (int32_t) gyro_temp[1];
MarijnJ 0:264ee5acfe00 523 gyro_bias[2] += (int32_t) gyro_temp[2];
MarijnJ 0:264ee5acfe00 524 }
MarijnJ 0:264ee5acfe00 525
MarijnJ 0:264ee5acfe00 526 // Normalize sums to get average count biases
MarijnJ 0:264ee5acfe00 527 accel_bias[0] /= (int32_t) packet_count;
MarijnJ 0:264ee5acfe00 528 accel_bias[1] /= (int32_t) packet_count;
MarijnJ 0:264ee5acfe00 529 accel_bias[2] /= (int32_t) packet_count;
MarijnJ 0:264ee5acfe00 530 gyro_bias[0] /= (int32_t) packet_count;
MarijnJ 0:264ee5acfe00 531 gyro_bias[1] /= (int32_t) packet_count;
MarijnJ 0:264ee5acfe00 532 gyro_bias[2] /= (int32_t) packet_count;
MarijnJ 0:264ee5acfe00 533
MarijnJ 0:264ee5acfe00 534 // Remove gravity from the z-axis accelerometer bias calculation
MarijnJ 0:264ee5acfe00 535 if (accel_bias[2] > 0L) {
MarijnJ 0:264ee5acfe00 536 accel_bias[2] -= (int32_t) accelsensitivity;
MarijnJ 0:264ee5acfe00 537 } else {
MarijnJ 0:264ee5acfe00 538 accel_bias[2] += (int32_t) accelsensitivity;
MarijnJ 0:264ee5acfe00 539 }
MarijnJ 0:264ee5acfe00 540
MarijnJ 0:264ee5acfe00 541 // Construct the gyro biases for push to the hardware gyro bias registers,
MarijnJ 0:264ee5acfe00 542 // which are reset to zero upon device startup. Divide by 4 to get
MarijnJ 0:264ee5acfe00 543 // 32.9 LSB per deg/s to conform to expected bias input format
MarijnJ 0:264ee5acfe00 544 data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF;
MarijnJ 0:264ee5acfe00 545
MarijnJ 0:264ee5acfe00 546 // Biases are additive, so change sign on calculated average gyro biases
MarijnJ 0:264ee5acfe00 547 data[1] = (-gyro_bias[0]/4) & 0xFF;
MarijnJ 0:264ee5acfe00 548 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
MarijnJ 0:264ee5acfe00 549 data[3] = (-gyro_bias[1]/4) & 0xFF;
MarijnJ 0:264ee5acfe00 550 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
MarijnJ 0:264ee5acfe00 551 data[5] = (-gyro_bias[2]/4) & 0xFF;
MarijnJ 0:264ee5acfe00 552
MarijnJ 0:264ee5acfe00 553 // Push gyro biases to hardware registers
MarijnJ 0:264ee5acfe00 554 /*
MarijnJ 0:264ee5acfe00 555 writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
MarijnJ 0:264ee5acfe00 556 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
MarijnJ 0:264ee5acfe00 557 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
MarijnJ 0:264ee5acfe00 558 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
MarijnJ 0:264ee5acfe00 559 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
MarijnJ 0:264ee5acfe00 560 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
MarijnJ 0:264ee5acfe00 561 */
MarijnJ 0:264ee5acfe00 562 // Construct gyro bias in deg/s for later manual subtraction
MarijnJ 0:264ee5acfe00 563 gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
MarijnJ 0:264ee5acfe00 564 gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
MarijnJ 0:264ee5acfe00 565 gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
MarijnJ 0:264ee5acfe00 566
MarijnJ 0:264ee5acfe00 567 // Construct the accelerometer biases for push to the hardware
MarijnJ 0:264ee5acfe00 568 // accelerometer bias registers. These registers contain factory trim values
MarijnJ 0:264ee5acfe00 569 // which must be added to the calculated accelerometer biases; on boot up
MarijnJ 0:264ee5acfe00 570 // these registers will hold non-zero values. In addition, bit 0 of the
MarijnJ 0:264ee5acfe00 571 // lower byte must be preserved since it is used for temperature compensation
MarijnJ 0:264ee5acfe00 572 // calculations. Accelerometer bias registers expect bias input as
MarijnJ 0:264ee5acfe00 573 // 2048 LSB per g, so that the accelerometer biases calculated above must
MarijnJ 0:264ee5acfe00 574 // be divided by 8.
MarijnJ 0:264ee5acfe00 575 // A place to hold the factory accelerometer trim biases
MarijnJ 0:264ee5acfe00 576 int32_t accel_bias_reg[3] = {0, 0, 0};
MarijnJ 0:264ee5acfe00 577
MarijnJ 0:264ee5acfe00 578 // Read factory accelerometer trim values
MarijnJ 0:264ee5acfe00 579 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]);
MarijnJ 0:264ee5acfe00 580 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
MarijnJ 0:264ee5acfe00 581 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
MarijnJ 0:264ee5acfe00 582 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
MarijnJ 0:264ee5acfe00 583 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
MarijnJ 0:264ee5acfe00 584 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
MarijnJ 0:264ee5acfe00 585
MarijnJ 0:264ee5acfe00 586 // Define mask for temperature compensation bit 0 of lower byte of
MarijnJ 0:264ee5acfe00 587 // accelerometer bias registers
MarijnJ 0:264ee5acfe00 588 uint32_t mask = 1uL;
MarijnJ 0:264ee5acfe00 589
MarijnJ 0:264ee5acfe00 590 // Define array to hold mask bit for each accelerometer bias axis
MarijnJ 0:264ee5acfe00 591 uint8_t mask_bit[3] = {0, 0, 0};
MarijnJ 0:264ee5acfe00 592
MarijnJ 0:264ee5acfe00 593 for (ii = 0; ii < 3; ii++) {
MarijnJ 0:264ee5acfe00 594 // If temperature compensation bit is set, record that fact in mask_bit
MarijnJ 0:264ee5acfe00 595 if (accel_bias_reg[ii] & mask) {
MarijnJ 0:264ee5acfe00 596 mask_bit[ii] = 0x01;
MarijnJ 0:264ee5acfe00 597 }
MarijnJ 0:264ee5acfe00 598 }
MarijnJ 0:264ee5acfe00 599
MarijnJ 0:264ee5acfe00 600 // Construct total accelerometer bias, including calculated average
MarijnJ 0:264ee5acfe00 601 // accelerometer bias from above. Subtract calculated averaged
MarijnJ 0:264ee5acfe00 602 // accelerometer bias scaled to 2048 LSB/g (16 g full scale)
MarijnJ 0:264ee5acfe00 603 accel_bias_reg[0] -= (accel_bias[0]/8);
MarijnJ 0:264ee5acfe00 604 accel_bias_reg[1] -= (accel_bias[1]/8);
MarijnJ 0:264ee5acfe00 605 accel_bias_reg[2] -= (accel_bias[2]/8);
MarijnJ 0:264ee5acfe00 606
MarijnJ 0:264ee5acfe00 607 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
MarijnJ 0:264ee5acfe00 608 data[1] = (accel_bias_reg[0]) & 0xFF;
MarijnJ 0:264ee5acfe00 609
MarijnJ 0:264ee5acfe00 610 // Preserve temperature compensation bit when writing back to accelerometer bias registers
MarijnJ 0:264ee5acfe00 611 data[1] = data[1] | mask_bit[0];
MarijnJ 0:264ee5acfe00 612 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
MarijnJ 0:264ee5acfe00 613 data[3] = (accel_bias_reg[1]) & 0xFF;
MarijnJ 0:264ee5acfe00 614
MarijnJ 0:264ee5acfe00 615 // Preserve temperature compensation bit when writing back to accelerometer bias registers
MarijnJ 0:264ee5acfe00 616 data[3] = data[3] | mask_bit[1];
MarijnJ 0:264ee5acfe00 617 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
MarijnJ 0:264ee5acfe00 618 data[5] = (accel_bias_reg[2]) & 0xFF;
MarijnJ 0:264ee5acfe00 619
MarijnJ 0:264ee5acfe00 620 // Preserve temperature compensation bit when writing back to accelerometer bias registers
MarijnJ 0:264ee5acfe00 621 data[5] = data[5] | mask_bit[2];
MarijnJ 0:264ee5acfe00 622
MarijnJ 0:264ee5acfe00 623 // Apparently this is not working for the acceleration biases in the MPU-9250
MarijnJ 0:264ee5acfe00 624 // Are we handling the temperature correction bit properly?
MarijnJ 0:264ee5acfe00 625 // Push accelerometer biases to hardware registers
MarijnJ 0:264ee5acfe00 626 /*
MarijnJ 0:264ee5acfe00 627 writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
MarijnJ 0:264ee5acfe00 628 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
MarijnJ 0:264ee5acfe00 629 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
MarijnJ 0:264ee5acfe00 630 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
MarijnJ 0:264ee5acfe00 631 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
MarijnJ 0:264ee5acfe00 632 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
MarijnJ 0:264ee5acfe00 633 */
MarijnJ 0:264ee5acfe00 634
MarijnJ 0:264ee5acfe00 635 // Output scaled accelerometer biases for manual subtraction in the main program
MarijnJ 0:264ee5acfe00 636 accelBias[0] = (float)accel_bias[0] / (float)accelsensitivity;
MarijnJ 0:264ee5acfe00 637 accelBias[1] = (float)accel_bias[1] / (float)accelsensitivity;
MarijnJ 0:264ee5acfe00 638 accelBias[2] = (float)accel_bias[2] / (float)accelsensitivity;
MarijnJ 0:264ee5acfe00 639 }
MarijnJ 0:264ee5acfe00 640
MarijnJ 0:264ee5acfe00 641
MarijnJ 0:264ee5acfe00 642 // Accelerometer and gyroscope self test; check calibration wrt factory settings
MarijnJ 0:264ee5acfe00 643 void MPU9250SelfTest(float *destination) {
MarijnJ 0:264ee5acfe00 644 // Should return percent deviation from factory trim values,
MarijnJ 0:264ee5acfe00 645 // +/- 14 or less deviation is a pass {
MarijnJ 0:264ee5acfe00 646 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
MarijnJ 0:264ee5acfe00 647 uint8_t selfTest[6];
MarijnJ 0:264ee5acfe00 648 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
MarijnJ 0:264ee5acfe00 649 float factoryTrim[6];
MarijnJ 0:264ee5acfe00 650 uint8_t FS = 0;
MarijnJ 0:264ee5acfe00 651
MarijnJ 0:264ee5acfe00 652 // Set gyro sample rate to 1 kHz
MarijnJ 0:264ee5acfe00 653 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);
MarijnJ 0:264ee5acfe00 654
MarijnJ 0:264ee5acfe00 655 // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
MarijnJ 0:264ee5acfe00 656 writeByte(MPU9250_ADDRESS, CONFIG, 0x02);
MarijnJ 0:264ee5acfe00 657
MarijnJ 0:264ee5acfe00 658 // Set full scale range for the gyro to 250 dps
MarijnJ 0:264ee5acfe00 659 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS);
MarijnJ 0:264ee5acfe00 660
MarijnJ 0:264ee5acfe00 661 // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
MarijnJ 0:264ee5acfe00 662 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02);
MarijnJ 0:264ee5acfe00 663
MarijnJ 0:264ee5acfe00 664 // Set full scale range for the accelerometer to 2 g
MarijnJ 0:264ee5acfe00 665 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS);
MarijnJ 0:264ee5acfe00 666
MarijnJ 0:264ee5acfe00 667 // Get average current values of gyro and acclerometer
MarijnJ 0:264ee5acfe00 668 for (int ii = 0; ii < 200; ii++) {
MarijnJ 0:264ee5acfe00 669 // Read the six raw data registers into data array
MarijnJ 0:264ee5acfe00 670 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
MarijnJ 0:264ee5acfe00 671
MarijnJ 0:264ee5acfe00 672 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 673 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
MarijnJ 0:264ee5acfe00 674 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
MarijnJ 0:264ee5acfe00 675 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
MarijnJ 0:264ee5acfe00 676
MarijnJ 0:264ee5acfe00 677 // Read the six raw data registers sequentially into data array
MarijnJ 0:264ee5acfe00 678 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
MarijnJ 0:264ee5acfe00 679
MarijnJ 0:264ee5acfe00 680 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 681 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
MarijnJ 0:264ee5acfe00 682 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
MarijnJ 0:264ee5acfe00 683 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
MarijnJ 0:264ee5acfe00 684 }
MarijnJ 0:264ee5acfe00 685
MarijnJ 0:264ee5acfe00 686 // Get average of 200 values and store as average current readings
MarijnJ 0:264ee5acfe00 687 for (int ii = 0; ii < 3; ii++) {
MarijnJ 0:264ee5acfe00 688 aAvg[ii] /= 200;
MarijnJ 0:264ee5acfe00 689 gAvg[ii] /= 200;
MarijnJ 0:264ee5acfe00 690 }
MarijnJ 0:264ee5acfe00 691
MarijnJ 0:264ee5acfe00 692 // Configure the accelerometer for self-test
MarijnJ 0:264ee5acfe00 693 // Enable self test on all three axes and set accelerometer range to +/- 2 g
MarijnJ 0:264ee5acfe00 694 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0);
MarijnJ 0:264ee5acfe00 695
MarijnJ 0:264ee5acfe00 696 // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
MarijnJ 0:264ee5acfe00 697 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0);
MarijnJ 0:264ee5acfe00 698
MarijnJ 0:264ee5acfe00 699 // Delay a while to let the device stabilize
MarijnJ 0:264ee5acfe00 700 wait(0.1);
MarijnJ 0:264ee5acfe00 701
MarijnJ 0:264ee5acfe00 702 // Get average self-test values of gyro and acclerometer
MarijnJ 0:264ee5acfe00 703 for (int ii = 0; ii < 200; ii++) {
MarijnJ 0:264ee5acfe00 704 // Read the six raw data registers into data array
MarijnJ 0:264ee5acfe00 705 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
MarijnJ 0:264ee5acfe00 706
MarijnJ 0:264ee5acfe00 707 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 708 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
MarijnJ 0:264ee5acfe00 709 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
MarijnJ 0:264ee5acfe00 710 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
MarijnJ 0:264ee5acfe00 711
MarijnJ 0:264ee5acfe00 712 // Read the six raw data registers sequentially into data array
MarijnJ 0:264ee5acfe00 713 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
MarijnJ 0:264ee5acfe00 714
MarijnJ 0:264ee5acfe00 715 // Turn the MSB and LSB into a signed 16-bit value
MarijnJ 0:264ee5acfe00 716 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
MarijnJ 0:264ee5acfe00 717 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
MarijnJ 0:264ee5acfe00 718 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
MarijnJ 0:264ee5acfe00 719 }
MarijnJ 0:264ee5acfe00 720
MarijnJ 0:264ee5acfe00 721 // Get average of 200 values and store as average self-test readings
MarijnJ 0:264ee5acfe00 722 for (int ii = 0; ii < 3; ii++) {
MarijnJ 0:264ee5acfe00 723 aSTAvg[ii] /= 200;
MarijnJ 0:264ee5acfe00 724 gSTAvg[ii] /= 200;
MarijnJ 0:264ee5acfe00 725 }
MarijnJ 0:264ee5acfe00 726
MarijnJ 0:264ee5acfe00 727 // Configure the gyro and accelerometer for normal operation
MarijnJ 0:264ee5acfe00 728 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
MarijnJ 0:264ee5acfe00 729 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
MarijnJ 0:264ee5acfe00 730
MarijnJ 0:264ee5acfe00 731 // Delay a while to let the device stabilize
MarijnJ 0:264ee5acfe00 732 wait(0.1);
MarijnJ 0:264ee5acfe00 733
MarijnJ 0:264ee5acfe00 734 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
MarijnJ 0:264ee5acfe00 735 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL);
MarijnJ 0:264ee5acfe00 736 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL);
MarijnJ 0:264ee5acfe00 737 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL);
MarijnJ 0:264ee5acfe00 738 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO);
MarijnJ 0:264ee5acfe00 739 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO);
MarijnJ 0:264ee5acfe00 740 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO);
MarijnJ 0:264ee5acfe00 741
MarijnJ 0:264ee5acfe00 742 // Retrieve factory self-test value from self-test code reads
MarijnJ 0:264ee5acfe00 743 // FT[Xa] factory trim calculation, and FT[Ya], FT[Xg], etc
MarijnJ 0:264ee5acfe00 744 factoryTrim[0] = (float) (2620 / 1<<FS) * (pow(1.01, ((float) selfTest[0] - 1.0)));
MarijnJ 0:264ee5acfe00 745 factoryTrim[1] = (float) (2620 / 1<<FS) * (pow(1.01, ((float) selfTest[1] - 1.0)));
MarijnJ 0:264ee5acfe00 746 factoryTrim[2] = (float) (2620 / 1<<FS) * (pow(1.01, ((float) selfTest[2] - 1.0)));
MarijnJ 0:264ee5acfe00 747 factoryTrim[3] = (float) (2620 / 1<<FS) * (pow(1.01, ((float) selfTest[3] - 1.0)));
MarijnJ 0:264ee5acfe00 748 factoryTrim[4] = (float) (2620 / 1<<FS) * (pow(1.01, ((float) selfTest[4] - 1.0)));
MarijnJ 0:264ee5acfe00 749 factoryTrim[5] = (float) (2620 / 1<<FS) * (pow(1.01, ((float) selfTest[5] - 1.0)));
MarijnJ 0:264ee5acfe00 750
MarijnJ 0:264ee5acfe00 751 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim
MarijnJ 0:264ee5acfe00 752 // of the Self-Test Response. To get percent, must multiply by 100
MarijnJ 0:264ee5acfe00 753 for (int i = 0; i < 3; i++) {
MarijnJ 0:264ee5acfe00 754 // Report percent differences
MarijnJ 0:264ee5acfe00 755 destination[i] = 100.0 * ((float) (aSTAvg[i] - aAvg[i])) / factoryTrim[i];
MarijnJ 0:264ee5acfe00 756
MarijnJ 0:264ee5acfe00 757 // Report percent differences
MarijnJ 0:264ee5acfe00 758 destination[i + 3] = 100.0 * ((float) (gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3];
MarijnJ 0:264ee5acfe00 759 }
MarijnJ 0:264ee5acfe00 760 }
MarijnJ 0:264ee5acfe00 761
MarijnJ 0:264ee5acfe00 762
MarijnJ 0:264ee5acfe00 763
MarijnJ 0:264ee5acfe00 764 // Implementation of Sebastian Madgwick's "...efficient orientation filter
MarijnJ 0:264ee5acfe00 765 // for... inertial/magnetic sensor arrays"
MarijnJ 0:264ee5acfe00 766 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
MarijnJ 0:264ee5acfe00 767 // which fuses acceleration, rotation rate, and magnetic moments to produce a
MarijnJ 0:264ee5acfe00 768 // quaternion-based estimate of absolute
MarijnJ 0:264ee5acfe00 769 // device orientation -- which can be converted to yaw, pitch, and roll.
MarijnJ 0:264ee5acfe00 770 // Useful for stabilizing quadcopters, etc. The performance of the orientation
MarijnJ 0:264ee5acfe00 771 // filter is at least as good as conventional Kalman-based filtering algorithms
MarijnJ 0:264ee5acfe00 772 // but is much less computationally intensive---it can be performed on
MarijnJ 0:264ee5acfe00 773 // a 3.3 V Pro Mini operating at 8 MHz!
MarijnJ 0:264ee5acfe00 774 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
MarijnJ 0:264ee5acfe00 775 float gz, float mx, float my, float mz) {
MarijnJ 0:264ee5acfe00 776 // Short name local variable for readability
MarijnJ 0:264ee5acfe00 777 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
MarijnJ 0:264ee5acfe00 778 float norm;
MarijnJ 0:264ee5acfe00 779 float hx, hy, _2bx, _2bz;
MarijnJ 0:264ee5acfe00 780 float s1, s2, s3, s4;
MarijnJ 0:264ee5acfe00 781 float qDot1, qDot2, qDot3, qDot4;
MarijnJ 0:264ee5acfe00 782
MarijnJ 0:264ee5acfe00 783 // Auxiliary variables to avoid repeated arithmetic
MarijnJ 0:264ee5acfe00 784 float _2q1mx;
MarijnJ 0:264ee5acfe00 785 float _2q1my;
MarijnJ 0:264ee5acfe00 786 float _2q1mz;
MarijnJ 0:264ee5acfe00 787 float _2q2mx;
MarijnJ 0:264ee5acfe00 788 float _4bx;
MarijnJ 0:264ee5acfe00 789 float _4bz;
MarijnJ 0:264ee5acfe00 790 float _2q1 = 2.0f * q1;
MarijnJ 0:264ee5acfe00 791 float _2q2 = 2.0f * q2;
MarijnJ 0:264ee5acfe00 792 float _2q3 = 2.0f * q3;
MarijnJ 0:264ee5acfe00 793 float _2q4 = 2.0f * q4;
MarijnJ 0:264ee5acfe00 794 float _2q1q3 = 2.0f * q1 * q3;
MarijnJ 0:264ee5acfe00 795 float _2q3q4 = 2.0f * q3 * q4;
MarijnJ 0:264ee5acfe00 796 float q1q1 = q1 * q1;
MarijnJ 0:264ee5acfe00 797 float q1q2 = q1 * q2;
MarijnJ 0:264ee5acfe00 798 float q1q3 = q1 * q3;
MarijnJ 0:264ee5acfe00 799 float q1q4 = q1 * q4;
MarijnJ 0:264ee5acfe00 800 float q2q2 = q2 * q2;
MarijnJ 0:264ee5acfe00 801 float q2q3 = q2 * q3;
MarijnJ 0:264ee5acfe00 802 float q2q4 = q2 * q4;
MarijnJ 0:264ee5acfe00 803 float q3q3 = q3 * q3;
MarijnJ 0:264ee5acfe00 804 float q3q4 = q3 * q4;
MarijnJ 0:264ee5acfe00 805 float q4q4 = q4 * q4;
MarijnJ 0:264ee5acfe00 806
MarijnJ 0:264ee5acfe00 807 // Normalise accelerometer measurement
MarijnJ 0:264ee5acfe00 808 norm = sqrt(ax * ax + ay * ay + az * az);
MarijnJ 0:264ee5acfe00 809
MarijnJ 0:264ee5acfe00 810 // Handle NaN
MarijnJ 0:264ee5acfe00 811 if (norm == 0.0f) {
MarijnJ 0:264ee5acfe00 812 return;
MarijnJ 0:264ee5acfe00 813 }
MarijnJ 0:264ee5acfe00 814
MarijnJ 0:264ee5acfe00 815 norm = 1.0f / norm;
MarijnJ 0:264ee5acfe00 816 ax *= norm;
MarijnJ 0:264ee5acfe00 817 ay *= norm;
MarijnJ 0:264ee5acfe00 818 az *= norm;
MarijnJ 0:264ee5acfe00 819
MarijnJ 0:264ee5acfe00 820 // Normalise magnetometer measurement
MarijnJ 0:264ee5acfe00 821 norm = sqrt(mx * mx + my * my + mz * mz);
MarijnJ 0:264ee5acfe00 822
MarijnJ 0:264ee5acfe00 823 // Handle NaN
MarijnJ 0:264ee5acfe00 824 if (norm == 0.0f) {
MarijnJ 0:264ee5acfe00 825 return;
MarijnJ 0:264ee5acfe00 826 }
MarijnJ 0:264ee5acfe00 827
MarijnJ 0:264ee5acfe00 828 norm = 1.0f/norm;
MarijnJ 0:264ee5acfe00 829 mx *= norm;
MarijnJ 0:264ee5acfe00 830 my *= norm;
MarijnJ 0:264ee5acfe00 831 mz *= norm;
MarijnJ 0:264ee5acfe00 832
MarijnJ 0:264ee5acfe00 833 // Reference direction of Earth's magnetic field
MarijnJ 0:264ee5acfe00 834 _2q1mx = 2.0f * q1 * mx;
MarijnJ 0:264ee5acfe00 835 _2q1my = 2.0f * q1 * my;
MarijnJ 0:264ee5acfe00 836 _2q1mz = 2.0f * q1 * mz;
MarijnJ 0:264ee5acfe00 837 _2q2mx = 2.0f * q2 * mx;
MarijnJ 0:264ee5acfe00 838 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3
MarijnJ 0:264ee5acfe00 839 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
MarijnJ 0:264ee5acfe00 840 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2
MarijnJ 0:264ee5acfe00 841 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
MarijnJ 0:264ee5acfe00 842 _2bx = sqrt(hx * hx + hy * hy);
MarijnJ 0:264ee5acfe00 843 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2
MarijnJ 0:264ee5acfe00 844 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
MarijnJ 0:264ee5acfe00 845 _4bx = 2.0f * _2bx;
MarijnJ 0:264ee5acfe00 846 _4bz = 2.0f * _2bz;
MarijnJ 0:264ee5acfe00 847
MarijnJ 0:264ee5acfe00 848 // Gradient descent algorithm corrective step
MarijnJ 0:264ee5acfe00 849 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay)
MarijnJ 0:264ee5acfe00 850 - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
MarijnJ 0:264ee5acfe00 851 + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
MarijnJ 0:264ee5acfe00 852 + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
MarijnJ 0:264ee5acfe00 853 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay)
MarijnJ 0:264ee5acfe00 854 - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az)
MarijnJ 0:264ee5acfe00 855 + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
MarijnJ 0:264ee5acfe00 856 + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
MarijnJ 0:264ee5acfe00 857 + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4)
MarijnJ 0:264ee5acfe00 858 + _2bz * (0.5f - q2q2 - q3q3) - mz);
MarijnJ 0:264ee5acfe00 859 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay)
MarijnJ 0:264ee5acfe00 860 - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az)
MarijnJ 0:264ee5acfe00 861 + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4)
MarijnJ 0:264ee5acfe00 862 + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4)
MarijnJ 0:264ee5acfe00 863 + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4)
MarijnJ 0:264ee5acfe00 864 + _2bz * (0.5f - q2q2 - q3q3) - mz);
MarijnJ 0:264ee5acfe00 865 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2
MarijnJ 0:264ee5acfe00 866 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4)
MarijnJ 0:264ee5acfe00 867 + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4)
MarijnJ 0:264ee5acfe00 868 + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4)
MarijnJ 0:264ee5acfe00 869 + _2bz * (0.5f - q2q2 - q3q3) - mz);
MarijnJ 0:264ee5acfe00 870
MarijnJ 0:264ee5acfe00 871 // Normalise step magnitude
MarijnJ 0:264ee5acfe00 872 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);
MarijnJ 0:264ee5acfe00 873 norm = 1.0f / norm;
MarijnJ 0:264ee5acfe00 874 s1 *= norm;
MarijnJ 0:264ee5acfe00 875 s2 *= norm;
MarijnJ 0:264ee5acfe00 876 s3 *= norm;
MarijnJ 0:264ee5acfe00 877 s4 *= norm;
MarijnJ 0:264ee5acfe00 878
MarijnJ 0:264ee5acfe00 879 // Compute rate of change of quaternion
MarijnJ 0:264ee5acfe00 880 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
MarijnJ 0:264ee5acfe00 881 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
MarijnJ 0:264ee5acfe00 882 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
MarijnJ 0:264ee5acfe00 883 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
MarijnJ 0:264ee5acfe00 884
MarijnJ 0:264ee5acfe00 885 // Integrate to yield quaternion
MarijnJ 0:264ee5acfe00 886 q1 += qDot1 * deltat;
MarijnJ 0:264ee5acfe00 887 q2 += qDot2 * deltat;
MarijnJ 0:264ee5acfe00 888 q3 += qDot3 * deltat;
MarijnJ 0:264ee5acfe00 889 q4 += qDot4 * deltat;
MarijnJ 0:264ee5acfe00 890
MarijnJ 0:264ee5acfe00 891 // Normalise quaternion
MarijnJ 0:264ee5acfe00 892 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
MarijnJ 0:264ee5acfe00 893 norm = 1.0f / norm;
MarijnJ 0:264ee5acfe00 894 q[0] = q1 * norm;
MarijnJ 0:264ee5acfe00 895 q[1] = q2 * norm;
MarijnJ 0:264ee5acfe00 896 q[2] = q3 * norm;
MarijnJ 0:264ee5acfe00 897 q[3] = q4 * norm;
MarijnJ 0:264ee5acfe00 898 }
MarijnJ 0:264ee5acfe00 899
MarijnJ 0:264ee5acfe00 900
MarijnJ 0:264ee5acfe00 901 // Similar to Madgwick scheme but uses proportional and integral filtering on
MarijnJ 0:264ee5acfe00 902 // the error between estimated reference vectors and measured ones.
MarijnJ 0:264ee5acfe00 903 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
MarijnJ 0:264ee5acfe00 904 float gz, float mx, float my, float mz) {
MarijnJ 0:264ee5acfe00 905 // short name local variable for readability
MarijnJ 0:264ee5acfe00 906 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
MarijnJ 0:264ee5acfe00 907 float norm;
MarijnJ 0:264ee5acfe00 908 float hx, hy, bx, bz;
MarijnJ 0:264ee5acfe00 909 float vx, vy, vz, wx, wy, wz;
MarijnJ 0:264ee5acfe00 910 float ex, ey, ez;
MarijnJ 0:264ee5acfe00 911 float pa, pb, pc;
MarijnJ 0:264ee5acfe00 912
MarijnJ 0:264ee5acfe00 913 // Auxiliary variables to avoid repeated arithmetic
MarijnJ 0:264ee5acfe00 914 float q1q1 = q1 * q1;
MarijnJ 0:264ee5acfe00 915 float q1q2 = q1 * q2;
MarijnJ 0:264ee5acfe00 916 float q1q3 = q1 * q3;
MarijnJ 0:264ee5acfe00 917 float q1q4 = q1 * q4;
MarijnJ 0:264ee5acfe00 918 float q2q2 = q2 * q2;
MarijnJ 0:264ee5acfe00 919 float q2q3 = q2 * q3;
MarijnJ 0:264ee5acfe00 920 float q2q4 = q2 * q4;
MarijnJ 0:264ee5acfe00 921 float q3q3 = q3 * q3;
MarijnJ 0:264ee5acfe00 922 float q3q4 = q3 * q4;
MarijnJ 0:264ee5acfe00 923 float q4q4 = q4 * q4;
MarijnJ 0:264ee5acfe00 924
MarijnJ 0:264ee5acfe00 925 // Normalise accelerometer measurement
MarijnJ 0:264ee5acfe00 926 norm = sqrt(ax * ax + ay * ay + az * az);
MarijnJ 0:264ee5acfe00 927
MarijnJ 0:264ee5acfe00 928 // Handle NaN
MarijnJ 0:264ee5acfe00 929 if (norm == 0.0f) {
MarijnJ 0:264ee5acfe00 930 return;
MarijnJ 0:264ee5acfe00 931 }
MarijnJ 0:264ee5acfe00 932
MarijnJ 0:264ee5acfe00 933 // Use reciprocal for division
MarijnJ 0:264ee5acfe00 934 norm = 1.0f / norm;
MarijnJ 0:264ee5acfe00 935 ax *= norm;
MarijnJ 0:264ee5acfe00 936 ay *= norm;
MarijnJ 0:264ee5acfe00 937 az *= norm;
MarijnJ 0:264ee5acfe00 938
MarijnJ 0:264ee5acfe00 939 // Normalise magnetometer measurement
MarijnJ 0:264ee5acfe00 940 norm = sqrt(mx * mx + my * my + mz * mz);
MarijnJ 0:264ee5acfe00 941
MarijnJ 0:264ee5acfe00 942 // Handle NaN
MarijnJ 0:264ee5acfe00 943 if (norm == 0.0f) {
MarijnJ 0:264ee5acfe00 944 return;
MarijnJ 0:264ee5acfe00 945 }
MarijnJ 0:264ee5acfe00 946
MarijnJ 0:264ee5acfe00 947 // Use reciprocal for division
MarijnJ 0:264ee5acfe00 948 norm = 1.0f / norm;
MarijnJ 0:264ee5acfe00 949 mx *= norm;
MarijnJ 0:264ee5acfe00 950 my *= norm;
MarijnJ 0:264ee5acfe00 951 mz *= norm;
MarijnJ 0:264ee5acfe00 952
MarijnJ 0:264ee5acfe00 953 // Reference direction of Earth's magnetic field
MarijnJ 0:264ee5acfe00 954 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4)
MarijnJ 0:264ee5acfe00 955 + 2.0f * mz * (q2q4 + q1q3);
MarijnJ 0:264ee5acfe00 956 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4)
MarijnJ 0:264ee5acfe00 957 + 2.0f * mz * (q3q4 - q1q2);
MarijnJ 0:264ee5acfe00 958 bx = sqrt((hx * hx) + (hy * hy));
MarijnJ 0:264ee5acfe00 959 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2)
MarijnJ 0:264ee5acfe00 960 + 2.0f * mz * (0.5f - q2q2 - q3q3);
MarijnJ 0:264ee5acfe00 961
MarijnJ 0:264ee5acfe00 962 // Estimated direction of gravity and magnetic field
MarijnJ 0:264ee5acfe00 963 vx = 2.0f * (q2q4 - q1q3);
MarijnJ 0:264ee5acfe00 964 vy = 2.0f * (q1q2 + q3q4);
MarijnJ 0:264ee5acfe00 965 vz = q1q1 - q2q2 - q3q3 + q4q4;
MarijnJ 0:264ee5acfe00 966 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
MarijnJ 0:264ee5acfe00 967 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
MarijnJ 0:264ee5acfe00 968 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
MarijnJ 0:264ee5acfe00 969
MarijnJ 0:264ee5acfe00 970 // Error is cross product between estimated direction and measured direction of gravity
MarijnJ 0:264ee5acfe00 971 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
MarijnJ 0:264ee5acfe00 972 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
MarijnJ 0:264ee5acfe00 973 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
MarijnJ 0:264ee5acfe00 974
MarijnJ 0:264ee5acfe00 975 if (Ki > 0.0f) {
MarijnJ 0:264ee5acfe00 976 // Accumulate integral error
MarijnJ 0:264ee5acfe00 977 eInt[0] += ex;
MarijnJ 0:264ee5acfe00 978 eInt[1] += ey;
MarijnJ 0:264ee5acfe00 979 eInt[2] += ez;
MarijnJ 0:264ee5acfe00 980 } else {
MarijnJ 0:264ee5acfe00 981 // Prevent integral wind up
MarijnJ 0:264ee5acfe00 982 eInt[0] = 0.0f;
MarijnJ 0:264ee5acfe00 983 eInt[1] = 0.0f;
MarijnJ 0:264ee5acfe00 984 eInt[2] = 0.0f;
MarijnJ 0:264ee5acfe00 985 }
MarijnJ 0:264ee5acfe00 986
MarijnJ 0:264ee5acfe00 987 // Apply feedback terms
MarijnJ 0:264ee5acfe00 988 gx = gx + Kp * ex + Ki * eInt[0];
MarijnJ 0:264ee5acfe00 989 gy = gy + Kp * ey + Ki * eInt[1];
MarijnJ 0:264ee5acfe00 990 gz = gz + Kp * ez + Ki * eInt[2];
MarijnJ 0:264ee5acfe00 991
MarijnJ 0:264ee5acfe00 992 // Integrate rate of change of quaternion
MarijnJ 0:264ee5acfe00 993 pa = q2;
MarijnJ 0:264ee5acfe00 994 pb = q3;
MarijnJ 0:264ee5acfe00 995 pc = q4;
MarijnJ 0:264ee5acfe00 996 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
MarijnJ 0:264ee5acfe00 997 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
MarijnJ 0:264ee5acfe00 998 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
MarijnJ 0:264ee5acfe00 999 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
MarijnJ 0:264ee5acfe00 1000
MarijnJ 0:264ee5acfe00 1001 // Normalise quaternion
MarijnJ 0:264ee5acfe00 1002 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
MarijnJ 0:264ee5acfe00 1003 norm = 1.0f / norm;
MarijnJ 0:264ee5acfe00 1004 q[0] = q1 * norm;
MarijnJ 0:264ee5acfe00 1005 q[1] = q2 * norm;
MarijnJ 0:264ee5acfe00 1006 q[2] = q3 * norm;
MarijnJ 0:264ee5acfe00 1007 q[3] = q4 * norm;
MarijnJ 0:264ee5acfe00 1008 }
MarijnJ 0:264ee5acfe00 1009
MarijnJ 0:264ee5acfe00 1010 };
MarijnJ 0:264ee5acfe00 1011
MarijnJ 0:264ee5acfe00 1012 #endif