Josué Olmeda / Mbed 2 deprecated STM32L152withMPU-9250

This program use the STM32L152 developement board and the MPU-9250 9-axis InvenSense movement sensor. The communication between both devices is made through I2C serial interface. This algorithm calibrates and reads data from accelerometer, gyroscope, magnetometer and the internal temperature sensor. The lecture is made each time has a new meassured value (both gyro and accel data). A comunication with a computer is made using serial interface. The user can see the data measured with 1 second update rate.

Dependencies:   mbed

Dependents:   2_MPU9250_attitude

MPU9250.h@0:1a6e8ffa801b, 2015-04-30 (annotated)

Committer:
xosuuu
Date:
Thu Apr 30 11:22:00 2015 +0000
Revision:
0:1a6e8ffa801b
Child:
1:61bf659e4a1f
void getCompassOrientation()

Who changed what in which revision?

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