Team H - TVZ Seminar / Mbed 2 deprecated Zavrsni_Daljinski

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SSD1308_128x64_I2C

MPU9250.h@2:0cf2ef636323, 2019-09-07 (annotated)

Committer:
jjokocha
Date:
Sat Sep 07 16:55:08 2019 +0000
Revision:
2:0cf2ef636323
kommit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjokocha 2:0cf2ef636323 1 #ifndef MPU9250_H
jjokocha 2:0cf2ef636323 2 #define MPU9250_H
jjokocha 2:0cf2ef636323 3
jjokocha 2:0cf2ef636323 4 #include "mbed.h"
jjokocha 2:0cf2ef636323 5 #include "math.h"
jjokocha 2:0cf2ef636323 6
jjokocha 2:0cf2ef636323 7 #define USE_ISR 0 // poll or data ready interrupt
jjokocha 2:0cf2ef636323 8
jjokocha 2:0cf2ef636323 9 // See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in
jjokocha 2:0cf2ef636323 10 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
jjokocha 2:0cf2ef636323 11 //
jjokocha 2:0cf2ef636323 12 //Magnetometer Registers
jjokocha 2:0cf2ef636323 13 #define AK8963_ADDRESS 0x0C<<1
jjokocha 2:0cf2ef636323 14 #define WHO_AM_I_AK8963 0x00 // should return 0x48
jjokocha 2:0cf2ef636323 15 #define INFO 0x01
jjokocha 2:0cf2ef636323 16 #define AK8963_ST1 0x02 // data ready status bit 0
jjokocha 2:0cf2ef636323 17 #define AK8963_XOUT_L 0x03 // data
jjokocha 2:0cf2ef636323 18 #define AK8963_XOUT_H 0x04
jjokocha 2:0cf2ef636323 19 #define AK8963_YOUT_L 0x05
jjokocha 2:0cf2ef636323 20 #define AK8963_YOUT_H 0x06
jjokocha 2:0cf2ef636323 21 #define AK8963_ZOUT_L 0x07
jjokocha 2:0cf2ef636323 22 #define AK8963_ZOUT_H 0x08
jjokocha 2:0cf2ef636323 23 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
jjokocha 2:0cf2ef636323 24 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
jjokocha 2:0cf2ef636323 25 #define AK8963_ASTC 0x0C // Self test control
jjokocha 2:0cf2ef636323 26 #define AK8963_I2CDIS 0x0F // I2C disable
jjokocha 2:0cf2ef636323 27 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
jjokocha 2:0cf2ef636323 28 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
jjokocha 2:0cf2ef636323 29 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
jjokocha 2:0cf2ef636323 30
jjokocha 2:0cf2ef636323 31 #define SELF_TEST_X_GYRO 0x00
jjokocha 2:0cf2ef636323 32 #define SELF_TEST_Y_GYRO 0x01
jjokocha 2:0cf2ef636323 33 #define SELF_TEST_Z_GYRO 0x02
jjokocha 2:0cf2ef636323 34
jjokocha 2:0cf2ef636323 35 /*#define X_FINE_GAIN 0x03 // [7:0] fine gain
jjokocha 2:0cf2ef636323 36 #define Y_FINE_GAIN 0x04
jjokocha 2:0cf2ef636323 37 #define Z_FINE_GAIN 0x05
jjokocha 2:0cf2ef636323 38 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
jjokocha 2:0cf2ef636323 39 #define XA_OFFSET_L_TC 0x07
jjokocha 2:0cf2ef636323 40 #define YA_OFFSET_H 0x08
jjokocha 2:0cf2ef636323 41 #define YA_OFFSET_L_TC 0x09
jjokocha 2:0cf2ef636323 42 #define ZA_OFFSET_H 0x0A
jjokocha 2:0cf2ef636323 43 #define ZA_OFFSET_L_TC 0x0B */
jjokocha 2:0cf2ef636323 44
jjokocha 2:0cf2ef636323 45 #define SELF_TEST_X_ACCEL 0x0D
jjokocha 2:0cf2ef636323 46 #define SELF_TEST_Y_ACCEL 0x0E
jjokocha 2:0cf2ef636323 47 #define SELF_TEST_Z_ACCEL 0x0F
jjokocha 2:0cf2ef636323 48
jjokocha 2:0cf2ef636323 49 #define SELF_TEST_A 0x10
jjokocha 2:0cf2ef636323 50
jjokocha 2:0cf2ef636323 51 #define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
jjokocha 2:0cf2ef636323 52 #define XG_OFFSET_L 0x14
jjokocha 2:0cf2ef636323 53 #define YG_OFFSET_H 0x15
jjokocha 2:0cf2ef636323 54 #define YG_OFFSET_L 0x16
jjokocha 2:0cf2ef636323 55 #define ZG_OFFSET_H 0x17
jjokocha 2:0cf2ef636323 56 #define ZG_OFFSET_L 0x18
jjokocha 2:0cf2ef636323 57 #define SMPLRT_DIV 0x19
jjokocha 2:0cf2ef636323 58 #define CONFIG 0x1A
jjokocha 2:0cf2ef636323 59 #define GYRO_CONFIG 0x1B
jjokocha 2:0cf2ef636323 60 #define ACCEL_CONFIG 0x1C
jjokocha 2:0cf2ef636323 61 #define ACCEL_CONFIG2 0x1D
jjokocha 2:0cf2ef636323 62 #define LP_ACCEL_ODR 0x1E
jjokocha 2:0cf2ef636323 63 #define WOM_THR 0x1F
jjokocha 2:0cf2ef636323 64
jjokocha 2:0cf2ef636323 65 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
jjokocha 2:0cf2ef636323 66 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
jjokocha 2:0cf2ef636323 67 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
jjokocha 2:0cf2ef636323 68
jjokocha 2:0cf2ef636323 69 #define FIFO_EN 0x23
jjokocha 2:0cf2ef636323 70 #define I2C_MST_CTRL 0x24
jjokocha 2:0cf2ef636323 71 #define I2C_SLV0_ADDR 0x25
jjokocha 2:0cf2ef636323 72 #define I2C_SLV0_REG 0x26
jjokocha 2:0cf2ef636323 73 #define I2C_SLV0_CTRL 0x27
jjokocha 2:0cf2ef636323 74 #define I2C_SLV1_ADDR 0x28
jjokocha 2:0cf2ef636323 75 #define I2C_SLV1_REG 0x29
jjokocha 2:0cf2ef636323 76 #define I2C_SLV1_CTRL 0x2A
jjokocha 2:0cf2ef636323 77 #define I2C_SLV2_ADDR 0x2B
jjokocha 2:0cf2ef636323 78 #define I2C_SLV2_REG 0x2C
jjokocha 2:0cf2ef636323 79 #define I2C_SLV2_CTRL 0x2D
jjokocha 2:0cf2ef636323 80 #define I2C_SLV3_ADDR 0x2E
jjokocha 2:0cf2ef636323 81 #define I2C_SLV3_REG 0x2F
jjokocha 2:0cf2ef636323 82 #define I2C_SLV3_CTRL 0x30
jjokocha 2:0cf2ef636323 83 #define I2C_SLV4_ADDR 0x31
jjokocha 2:0cf2ef636323 84 #define I2C_SLV4_REG 0x32
jjokocha 2:0cf2ef636323 85 #define I2C_SLV4_DO 0x33
jjokocha 2:0cf2ef636323 86 #define I2C_SLV4_CTRL 0x34
jjokocha 2:0cf2ef636323 87 #define I2C_SLV4_DI 0x35
jjokocha 2:0cf2ef636323 88 #define I2C_MST_STATUS 0x36
jjokocha 2:0cf2ef636323 89 #define INT_PIN_CFG 0x37
jjokocha 2:0cf2ef636323 90 #define INT_ENABLE 0x38
jjokocha 2:0cf2ef636323 91 #define DMP_INT_STATUS 0x39 // Check DMP interrupt
jjokocha 2:0cf2ef636323 92 #define INT_STATUS 0x3A
jjokocha 2:0cf2ef636323 93 #define ACCEL_XOUT_H 0x3B
jjokocha 2:0cf2ef636323 94 #define ACCEL_XOUT_L 0x3C
jjokocha 2:0cf2ef636323 95 #define ACCEL_YOUT_H 0x3D
jjokocha 2:0cf2ef636323 96 #define ACCEL_YOUT_L 0x3E
jjokocha 2:0cf2ef636323 97 #define ACCEL_ZOUT_H 0x3F
jjokocha 2:0cf2ef636323 98 #define ACCEL_ZOUT_L 0x40
jjokocha 2:0cf2ef636323 99 #define TEMP_OUT_H 0x41
jjokocha 2:0cf2ef636323 100 #define TEMP_OUT_L 0x42
jjokocha 2:0cf2ef636323 101 #define GYRO_XOUT_H 0x43
jjokocha 2:0cf2ef636323 102 #define GYRO_XOUT_L 0x44
jjokocha 2:0cf2ef636323 103 #define GYRO_YOUT_H 0x45
jjokocha 2:0cf2ef636323 104 #define GYRO_YOUT_L 0x46
jjokocha 2:0cf2ef636323 105 #define GYRO_ZOUT_H 0x47
jjokocha 2:0cf2ef636323 106 #define GYRO_ZOUT_L 0x48
jjokocha 2:0cf2ef636323 107 #define EXT_SENS_DATA_00 0x49
jjokocha 2:0cf2ef636323 108 #define EXT_SENS_DATA_01 0x4A
jjokocha 2:0cf2ef636323 109 #define EXT_SENS_DATA_02 0x4B
jjokocha 2:0cf2ef636323 110 #define EXT_SENS_DATA_03 0x4C
jjokocha 2:0cf2ef636323 111 #define EXT_SENS_DATA_04 0x4D
jjokocha 2:0cf2ef636323 112 #define EXT_SENS_DATA_05 0x4E
jjokocha 2:0cf2ef636323 113 #define EXT_SENS_DATA_06 0x4F
jjokocha 2:0cf2ef636323 114 #define EXT_SENS_DATA_07 0x50
jjokocha 2:0cf2ef636323 115 #define EXT_SENS_DATA_08 0x51
jjokocha 2:0cf2ef636323 116 #define EXT_SENS_DATA_09 0x52
jjokocha 2:0cf2ef636323 117 #define EXT_SENS_DATA_10 0x53
jjokocha 2:0cf2ef636323 118 #define EXT_SENS_DATA_11 0x54
jjokocha 2:0cf2ef636323 119 #define EXT_SENS_DATA_12 0x55
jjokocha 2:0cf2ef636323 120 #define EXT_SENS_DATA_13 0x56
jjokocha 2:0cf2ef636323 121 #define EXT_SENS_DATA_14 0x57
jjokocha 2:0cf2ef636323 122 #define EXT_SENS_DATA_15 0x58
jjokocha 2:0cf2ef636323 123 #define EXT_SENS_DATA_16 0x59
jjokocha 2:0cf2ef636323 124 #define EXT_SENS_DATA_17 0x5A
jjokocha 2:0cf2ef636323 125 #define EXT_SENS_DATA_18 0x5B
jjokocha 2:0cf2ef636323 126 #define EXT_SENS_DATA_19 0x5C
jjokocha 2:0cf2ef636323 127 #define EXT_SENS_DATA_20 0x5D
jjokocha 2:0cf2ef636323 128 #define EXT_SENS_DATA_21 0x5E
jjokocha 2:0cf2ef636323 129 #define EXT_SENS_DATA_22 0x5F
jjokocha 2:0cf2ef636323 130 #define EXT_SENS_DATA_23 0x60
jjokocha 2:0cf2ef636323 131 #define MOT_DETECT_STATUS 0x61
jjokocha 2:0cf2ef636323 132 #define I2C_SLV0_DO 0x63
jjokocha 2:0cf2ef636323 133 #define I2C_SLV1_DO 0x64
jjokocha 2:0cf2ef636323 134 #define I2C_SLV2_DO 0x65
jjokocha 2:0cf2ef636323 135 #define I2C_SLV3_DO 0x66
jjokocha 2:0cf2ef636323 136 #define I2C_MST_DELAY_CTRL 0x67
jjokocha 2:0cf2ef636323 137 #define SIGNAL_PATH_RESET 0x68
jjokocha 2:0cf2ef636323 138 #define MOT_DETECT_CTRL 0x69
jjokocha 2:0cf2ef636323 139 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
jjokocha 2:0cf2ef636323 140 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
jjokocha 2:0cf2ef636323 141 #define PWR_MGMT_2 0x6C
jjokocha 2:0cf2ef636323 142 #define DMP_BANK 0x6D // Activates a specific bank in the DMP
jjokocha 2:0cf2ef636323 143 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
jjokocha 2:0cf2ef636323 144 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write
jjokocha 2:0cf2ef636323 145 #define DMP_REG_1 0x70
jjokocha 2:0cf2ef636323 146 #define DMP_REG_2 0x71
jjokocha 2:0cf2ef636323 147 #define FIFO_COUNTH 0x72
jjokocha 2:0cf2ef636323 148 #define FIFO_COUNTL 0x73
jjokocha 2:0cf2ef636323 149 #define FIFO_R_W 0x74
jjokocha 2:0cf2ef636323 150 #define WHO_AM_I_MPU9250 0x75 // Should return 0x71
jjokocha 2:0cf2ef636323 151 #define XA_OFFSET_H 0x77
jjokocha 2:0cf2ef636323 152 #define XA_OFFSET_L 0x78
jjokocha 2:0cf2ef636323 153 #define YA_OFFSET_H 0x7A
jjokocha 2:0cf2ef636323 154 #define YA_OFFSET_L 0x7B
jjokocha 2:0cf2ef636323 155 #define ZA_OFFSET_H 0x7D
jjokocha 2:0cf2ef636323 156 #define ZA_OFFSET_L 0x7E
jjokocha 2:0cf2ef636323 157
jjokocha 2:0cf2ef636323 158 // Using the MSENSR-9250 breakout board, ADO is set to 0
jjokocha 2:0cf2ef636323 159 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
jjokocha 2:0cf2ef636323 160 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
jjokocha 2:0cf2ef636323 161 #define ADO 0
jjokocha 2:0cf2ef636323 162 #if ADO
jjokocha 2:0cf2ef636323 163 #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1
jjokocha 2:0cf2ef636323 164 #else
jjokocha 2:0cf2ef636323 165 #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0
jjokocha 2:0cf2ef636323 166 #endif
jjokocha 2:0cf2ef636323 167
jjokocha 2:0cf2ef636323 168 // Set initial input parameters
jjokocha 2:0cf2ef636323 169 enum Ascale {
jjokocha 2:0cf2ef636323 170 AFS_2G = 0,
jjokocha 2:0cf2ef636323 171 AFS_4G,
jjokocha 2:0cf2ef636323 172 AFS_8G,
jjokocha 2:0cf2ef636323 173 AFS_16G
jjokocha 2:0cf2ef636323 174 };
jjokocha 2:0cf2ef636323 175
jjokocha 2:0cf2ef636323 176 enum Gscale {
jjokocha 2:0cf2ef636323 177 GFS_250DPS = 0,
jjokocha 2:0cf2ef636323 178 GFS_500DPS,
jjokocha 2:0cf2ef636323 179 GFS_1000DPS,
jjokocha 2:0cf2ef636323 180 GFS_2000DPS
jjokocha 2:0cf2ef636323 181 };
jjokocha 2:0cf2ef636323 182
jjokocha 2:0cf2ef636323 183 enum Mscale {
jjokocha 2:0cf2ef636323 184 MFS_14BITS = 0, // 0.6 mG per LSB
jjokocha 2:0cf2ef636323 185 MFS_16BITS // 0.15 mG per LSB
jjokocha 2:0cf2ef636323 186 };
jjokocha 2:0cf2ef636323 187
jjokocha 2:0cf2ef636323 188 uint8_t Ascale = AFS_2G; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
jjokocha 2:0cf2ef636323 189 uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
jjokocha 2:0cf2ef636323 190 uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
jjokocha 2:0cf2ef636323 191 uint8_t Mmode = 0x06; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
jjokocha 2:0cf2ef636323 192 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
jjokocha 2:0cf2ef636323 193
jjokocha 2:0cf2ef636323 194 //Set up I2C, (SDA,SCL)
jjokocha 2:0cf2ef636323 195 //#define I2C_SDA PB_7
jjokocha 2:0cf2ef636323 196 //#define I2C_SCL PB_6
jjokocha 2:0cf2ef636323 197 I2C i2c(I2C_SDA, I2C_SCL);
jjokocha 2:0cf2ef636323 198
jjokocha 2:0cf2ef636323 199 DigitalOut myled(LED1);
jjokocha 2:0cf2ef636323 200
jjokocha 2:0cf2ef636323 201 // Pin definitions
jjokocha 2:0cf2ef636323 202 int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins
jjokocha 2:0cf2ef636323 203
jjokocha 2:0cf2ef636323 204 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
jjokocha 2:0cf2ef636323 205 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
jjokocha 2:0cf2ef636323 206 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
jjokocha 2:0cf2ef636323 207 float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias
jjokocha 2:0cf2ef636323 208 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
jjokocha 2:0cf2ef636323 209 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
jjokocha 2:0cf2ef636323 210 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
jjokocha 2:0cf2ef636323 211 float temperature;
jjokocha 2:0cf2ef636323 212 float SelfTest[6];
jjokocha 2:0cf2ef636323 213
jjokocha 2:0cf2ef636323 214
jjokocha 2:0cf2ef636323 215
jjokocha 2:0cf2ef636323 216 // parameters for 6 DoF sensor fusion calculations
jjokocha 2:0cf2ef636323 217 float PI = 3.14159265358979323846f;
jjokocha 2:0cf2ef636323 218 float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
jjokocha 2:0cf2ef636323 219 float beta = sqrtf(3.0f / 4.0f) * GyroMeasError; // compute beta
jjokocha 2:0cf2ef636323 220 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
jjokocha 2:0cf2ef636323 221 float zeta = sqrtf(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
jjokocha 2:0cf2ef636323 222 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
jjokocha 2:0cf2ef636323 223 #define Ki 0.0f
jjokocha 2:0cf2ef636323 224
jjokocha 2:0cf2ef636323 225 float pitch, yaw, roll;
jjokocha 2:0cf2ef636323 226 float deltat = 0.0f; // integration interval for both filter schemes
jjokocha 2:0cf2ef636323 227 int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval
jjokocha 2:0cf2ef636323 228 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
jjokocha 2:0cf2ef636323 229 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
jjokocha 2:0cf2ef636323 230
jjokocha 2:0cf2ef636323 231 class MPU9250 {
jjokocha 2:0cf2ef636323 232
jjokocha 2:0cf2ef636323 233 protected:
jjokocha 2:0cf2ef636323 234
jjokocha 2:0cf2ef636323 235 public:
jjokocha 2:0cf2ef636323 236 //===================================================================================================================
jjokocha 2:0cf2ef636323 237 //====== Set of useful function to access acceleratio, gyroscope, and temperature data
jjokocha 2:0cf2ef636323 238 //===================================================================================================================
jjokocha 2:0cf2ef636323 239
jjokocha 2:0cf2ef636323 240 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
jjokocha 2:0cf2ef636323 241 {
jjokocha 2:0cf2ef636323 242 char data_write[2];
jjokocha 2:0cf2ef636323 243 data_write[0] = subAddress;
jjokocha 2:0cf2ef636323 244 data_write[1] = data;
jjokocha 2:0cf2ef636323 245 i2c.write(address, data_write, 2, 0);
jjokocha 2:0cf2ef636323 246 }
jjokocha 2:0cf2ef636323 247
jjokocha 2:0cf2ef636323 248 char readByte(uint8_t address, uint8_t subAddress)
jjokocha 2:0cf2ef636323 249 {
jjokocha 2:0cf2ef636323 250 char data[1]; // `data` will store the register data
jjokocha 2:0cf2ef636323 251 char data_write[1];
jjokocha 2:0cf2ef636323 252 data_write[0] = subAddress;
jjokocha 2:0cf2ef636323 253 i2c.write(address, data_write, 1, 1); // no stop
jjokocha 2:0cf2ef636323 254 i2c.read(address, data, 1, 0);
jjokocha 2:0cf2ef636323 255 return data[0];
jjokocha 2:0cf2ef636323 256 }
jjokocha 2:0cf2ef636323 257
jjokocha 2:0cf2ef636323 258 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
jjokocha 2:0cf2ef636323 259 {
jjokocha 2:0cf2ef636323 260 char data[14];
jjokocha 2:0cf2ef636323 261 char data_write[1];
jjokocha 2:0cf2ef636323 262 data_write[0] = subAddress;
jjokocha 2:0cf2ef636323 263 i2c.write(address, data_write, 1, 1); // no stop
jjokocha 2:0cf2ef636323 264 i2c.read(address, data, count, 0);
jjokocha 2:0cf2ef636323 265 for(int ii = 0; ii < count; ii++) {
jjokocha 2:0cf2ef636323 266 dest[ii] = data[ii];
jjokocha 2:0cf2ef636323 267 }
jjokocha 2:0cf2ef636323 268 }
jjokocha 2:0cf2ef636323 269
jjokocha 2:0cf2ef636323 270
jjokocha 2:0cf2ef636323 271 void getMres() {
jjokocha 2:0cf2ef636323 272 switch (Mscale)
jjokocha 2:0cf2ef636323 273 {
jjokocha 2:0cf2ef636323 274 // Possible magnetometer scales (and their register bit settings) are:
jjokocha 2:0cf2ef636323 275 // 14 bit resolution (0) and 16 bit resolution (1)
jjokocha 2:0cf2ef636323 276 case MFS_14BITS:
jjokocha 2:0cf2ef636323 277 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
jjokocha 2:0cf2ef636323 278 break;
jjokocha 2:0cf2ef636323 279 case MFS_16BITS:
jjokocha 2:0cf2ef636323 280 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
jjokocha 2:0cf2ef636323 281 break;
jjokocha 2:0cf2ef636323 282 }
jjokocha 2:0cf2ef636323 283 }
jjokocha 2:0cf2ef636323 284
jjokocha 2:0cf2ef636323 285
jjokocha 2:0cf2ef636323 286 void getGres() {
jjokocha 2:0cf2ef636323 287 switch (Gscale)
jjokocha 2:0cf2ef636323 288 {
jjokocha 2:0cf2ef636323 289 // Possible gyro scales (and their register bit settings) are:
jjokocha 2:0cf2ef636323 290 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
jjokocha 2:0cf2ef636323 291 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
jjokocha 2:0cf2ef636323 292 case GFS_250DPS:
jjokocha 2:0cf2ef636323 293 gRes = 250.0/32768.0;
jjokocha 2:0cf2ef636323 294 break;
jjokocha 2:0cf2ef636323 295 case GFS_500DPS:
jjokocha 2:0cf2ef636323 296 gRes = 500.0/32768.0;
jjokocha 2:0cf2ef636323 297 break;
jjokocha 2:0cf2ef636323 298 case GFS_1000DPS:
jjokocha 2:0cf2ef636323 299 gRes = 1000.0/32768.0;
jjokocha 2:0cf2ef636323 300 break;
jjokocha 2:0cf2ef636323 301 case GFS_2000DPS:
jjokocha 2:0cf2ef636323 302 gRes = 2000.0/32768.0;
jjokocha 2:0cf2ef636323 303 break;
jjokocha 2:0cf2ef636323 304 }
jjokocha 2:0cf2ef636323 305 }
jjokocha 2:0cf2ef636323 306
jjokocha 2:0cf2ef636323 307
jjokocha 2:0cf2ef636323 308 void getAres() {
jjokocha 2:0cf2ef636323 309 switch (Ascale)
jjokocha 2:0cf2ef636323 310 {
jjokocha 2:0cf2ef636323 311 // Possible accelerometer scales (and their register bit settings) are:
jjokocha 2:0cf2ef636323 312 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
jjokocha 2:0cf2ef636323 313 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
jjokocha 2:0cf2ef636323 314 case AFS_2G:
jjokocha 2:0cf2ef636323 315 aRes = 2.0/32768.0;
jjokocha 2:0cf2ef636323 316 break;
jjokocha 2:0cf2ef636323 317 case AFS_4G:
jjokocha 2:0cf2ef636323 318 aRes = 4.0/32768.0;
jjokocha 2:0cf2ef636323 319 break;
jjokocha 2:0cf2ef636323 320 case AFS_8G:
jjokocha 2:0cf2ef636323 321 aRes = 8.0/32768.0;
jjokocha 2:0cf2ef636323 322 break;
jjokocha 2:0cf2ef636323 323 case AFS_16G:
jjokocha 2:0cf2ef636323 324 aRes = 16.0/32768.0;
jjokocha 2:0cf2ef636323 325 break;
jjokocha 2:0cf2ef636323 326 }
jjokocha 2:0cf2ef636323 327 }
jjokocha 2:0cf2ef636323 328
jjokocha 2:0cf2ef636323 329
jjokocha 2:0cf2ef636323 330 void readAccelData(int16_t * destination)
jjokocha 2:0cf2ef636323 331 {
jjokocha 2:0cf2ef636323 332 uint8_t rawData[6]; // x/y/z accel register data stored here
jjokocha 2:0cf2ef636323 333 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
jjokocha 2:0cf2ef636323 334 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 335 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
jjokocha 2:0cf2ef636323 336 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
jjokocha 2:0cf2ef636323 337 }
jjokocha 2:0cf2ef636323 338
jjokocha 2:0cf2ef636323 339 void readGyroData(int16_t * destination)
jjokocha 2:0cf2ef636323 340 {
jjokocha 2:0cf2ef636323 341 uint8_t rawData[6]; // x/y/z gyro register data stored here
jjokocha 2:0cf2ef636323 342 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
jjokocha 2:0cf2ef636323 343 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 344 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
jjokocha 2:0cf2ef636323 345 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
jjokocha 2:0cf2ef636323 346 }
jjokocha 2:0cf2ef636323 347
jjokocha 2:0cf2ef636323 348 void readMagData(int16_t * destination)
jjokocha 2:0cf2ef636323 349 {
jjokocha 2:0cf2ef636323 350 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
jjokocha 2:0cf2ef636323 351 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
jjokocha 2:0cf2ef636323 352 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
jjokocha 2:0cf2ef636323 353 uint8_t c = rawData[6]; // End data read by reading ST2 register
jjokocha 2:0cf2ef636323 354 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
jjokocha 2:0cf2ef636323 355 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 356 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
jjokocha 2:0cf2ef636323 357 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
jjokocha 2:0cf2ef636323 358 }
jjokocha 2:0cf2ef636323 359 }
jjokocha 2:0cf2ef636323 360 }
jjokocha 2:0cf2ef636323 361
jjokocha 2:0cf2ef636323 362 int16_t readTempData()
jjokocha 2:0cf2ef636323 363 {
jjokocha 2:0cf2ef636323 364 uint8_t rawData[2]; // x/y/z gyro register data stored here
jjokocha 2:0cf2ef636323 365 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
jjokocha 2:0cf2ef636323 366 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
jjokocha 2:0cf2ef636323 367 }
jjokocha 2:0cf2ef636323 368
jjokocha 2:0cf2ef636323 369
jjokocha 2:0cf2ef636323 370 void resetMPU9250() {
jjokocha 2:0cf2ef636323 371 // reset device
jjokocha 2:0cf2ef636323 372 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
jjokocha 2:0cf2ef636323 373 wait(0.1);
jjokocha 2:0cf2ef636323 374 }
jjokocha 2:0cf2ef636323 375
jjokocha 2:0cf2ef636323 376 void initAK8963(float * destination)
jjokocha 2:0cf2ef636323 377 {
jjokocha 2:0cf2ef636323 378 // First extract the factory calibration for each magnetometer axis
jjokocha 2:0cf2ef636323 379 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
jjokocha 2:0cf2ef636323 380 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
jjokocha 2:0cf2ef636323 381 wait(0.01);
jjokocha 2:0cf2ef636323 382 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
jjokocha 2:0cf2ef636323 383 wait(0.01);
jjokocha 2:0cf2ef636323 384 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
jjokocha 2:0cf2ef636323 385 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
jjokocha 2:0cf2ef636323 386 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
jjokocha 2:0cf2ef636323 387 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
jjokocha 2:0cf2ef636323 388 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
jjokocha 2:0cf2ef636323 389 wait(0.01);
jjokocha 2:0cf2ef636323 390 // Configure the magnetometer for continuous read and highest resolution
jjokocha 2:0cf2ef636323 391 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
jjokocha 2:0cf2ef636323 392 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
jjokocha 2:0cf2ef636323 393 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
jjokocha 2:0cf2ef636323 394 wait(0.01);
jjokocha 2:0cf2ef636323 395 }
jjokocha 2:0cf2ef636323 396
jjokocha 2:0cf2ef636323 397
jjokocha 2:0cf2ef636323 398 void initMPU9250()
jjokocha 2:0cf2ef636323 399 {
jjokocha 2:0cf2ef636323 400 // Initialize MPU9250 device
jjokocha 2:0cf2ef636323 401 // wake up device
jjokocha 2:0cf2ef636323 402 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
jjokocha 2:0cf2ef636323 403 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
jjokocha 2:0cf2ef636323 404
jjokocha 2:0cf2ef636323 405 // get stable time source
jjokocha 2:0cf2ef636323 406 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
jjokocha 2:0cf2ef636323 407
jjokocha 2:0cf2ef636323 408 // Configure Gyro and Accelerometer
jjokocha 2:0cf2ef636323 409 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
jjokocha 2:0cf2ef636323 410 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
jjokocha 2:0cf2ef636323 411 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
jjokocha 2:0cf2ef636323 412 writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
jjokocha 2:0cf2ef636323 413
jjokocha 2:0cf2ef636323 414 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
jjokocha 2:0cf2ef636323 415 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
jjokocha 2:0cf2ef636323 416
jjokocha 2:0cf2ef636323 417 // Set gyroscope full scale range
jjokocha 2:0cf2ef636323 418 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
jjokocha 2:0cf2ef636323 419 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
jjokocha 2:0cf2ef636323 420 // c = c & ~0xE0; // Clear self-test bits [7:5]
jjokocha 2:0cf2ef636323 421 c = c & ~0x02; // Clear Fchoice bits [1:0]
jjokocha 2:0cf2ef636323 422 c = c & ~0x18; // Clear AFS bits [4:3]
jjokocha 2:0cf2ef636323 423 c = c | Gscale << 3; // Set full scale range for the gyro
jjokocha 2:0cf2ef636323 424 // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
jjokocha 2:0cf2ef636323 425 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
jjokocha 2:0cf2ef636323 426
jjokocha 2:0cf2ef636323 427 // Set accelerometer full-scale range configuration
jjokocha 2:0cf2ef636323 428 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
jjokocha 2:0cf2ef636323 429 // c = c & ~0xE0; // Clear self-test bits [7:5]
jjokocha 2:0cf2ef636323 430 c = c & ~0x18; // Clear AFS bits [4:3]
jjokocha 2:0cf2ef636323 431 c = c | Ascale << 3; // Set full scale range for the accelerometer
jjokocha 2:0cf2ef636323 432 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
jjokocha 2:0cf2ef636323 433
jjokocha 2:0cf2ef636323 434 // Set accelerometer sample rate configuration
jjokocha 2:0cf2ef636323 435 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
jjokocha 2:0cf2ef636323 436 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
jjokocha 2:0cf2ef636323 437 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
jjokocha 2:0cf2ef636323 438 c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
jjokocha 2:0cf2ef636323 439 c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
jjokocha 2:0cf2ef636323 440 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
jjokocha 2:0cf2ef636323 441
jjokocha 2:0cf2ef636323 442 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
jjokocha 2:0cf2ef636323 443 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
jjokocha 2:0cf2ef636323 444
jjokocha 2:0cf2ef636323 445 // Configure Interrupts and Bypass Enable
jjokocha 2:0cf2ef636323 446 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
jjokocha 2:0cf2ef636323 447 // can join the I2C bus and all can be controlled by the Arduino as master
jjokocha 2:0cf2ef636323 448
jjokocha 2:0cf2ef636323 449 #if USE_ISR
jjokocha 2:0cf2ef636323 450 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12); // INT is 50 microsecond pulse and any read to clear
jjokocha 2:0cf2ef636323 451 #else
jjokocha 2:0cf2ef636323 452 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
jjokocha 2:0cf2ef636323 453 #endif
jjokocha 2:0cf2ef636323 454 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
jjokocha 2:0cf2ef636323 455 }
jjokocha 2:0cf2ef636323 456
jjokocha 2:0cf2ef636323 457 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
jjokocha 2:0cf2ef636323 458 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
jjokocha 2:0cf2ef636323 459 void calibrateMPU9250(float * dest1, float * dest2)
jjokocha 2:0cf2ef636323 460 {
jjokocha 2:0cf2ef636323 461 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
jjokocha 2:0cf2ef636323 462 uint16_t ii, packet_count, fifo_count;
jjokocha 2:0cf2ef636323 463 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
jjokocha 2:0cf2ef636323 464
jjokocha 2:0cf2ef636323 465 // reset device, reset all registers, clear gyro and accelerometer bias registers
jjokocha 2:0cf2ef636323 466 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
jjokocha 2:0cf2ef636323 467 wait(0.1);
jjokocha 2:0cf2ef636323 468
jjokocha 2:0cf2ef636323 469 // get stable time source
jjokocha 2:0cf2ef636323 470 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
jjokocha 2:0cf2ef636323 471 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
jjokocha 2:0cf2ef636323 472 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
jjokocha 2:0cf2ef636323 473 wait(0.2);
jjokocha 2:0cf2ef636323 474
jjokocha 2:0cf2ef636323 475 // Configure device for bias calculation
jjokocha 2:0cf2ef636323 476 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
jjokocha 2:0cf2ef636323 477 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
jjokocha 2:0cf2ef636323 478 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
jjokocha 2:0cf2ef636323 479 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
jjokocha 2:0cf2ef636323 480 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
jjokocha 2:0cf2ef636323 481 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
jjokocha 2:0cf2ef636323 482 wait(0.015);
jjokocha 2:0cf2ef636323 483
jjokocha 2:0cf2ef636323 484 // Configure MPU9250 gyro and accelerometer for bias calculation
jjokocha 2:0cf2ef636323 485 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
jjokocha 2:0cf2ef636323 486 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
jjokocha 2:0cf2ef636323 487 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
jjokocha 2:0cf2ef636323 488 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
jjokocha 2:0cf2ef636323 489
jjokocha 2:0cf2ef636323 490 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
jjokocha 2:0cf2ef636323 491 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
jjokocha 2:0cf2ef636323 492
jjokocha 2:0cf2ef636323 493 // Configure FIFO to capture accelerometer and gyro data for bias calculation
jjokocha 2:0cf2ef636323 494 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
jjokocha 2:0cf2ef636323 495 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
jjokocha 2:0cf2ef636323 496 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
jjokocha 2:0cf2ef636323 497
jjokocha 2:0cf2ef636323 498 // At end of sample accumulation, turn off FIFO sensor read
jjokocha 2:0cf2ef636323 499 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
jjokocha 2:0cf2ef636323 500 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
jjokocha 2:0cf2ef636323 501 fifo_count = ((uint16_t)data[0] << 8) | data[1];
jjokocha 2:0cf2ef636323 502 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
jjokocha 2:0cf2ef636323 503
jjokocha 2:0cf2ef636323 504 for (ii = 0; ii < packet_count; ii++) {
jjokocha 2:0cf2ef636323 505 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
jjokocha 2:0cf2ef636323 506 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
jjokocha 2:0cf2ef636323 507 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
jjokocha 2:0cf2ef636323 508 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
jjokocha 2:0cf2ef636323 509 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
jjokocha 2:0cf2ef636323 510 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
jjokocha 2:0cf2ef636323 511 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
jjokocha 2:0cf2ef636323 512 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
jjokocha 2:0cf2ef636323 513
jjokocha 2:0cf2ef636323 514 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
jjokocha 2:0cf2ef636323 515 accel_bias[1] += (int32_t) accel_temp[1];
jjokocha 2:0cf2ef636323 516 accel_bias[2] += (int32_t) accel_temp[2];
jjokocha 2:0cf2ef636323 517 gyro_bias[0] += (int32_t) gyro_temp[0];
jjokocha 2:0cf2ef636323 518 gyro_bias[1] += (int32_t) gyro_temp[1];
jjokocha 2:0cf2ef636323 519 gyro_bias[2] += (int32_t) gyro_temp[2];
jjokocha 2:0cf2ef636323 520
jjokocha 2:0cf2ef636323 521 }
jjokocha 2:0cf2ef636323 522 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
jjokocha 2:0cf2ef636323 523 accel_bias[1] /= (int32_t) packet_count;
jjokocha 2:0cf2ef636323 524 accel_bias[2] /= (int32_t) packet_count;
jjokocha 2:0cf2ef636323 525 gyro_bias[0] /= (int32_t) packet_count;
jjokocha 2:0cf2ef636323 526 gyro_bias[1] /= (int32_t) packet_count;
jjokocha 2:0cf2ef636323 527 gyro_bias[2] /= (int32_t) packet_count;
jjokocha 2:0cf2ef636323 528
jjokocha 2:0cf2ef636323 529 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
jjokocha 2:0cf2ef636323 530 else {accel_bias[2] += (int32_t) accelsensitivity;}
jjokocha 2:0cf2ef636323 531
jjokocha 2:0cf2ef636323 532 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
jjokocha 2:0cf2ef636323 533 data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
jjokocha 2:0cf2ef636323 534 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
jjokocha 2:0cf2ef636323 535 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
jjokocha 2:0cf2ef636323 536 data[3] = (-gyro_bias[1]/4) & 0xFF;
jjokocha 2:0cf2ef636323 537 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
jjokocha 2:0cf2ef636323 538 data[5] = (-gyro_bias[2]/4) & 0xFF;
jjokocha 2:0cf2ef636323 539
jjokocha 2:0cf2ef636323 540 /// Push gyro biases to hardware registers
jjokocha 2:0cf2ef636323 541 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
jjokocha 2:0cf2ef636323 542 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
jjokocha 2:0cf2ef636323 543 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
jjokocha 2:0cf2ef636323 544 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
jjokocha 2:0cf2ef636323 545 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
jjokocha 2:0cf2ef636323 546 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
jjokocha 2:0cf2ef636323 547 */
jjokocha 2:0cf2ef636323 548 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
jjokocha 2:0cf2ef636323 549 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
jjokocha 2:0cf2ef636323 550 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
jjokocha 2:0cf2ef636323 551
jjokocha 2:0cf2ef636323 552 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
jjokocha 2:0cf2ef636323 553 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
jjokocha 2:0cf2ef636323 554 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
jjokocha 2:0cf2ef636323 555 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
jjokocha 2:0cf2ef636323 556 // the accelerometer biases calculated above must be divided by 8.
jjokocha 2:0cf2ef636323 557
jjokocha 2:0cf2ef636323 558 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
jjokocha 2:0cf2ef636323 559 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
jjokocha 2:0cf2ef636323 560 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
jjokocha 2:0cf2ef636323 561 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
jjokocha 2:0cf2ef636323 562 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
jjokocha 2:0cf2ef636323 563 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
jjokocha 2:0cf2ef636323 564 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
jjokocha 2:0cf2ef636323 565
jjokocha 2:0cf2ef636323 566 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
jjokocha 2:0cf2ef636323 567 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
jjokocha 2:0cf2ef636323 568
jjokocha 2:0cf2ef636323 569 for(ii = 0; ii < 3; ii++) {
jjokocha 2:0cf2ef636323 570 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
jjokocha 2:0cf2ef636323 571 }
jjokocha 2:0cf2ef636323 572
jjokocha 2:0cf2ef636323 573 // Construct total accelerometer bias, including calculated average accelerometer bias from above
jjokocha 2:0cf2ef636323 574 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
jjokocha 2:0cf2ef636323 575 accel_bias_reg[1] -= (accel_bias[1]/8);
jjokocha 2:0cf2ef636323 576 accel_bias_reg[2] -= (accel_bias[2]/8);
jjokocha 2:0cf2ef636323 577
jjokocha 2:0cf2ef636323 578 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
jjokocha 2:0cf2ef636323 579 data[1] = (accel_bias_reg[0]) & 0xFF;
jjokocha 2:0cf2ef636323 580 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
jjokocha 2:0cf2ef636323 581 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
jjokocha 2:0cf2ef636323 582 data[3] = (accel_bias_reg[1]) & 0xFF;
jjokocha 2:0cf2ef636323 583 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
jjokocha 2:0cf2ef636323 584 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
jjokocha 2:0cf2ef636323 585 data[5] = (accel_bias_reg[2]) & 0xFF;
jjokocha 2:0cf2ef636323 586 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
jjokocha 2:0cf2ef636323 587
jjokocha 2:0cf2ef636323 588 // Apparently this is not working for the acceleration biases in the MPU-9250
jjokocha 2:0cf2ef636323 589 // Are we handling the temperature correction bit properly?
jjokocha 2:0cf2ef636323 590 // Push accelerometer biases to hardware registers
jjokocha 2:0cf2ef636323 591 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
jjokocha 2:0cf2ef636323 592 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
jjokocha 2:0cf2ef636323 593 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
jjokocha 2:0cf2ef636323 594 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
jjokocha 2:0cf2ef636323 595 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
jjokocha 2:0cf2ef636323 596 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
jjokocha 2:0cf2ef636323 597 */
jjokocha 2:0cf2ef636323 598 // Output scaled accelerometer biases for manual subtraction in the main program
jjokocha 2:0cf2ef636323 599 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
jjokocha 2:0cf2ef636323 600 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
jjokocha 2:0cf2ef636323 601 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
jjokocha 2:0cf2ef636323 602 }
jjokocha 2:0cf2ef636323 603
jjokocha 2:0cf2ef636323 604
jjokocha 2:0cf2ef636323 605 // Accelerometer and gyroscope self test; check calibration wrt factory settings
jjokocha 2:0cf2ef636323 606 void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
jjokocha 2:0cf2ef636323 607 {
jjokocha 2:0cf2ef636323 608 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
jjokocha 2:0cf2ef636323 609 uint8_t selfTest[6];
jjokocha 2:0cf2ef636323 610 int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
jjokocha 2:0cf2ef636323 611 float factoryTrim[6];
jjokocha 2:0cf2ef636323 612 uint8_t FS = 0;
jjokocha 2:0cf2ef636323 613
jjokocha 2:0cf2ef636323 614 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
jjokocha 2:0cf2ef636323 615 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
jjokocha 2:0cf2ef636323 616 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
jjokocha 2:0cf2ef636323 617 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
jjokocha 2:0cf2ef636323 618 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
jjokocha 2:0cf2ef636323 619
jjokocha 2:0cf2ef636323 620 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
jjokocha 2:0cf2ef636323 621
jjokocha 2:0cf2ef636323 622 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
jjokocha 2:0cf2ef636323 623 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 624 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
jjokocha 2:0cf2ef636323 625 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
jjokocha 2:0cf2ef636323 626
jjokocha 2:0cf2ef636323 627 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
jjokocha 2:0cf2ef636323 628 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 629 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
jjokocha 2:0cf2ef636323 630 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
jjokocha 2:0cf2ef636323 631 }
jjokocha 2:0cf2ef636323 632
jjokocha 2:0cf2ef636323 633 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
jjokocha 2:0cf2ef636323 634 aAvg[ii] /= 200;
jjokocha 2:0cf2ef636323 635 gAvg[ii] /= 200;
jjokocha 2:0cf2ef636323 636 }
jjokocha 2:0cf2ef636323 637
jjokocha 2:0cf2ef636323 638 // Configure the accelerometer for self-test
jjokocha 2:0cf2ef636323 639 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
jjokocha 2:0cf2ef636323 640 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
jjokocha 2:0cf2ef636323 641 wait(0.025); // Delay a while to let the device stabilize
jjokocha 2:0cf2ef636323 642
jjokocha 2:0cf2ef636323 643 for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
jjokocha 2:0cf2ef636323 644
jjokocha 2:0cf2ef636323 645 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
jjokocha 2:0cf2ef636323 646 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 647 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
jjokocha 2:0cf2ef636323 648 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
jjokocha 2:0cf2ef636323 649
jjokocha 2:0cf2ef636323 650 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
jjokocha 2:0cf2ef636323 651 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
jjokocha 2:0cf2ef636323 652 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
jjokocha 2:0cf2ef636323 653 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
jjokocha 2:0cf2ef636323 654 }
jjokocha 2:0cf2ef636323 655
jjokocha 2:0cf2ef636323 656 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
jjokocha 2:0cf2ef636323 657 aSTAvg[ii] /= 200;
jjokocha 2:0cf2ef636323 658 gSTAvg[ii] /= 200;
jjokocha 2:0cf2ef636323 659 }
jjokocha 2:0cf2ef636323 660
jjokocha 2:0cf2ef636323 661 // Configure the gyro and accelerometer for normal operation
jjokocha 2:0cf2ef636323 662 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
jjokocha 2:0cf2ef636323 663 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
jjokocha 2:0cf2ef636323 664 wait(0.025); // Delay a while to let the device stabilize
jjokocha 2:0cf2ef636323 665
jjokocha 2:0cf2ef636323 666 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
jjokocha 2:0cf2ef636323 667 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
jjokocha 2:0cf2ef636323 668 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
jjokocha 2:0cf2ef636323 669 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
jjokocha 2:0cf2ef636323 670 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
jjokocha 2:0cf2ef636323 671 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
jjokocha 2:0cf2ef636323 672 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
jjokocha 2:0cf2ef636323 673
jjokocha 2:0cf2ef636323 674 // Retrieve factory self-test value from self-test code reads
jjokocha 2:0cf2ef636323 675 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
jjokocha 2:0cf2ef636323 676 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
jjokocha 2:0cf2ef636323 677 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
jjokocha 2:0cf2ef636323 678 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
jjokocha 2:0cf2ef636323 679 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
jjokocha 2:0cf2ef636323 680 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
jjokocha 2:0cf2ef636323 681
jjokocha 2:0cf2ef636323 682 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
jjokocha 2:0cf2ef636323 683 // To get percent, must multiply by 100
jjokocha 2:0cf2ef636323 684 for (int i = 0; i < 3; i++) {
jjokocha 2:0cf2ef636323 685 destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.; // Report percent differences
jjokocha 2:0cf2ef636323 686 destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.; // Report percent differences
jjokocha 2:0cf2ef636323 687 }
jjokocha 2:0cf2ef636323 688
jjokocha 2:0cf2ef636323 689 }
jjokocha 2:0cf2ef636323 690
jjokocha 2:0cf2ef636323 691
jjokocha 2:0cf2ef636323 692
jjokocha 2:0cf2ef636323 693 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
jjokocha 2:0cf2ef636323 694 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
jjokocha 2:0cf2ef636323 695 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
jjokocha 2:0cf2ef636323 696 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
jjokocha 2:0cf2ef636323 697 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
jjokocha 2:0cf2ef636323 698 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
jjokocha 2:0cf2ef636323 699 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
jjokocha 2:0cf2ef636323 700 {
jjokocha 2:0cf2ef636323 701 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
jjokocha 2:0cf2ef636323 702 float norm;
jjokocha 2:0cf2ef636323 703 float hx, hy, _2bx, _2bz;
jjokocha 2:0cf2ef636323 704 float s1, s2, s3, s4;
jjokocha 2:0cf2ef636323 705 float qDot1, qDot2, qDot3, qDot4;
jjokocha 2:0cf2ef636323 706
jjokocha 2:0cf2ef636323 707 // Auxiliary variables to avoid repeated arithmetic
jjokocha 2:0cf2ef636323 708 float _2q1mx;
jjokocha 2:0cf2ef636323 709 float _2q1my;
jjokocha 2:0cf2ef636323 710 float _2q1mz;
jjokocha 2:0cf2ef636323 711 float _2q2mx;
jjokocha 2:0cf2ef636323 712 float _4bx;
jjokocha 2:0cf2ef636323 713 float _4bz;
jjokocha 2:0cf2ef636323 714 float _2q1 = 2.0f * q1;
jjokocha 2:0cf2ef636323 715 float _2q2 = 2.0f * q2;
jjokocha 2:0cf2ef636323 716 float _2q3 = 2.0f * q3;
jjokocha 2:0cf2ef636323 717 float _2q4 = 2.0f * q4;
jjokocha 2:0cf2ef636323 718 float _2q1q3 = 2.0f * q1 * q3;
jjokocha 2:0cf2ef636323 719 float _2q3q4 = 2.0f * q3 * q4;
jjokocha 2:0cf2ef636323 720 float q1q1 = q1 * q1;
jjokocha 2:0cf2ef636323 721 float q1q2 = q1 * q2;
jjokocha 2:0cf2ef636323 722 float q1q3 = q1 * q3;
jjokocha 2:0cf2ef636323 723 float q1q4 = q1 * q4;
jjokocha 2:0cf2ef636323 724 float q2q2 = q2 * q2;
jjokocha 2:0cf2ef636323 725 float q2q3 = q2 * q3;
jjokocha 2:0cf2ef636323 726 float q2q4 = q2 * q4;
jjokocha 2:0cf2ef636323 727 float q3q3 = q3 * q3;
jjokocha 2:0cf2ef636323 728 float q3q4 = q3 * q4;
jjokocha 2:0cf2ef636323 729 float q4q4 = q4 * q4;
jjokocha 2:0cf2ef636323 730
jjokocha 2:0cf2ef636323 731 // Normalise accelerometer measurement
jjokocha 2:0cf2ef636323 732 norm = sqrtf(ax * ax + ay * ay + az * az);
jjokocha 2:0cf2ef636323 733 if (norm == 0.0f) return; // handle NaN
jjokocha 2:0cf2ef636323 734 norm = 1.0f/norm;
jjokocha 2:0cf2ef636323 735 ax *= norm;
jjokocha 2:0cf2ef636323 736 ay *= norm;
jjokocha 2:0cf2ef636323 737 az *= norm;
jjokocha 2:0cf2ef636323 738
jjokocha 2:0cf2ef636323 739 // Normalise magnetometer measurement
jjokocha 2:0cf2ef636323 740 norm = sqrtf(mx * mx + my * my + mz * mz);
jjokocha 2:0cf2ef636323 741 if (norm == 0.0f) return; // handle NaN
jjokocha 2:0cf2ef636323 742 norm = 1.0f/norm;
jjokocha 2:0cf2ef636323 743 mx *= norm;
jjokocha 2:0cf2ef636323 744 my *= norm;
jjokocha 2:0cf2ef636323 745 mz *= norm;
jjokocha 2:0cf2ef636323 746
jjokocha 2:0cf2ef636323 747 // Reference direction of Earth's magnetic field
jjokocha 2:0cf2ef636323 748 _2q1mx = 2.0f * q1 * mx;
jjokocha 2:0cf2ef636323 749 _2q1my = 2.0f * q1 * my;
jjokocha 2:0cf2ef636323 750 _2q1mz = 2.0f * q1 * mz;
jjokocha 2:0cf2ef636323 751 _2q2mx = 2.0f * q2 * mx;
jjokocha 2:0cf2ef636323 752 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
jjokocha 2:0cf2ef636323 753 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
jjokocha 2:0cf2ef636323 754 _2bx = sqrtf(hx * hx + hy * hy);
jjokocha 2:0cf2ef636323 755 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
jjokocha 2:0cf2ef636323 756 _4bx = 2.0f * _2bx;
jjokocha 2:0cf2ef636323 757 _4bz = 2.0f * _2bz;
jjokocha 2:0cf2ef636323 758
jjokocha 2:0cf2ef636323 759 // Gradient decent algorithm corrective step
jjokocha 2:0cf2ef636323 760 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
jjokocha 2:0cf2ef636323 761 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
jjokocha 2:0cf2ef636323 762 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
jjokocha 2:0cf2ef636323 763 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
jjokocha 2:0cf2ef636323 764 norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
jjokocha 2:0cf2ef636323 765 norm = 1.0f/norm;
jjokocha 2:0cf2ef636323 766 s1 *= norm;
jjokocha 2:0cf2ef636323 767 s2 *= norm;
jjokocha 2:0cf2ef636323 768 s3 *= norm;
jjokocha 2:0cf2ef636323 769 s4 *= norm;
jjokocha 2:0cf2ef636323 770
jjokocha 2:0cf2ef636323 771 // Compute rate of change of quaternion
jjokocha 2:0cf2ef636323 772 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
jjokocha 2:0cf2ef636323 773 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
jjokocha 2:0cf2ef636323 774 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
jjokocha 2:0cf2ef636323 775 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
jjokocha 2:0cf2ef636323 776
jjokocha 2:0cf2ef636323 777 // Integrate to yield quaternion
jjokocha 2:0cf2ef636323 778 q1 += qDot1 * deltat;
jjokocha 2:0cf2ef636323 779 q2 += qDot2 * deltat;
jjokocha 2:0cf2ef636323 780 q3 += qDot3 * deltat;
jjokocha 2:0cf2ef636323 781 q4 += qDot4 * deltat;
jjokocha 2:0cf2ef636323 782 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
jjokocha 2:0cf2ef636323 783 norm = 1.0f/norm;
jjokocha 2:0cf2ef636323 784 q[0] = q1 * norm;
jjokocha 2:0cf2ef636323 785 q[1] = q2 * norm;
jjokocha 2:0cf2ef636323 786 q[2] = q3 * norm;
jjokocha 2:0cf2ef636323 787 q[3] = q4 * norm;
jjokocha 2:0cf2ef636323 788
jjokocha 2:0cf2ef636323 789 }
jjokocha 2:0cf2ef636323 790
jjokocha 2:0cf2ef636323 791
jjokocha 2:0cf2ef636323 792
jjokocha 2:0cf2ef636323 793 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
jjokocha 2:0cf2ef636323 794 // measured ones.
jjokocha 2:0cf2ef636323 795 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
jjokocha 2:0cf2ef636323 796 {
jjokocha 2:0cf2ef636323 797 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
jjokocha 2:0cf2ef636323 798 float norm;
jjokocha 2:0cf2ef636323 799 float hx, hy, bx, bz;
jjokocha 2:0cf2ef636323 800 float vx, vy, vz, wx, wy, wz;
jjokocha 2:0cf2ef636323 801 float ex, ey, ez;
jjokocha 2:0cf2ef636323 802 float pa, pb, pc;
jjokocha 2:0cf2ef636323 803
jjokocha 2:0cf2ef636323 804 // Auxiliary variables to avoid repeated arithmetic
jjokocha 2:0cf2ef636323 805 float q1q1 = q1 * q1;
jjokocha 2:0cf2ef636323 806 float q1q2 = q1 * q2;
jjokocha 2:0cf2ef636323 807 float q1q3 = q1 * q3;
jjokocha 2:0cf2ef636323 808 float q1q4 = q1 * q4;
jjokocha 2:0cf2ef636323 809 float q2q2 = q2 * q2;
jjokocha 2:0cf2ef636323 810 float q2q3 = q2 * q3;
jjokocha 2:0cf2ef636323 811 float q2q4 = q2 * q4;
jjokocha 2:0cf2ef636323 812 float q3q3 = q3 * q3;
jjokocha 2:0cf2ef636323 813 float q3q4 = q3 * q4;
jjokocha 2:0cf2ef636323 814 float q4q4 = q4 * q4;
jjokocha 2:0cf2ef636323 815
jjokocha 2:0cf2ef636323 816 // Normalise accelerometer measurement
jjokocha 2:0cf2ef636323 817 norm = sqrtf(ax * ax + ay * ay + az * az);
jjokocha 2:0cf2ef636323 818 if (norm == 0.0f) return; // handle NaN
jjokocha 2:0cf2ef636323 819 norm = 1.0f / norm; // use reciprocal for division
jjokocha 2:0cf2ef636323 820 ax *= norm;
jjokocha 2:0cf2ef636323 821 ay *= norm;
jjokocha 2:0cf2ef636323 822 az *= norm;
jjokocha 2:0cf2ef636323 823
jjokocha 2:0cf2ef636323 824 // Normalise magnetometer measurement
jjokocha 2:0cf2ef636323 825 norm = sqrtf(mx * mx + my * my + mz * mz);
jjokocha 2:0cf2ef636323 826 if (norm == 0.0f) return; // handle NaN
jjokocha 2:0cf2ef636323 827 norm = 1.0f / norm; // use reciprocal for division
jjokocha 2:0cf2ef636323 828 mx *= norm;
jjokocha 2:0cf2ef636323 829 my *= norm;
jjokocha 2:0cf2ef636323 830 mz *= norm;
jjokocha 2:0cf2ef636323 831
jjokocha 2:0cf2ef636323 832 // Reference direction of Earth's magnetic field
jjokocha 2:0cf2ef636323 833 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
jjokocha 2:0cf2ef636323 834 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
jjokocha 2:0cf2ef636323 835 bx = sqrtf((hx * hx) + (hy * hy));
jjokocha 2:0cf2ef636323 836 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
jjokocha 2:0cf2ef636323 837
jjokocha 2:0cf2ef636323 838 // Estimated direction of gravity and magnetic field
jjokocha 2:0cf2ef636323 839 vx = 2.0f * (q2q4 - q1q3);
jjokocha 2:0cf2ef636323 840 vy = 2.0f * (q1q2 + q3q4);
jjokocha 2:0cf2ef636323 841 vz = q1q1 - q2q2 - q3q3 + q4q4;
jjokocha 2:0cf2ef636323 842 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
jjokocha 2:0cf2ef636323 843 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
jjokocha 2:0cf2ef636323 844 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
jjokocha 2:0cf2ef636323 845
jjokocha 2:0cf2ef636323 846 // Error is cross product between estimated direction and measured direction of gravity
jjokocha 2:0cf2ef636323 847 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
jjokocha 2:0cf2ef636323 848 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
jjokocha 2:0cf2ef636323 849 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
jjokocha 2:0cf2ef636323 850 if (Ki > 0.0f)
jjokocha 2:0cf2ef636323 851 {
jjokocha 2:0cf2ef636323 852 eInt[0] += ex; // accumulate integral error
jjokocha 2:0cf2ef636323 853 eInt[1] += ey;
jjokocha 2:0cf2ef636323 854 eInt[2] += ez;
jjokocha 2:0cf2ef636323 855 }
jjokocha 2:0cf2ef636323 856 else
jjokocha 2:0cf2ef636323 857 {
jjokocha 2:0cf2ef636323 858 eInt[0] = 0.0f; // prevent integral wind up
jjokocha 2:0cf2ef636323 859 eInt[1] = 0.0f;
jjokocha 2:0cf2ef636323 860 eInt[2] = 0.0f;
jjokocha 2:0cf2ef636323 861 }
jjokocha 2:0cf2ef636323 862
jjokocha 2:0cf2ef636323 863 // Apply feedback terms
jjokocha 2:0cf2ef636323 864 gx = gx + Kp * ex + Ki * eInt[0];
jjokocha 2:0cf2ef636323 865 gy = gy + Kp * ey + Ki * eInt[1];
jjokocha 2:0cf2ef636323 866 gz = gz + Kp * ez + Ki * eInt[2];
jjokocha 2:0cf2ef636323 867
jjokocha 2:0cf2ef636323 868 // Integrate rate of change of quaternion
jjokocha 2:0cf2ef636323 869 pa = q2;
jjokocha 2:0cf2ef636323 870 pb = q3;
jjokocha 2:0cf2ef636323 871 pc = q4;
jjokocha 2:0cf2ef636323 872 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
jjokocha 2:0cf2ef636323 873 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
jjokocha 2:0cf2ef636323 874 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
jjokocha 2:0cf2ef636323 875 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
jjokocha 2:0cf2ef636323 876
jjokocha 2:0cf2ef636323 877 // Normalise quaternion
jjokocha 2:0cf2ef636323 878 norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
jjokocha 2:0cf2ef636323 879 norm = 1.0f / norm;
jjokocha 2:0cf2ef636323 880 q[0] = q1 * norm;
jjokocha 2:0cf2ef636323 881 q[1] = q2 * norm;
jjokocha 2:0cf2ef636323 882 q[2] = q3 * norm;
jjokocha 2:0cf2ef636323 883 q[3] = q4 * norm;
jjokocha 2:0cf2ef636323 884
jjokocha 2:0cf2ef636323 885 }
jjokocha 2:0cf2ef636323 886 };
jjokocha 2:0cf2ef636323 887 #endif