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@1:61bf659e4a1f, 2015-06-07 (annotated)

Committer:
xosuuu
Date:
Sun Jun 07 16:48:13 2015 +0000
Revision:
1:61bf659e4a1f
Parent:
0:1a6e8ffa801b
Child:
2:bb20d5091065
Sospecho que el I2C da tantos errores debido a que en el bucle infinito el programa hace continuas llamadas al I2C sin esperar. Por ello se ha querido probar incorporando dentro de bucle infinito waits de 5ms

Who changed what in which revision?

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