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@2:bb20d5091065, 2015-09-01 (annotated)

Committer:
xosuuu
Date:
Tue Sep 01 14:06:00 2015 +0000
Revision:
2:bb20d5091065
Parent:
1:61bf659e4a1f 
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.

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 2:bb20d5091065 221 float magn_x, magn_y;
xosuuu 0:1a6e8ffa801b 222
xosuuu 0:1a6e8ffa801b 223 int delt_t = 0; // used to control display output rate
xosuuu 0:1a6e8ffa801b 224 int count = 0; // used to control display output rate
xosuuu 0:1a6e8ffa801b 225
xosuuu 0:1a6e8ffa801b 226 // parameters for 6 DoF sensor fusion calculations
xosuuu 0:1a6e8ffa801b 227 float PI = 3.14159265358979323846f;
xosuuu 0:1a6e8ffa801b 228 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 229 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
xosuuu 0:1a6e8ffa801b 230 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
xosuuu 0:1a6e8ffa801b 231 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 232 #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 233 #define Ki 0.0f
xosuuu 0:1a6e8ffa801b 234
xosuuu 0:1a6e8ffa801b 235 float pitch, yaw, roll;
xosuuu 0:1a6e8ffa801b 236 float deltat = 0.0f; // integration interval for both filter schemes
xosuuu 0:1a6e8ffa801b 237 int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval
xosuuu 0:1a6e8ffa801b 238 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
xosuuu 0:1a6e8ffa801b 239 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
xosuuu 0:1a6e8ffa801b 240
xosuuu 0:1a6e8ffa801b 241 class MPU9250 {
xosuuu 0:1a6e8ffa801b 242
xosuuu 0:1a6e8ffa801b 243 protected:
xosuuu 0:1a6e8ffa801b 244
xosuuu 0:1a6e8ffa801b 245 public:
xosuuu 0:1a6e8ffa801b 246 //===================================================================================================================
xosuuu 0:1a6e8ffa801b 247 //====== Set of useful function to access acceleration, gyroscope, and temperature data
xosuuu 0:1a6e8ffa801b 248 //===================================================================================================================
xosuuu 0:1a6e8ffa801b 249
xosuuu 0:1a6e8ffa801b 250 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
xosuuu 0:1a6e8ffa801b 251 {
xosuuu 0:1a6e8ffa801b 252 char data_write[2];
xosuuu 0:1a6e8ffa801b 253 data_write[0] = subAddress;
xosuuu 0:1a6e8ffa801b 254 data_write[1] = data;
xosuuu 0:1a6e8ffa801b 255 I2Cstate = i2c.write(address, data_write, 2, 0);
xosuuu 0:1a6e8ffa801b 256 }
xosuuu 0:1a6e8ffa801b 257
xosuuu 0:1a6e8ffa801b 258 char readByte(uint8_t address, uint8_t subAddress)
xosuuu 0:1a6e8ffa801b 259 {
xosuuu 0:1a6e8ffa801b 260 char data[1]; // `data` will store the register data
xosuuu 0:1a6e8ffa801b 261 char data_write[1];
xosuuu 0:1a6e8ffa801b 262 data_write[0] = subAddress;
xosuuu 0:1a6e8ffa801b 263 I2Cstate = i2c.write(address, data_write, 1, 1); // no stop
xosuuu 2:bb20d5091065 264 I2Cstate = i2c.read(address, data, 1, 0);
xosuuu 0:1a6e8ffa801b 265 return data[0];
xosuuu 0:1a6e8ffa801b 266 }
xosuuu 0:1a6e8ffa801b 267
xosuuu 0:1a6e8ffa801b 268 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 269 {
xosuuu 0:1a6e8ffa801b 270 char data[14];
xosuuu 0:1a6e8ffa801b 271 char data_write[1];
xosuuu 0:1a6e8ffa801b 272 data_write[0] = subAddress;
xosuuu 0:1a6e8ffa801b 273 I2Cstate = i2c.write(address, data_write, 1, 1); // no stop
xosuuu 0:1a6e8ffa801b 274 I2Cstate = i2c.read(address, data, count, 0);
xosuuu 0:1a6e8ffa801b 275 for(int ii = 0; ii < count; ii++) {
xosuuu 0:1a6e8ffa801b 276 dest[ii] = data[ii];
xosuuu 0:1a6e8ffa801b 277 }
xosuuu 0:1a6e8ffa801b 278 }
xosuuu 0:1a6e8ffa801b 279
xosuuu 0:1a6e8ffa801b 280 void getMres() {
xosuuu 0:1a6e8ffa801b 281 switch (Mscale)
xosuuu 0:1a6e8ffa801b 282 {
xosuuu 0:1a6e8ffa801b 283 // Possible magnetometer scales (and their register bit settings) are:
xosuuu 0:1a6e8ffa801b 284 // 14 bit resolution (0) and 16 bit resolution (1)
xosuuu 0:1a6e8ffa801b 285 case MFS_14BITS:
xosuuu 0:1a6e8ffa801b 286 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
xosuuu 0:1a6e8ffa801b 287 break;
xosuuu 0:1a6e8ffa801b 288 case MFS_16BITS:
xosuuu 0:1a6e8ffa801b 289 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
xosuuu 0:1a6e8ffa801b 290 break;
xosuuu 0:1a6e8ffa801b 291 }
xosuuu 0:1a6e8ffa801b 292 }
xosuuu 0:1a6e8ffa801b 293
xosuuu 0:1a6e8ffa801b 294 void getGres() {
xosuuu 0:1a6e8ffa801b 295 switch (Gscale)
xosuuu 0:1a6e8ffa801b 296 {
xosuuu 0:1a6e8ffa801b 297 // Possible gyro scales (and their register bit settings) are:
xosuuu 0:1a6e8ffa801b 298 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
xosuuu 0:1a6e8ffa801b 299 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
xosuuu 0:1a6e8ffa801b 300 case GFS_250DPS:
xosuuu 0:1a6e8ffa801b 301 gRes = 250.0/32768.0;
xosuuu 0:1a6e8ffa801b 302 break;
xosuuu 0:1a6e8ffa801b 303 case GFS_500DPS:
xosuuu 0:1a6e8ffa801b 304 gRes = 500.0/32768.0;
xosuuu 0:1a6e8ffa801b 305 break;
xosuuu 0:1a6e8ffa801b 306 case GFS_1000DPS:
xosuuu 0:1a6e8ffa801b 307 gRes = 1000.0/32768.0;
xosuuu 0:1a6e8ffa801b 308 break;
xosuuu 0:1a6e8ffa801b 309 case GFS_2000DPS:
xosuuu 0:1a6e8ffa801b 310 gRes = 2000.0/32768.0;
xosuuu 0:1a6e8ffa801b 311 break;
xosuuu 0:1a6e8ffa801b 312 }
xosuuu 0:1a6e8ffa801b 313 }
xosuuu 0:1a6e8ffa801b 314
xosuuu 0:1a6e8ffa801b 315 void getAres() {
xosuuu 0:1a6e8ffa801b 316 switch (Ascale)
xosuuu 0:1a6e8ffa801b 317 {
xosuuu 0:1a6e8ffa801b 318 // Possible accelerometer scales (and their register bit settings) are:
xosuuu 0:1a6e8ffa801b 319 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
xosuuu 0:1a6e8ffa801b 320 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
xosuuu 0:1a6e8ffa801b 321 case AFS_2G:
xosuuu 0:1a6e8ffa801b 322 aRes = 2.0/32768.0;
xosuuu 0:1a6e8ffa801b 323 break;
xosuuu 0:1a6e8ffa801b 324 case AFS_4G:
xosuuu 0:1a6e8ffa801b 325 aRes = 4.0/32768.0;
xosuuu 0:1a6e8ffa801b 326 break;
xosuuu 0:1a6e8ffa801b 327 case AFS_8G:
xosuuu 0:1a6e8ffa801b 328 aRes = 8.0/32768.0;
xosuuu 0:1a6e8ffa801b 329 break;
xosuuu 0:1a6e8ffa801b 330 case AFS_16G:
xosuuu 0:1a6e8ffa801b 331 aRes = 16.0/32768.0;
xosuuu 0:1a6e8ffa801b 332 break;
xosuuu 0:1a6e8ffa801b 333 }
xosuuu 0:1a6e8ffa801b 334 }
xosuuu 0:1a6e8ffa801b 335
xosuuu 0:1a6e8ffa801b 336 void readAccelData(int16_t * destination){
xosuuu 0:1a6e8ffa801b 337
xosuuu 0:1a6e8ffa801b 338 uint8_t rawData[6]; // x/y/z accel register data stored here
xosuuu 0:1a6e8ffa801b 339 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers 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 readGyroData(int16_t * destination){
xosuuu 0:1a6e8ffa801b 346 uint8_t rawData[6]; // x/y/z gyro register data stored here
xosuuu 0:1a6e8ffa801b 347 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
xosuuu 0:1a6e8ffa801b 348 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 349 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
xosuuu 0:1a6e8ffa801b 350 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
xosuuu 0:1a6e8ffa801b 351 }
xosuuu 0:1a6e8ffa801b 352
xosuuu 0:1a6e8ffa801b 353 void readMagData(int16_t * destination){
xosuuu 0:1a6e8ffa801b 354 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 355 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
xosuuu 0:1a6e8ffa801b 356 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
xosuuu 0:1a6e8ffa801b 357 uint8_t c = rawData[6]; // End data read by reading ST2 register
xosuuu 0:1a6e8ffa801b 358 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
xosuuu 0:1a6e8ffa801b 359 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 360 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
xosuuu 0:1a6e8ffa801b 361 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
xosuuu 0:1a6e8ffa801b 362 }
xosuuu 0:1a6e8ffa801b 363 }
xosuuu 0:1a6e8ffa801b 364 }
xosuuu 0:1a6e8ffa801b 365
xosuuu 0:1a6e8ffa801b 366 int16_t readTempData(){
xosuuu 0:1a6e8ffa801b 367 uint8_t rawData[2]; // x/y/z gyro register data stored here
xosuuu 0:1a6e8ffa801b 368 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
xosuuu 0:1a6e8ffa801b 369 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
xosuuu 0:1a6e8ffa801b 370 }
xosuuu 0:1a6e8ffa801b 371
xosuuu 0:1a6e8ffa801b 372 void resetMPU9250(){
xosuuu 0:1a6e8ffa801b 373 // reset device
xosuuu 0:1a6e8ffa801b 374 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
xosuuu 0:1a6e8ffa801b 375 wait(0.1);
xosuuu 0:1a6e8ffa801b 376 }
xosuuu 0:1a6e8ffa801b 377
xosuuu 0:1a6e8ffa801b 378 void initAK8963(float * destination){
xosuuu 0:1a6e8ffa801b 379 // First extract the factory calibration for each magnetometer axis
xosuuu 0:1a6e8ffa801b 380 uint8_t rawData[3]; // x/y/z gyro calibration data stored here
xosuuu 0:1a6e8ffa801b 381 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
xosuuu 0:1a6e8ffa801b 382 wait(0.01);
xosuuu 0:1a6e8ffa801b 383 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
xosuuu 0:1a6e8ffa801b 384 wait(0.01);
xosuuu 0:1a6e8ffa801b 385 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
xosuuu 0:1a6e8ffa801b 386 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
xosuuu 0:1a6e8ffa801b 387 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
xosuuu 0:1a6e8ffa801b 388 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
xosuuu 0:1a6e8ffa801b 389 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
xosuuu 0:1a6e8ffa801b 390 wait(0.01);
xosuuu 0:1a6e8ffa801b 391 // Configure the magnetometer for continuous read and highest resolution
xosuuu 0:1a6e8ffa801b 392 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
xosuuu 0:1a6e8ffa801b 393 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
xosuuu 0:1a6e8ffa801b 394 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
xosuuu 0:1a6e8ffa801b 395 wait(0.01);
xosuuu 0:1a6e8ffa801b 396 }
xosuuu 0:1a6e8ffa801b 397
xosuuu 0:1a6e8ffa801b 398 void initMPU9250(){
xosuuu 0:1a6e8ffa801b 399 // Initialize MPU9250 device
xosuuu 0:1a6e8ffa801b 400 // wake up device
xosuuu 0:1a6e8ffa801b 401 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
xosuuu 0:1a6e8ffa801b 402 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
xosuuu 0:1a6e8ffa801b 403
xosuuu 0:1a6e8ffa801b 404 // get stable time source
xosuuu 0:1a6e8ffa801b 405 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 406
xosuuu 0:1a6e8ffa801b 407 // Configure Gyro and Accelerometer
xosuuu 0:1a6e8ffa801b 408 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
xosuuu 0:1a6e8ffa801b 409 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
xosuuu 0:1a6e8ffa801b 410 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
xosuuu 0:1a6e8ffa801b 411 writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
xosuuu 0:1a6e8ffa801b 412
xosuuu 0:1a6e8ffa801b 413 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
xosuuu 0:1a6e8ffa801b 414 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
xosuuu 0:1a6e8ffa801b 415
xosuuu 0:1a6e8ffa801b 416 // Set gyroscope full scale range
xosuuu 0:1a6e8ffa801b 417 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
xosuuu 0:1a6e8ffa801b 418 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
xosuuu 0:1a6e8ffa801b 419 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
xosuuu 0:1a6e8ffa801b 420 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
xosuuu 0:1a6e8ffa801b 421 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
xosuuu 0:1a6e8ffa801b 422
xosuuu 0:1a6e8ffa801b 423 // Set accelerometer configuration
xosuuu 0:1a6e8ffa801b 424 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
xosuuu 0:1a6e8ffa801b 425 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
xosuuu 0:1a6e8ffa801b 426 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
xosuuu 0:1a6e8ffa801b 427 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer
xosuuu 0:1a6e8ffa801b 428
xosuuu 0:1a6e8ffa801b 429 // Set accelerometer sample rate configuration
xosuuu 0:1a6e8ffa801b 430 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
xosuuu 0:1a6e8ffa801b 431 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
xosuuu 0:1a6e8ffa801b 432 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
xosuuu 0:1a6e8ffa801b 433 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
xosuuu 0:1a6e8ffa801b 434 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
xosuuu 0:1a6e8ffa801b 435
xosuuu 0:1a6e8ffa801b 436 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
xosuuu 0:1a6e8ffa801b 437 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
xosuuu 0:1a6e8ffa801b 438
xosuuu 0:1a6e8ffa801b 439 // Configure Interrupts and Bypass Enable
xosuuu 0:1a6e8ffa801b 440 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
xosuuu 0:1a6e8ffa801b 441 // can join the I2C bus and all can be controlled by the Arduino as master
xosuuu 0:1a6e8ffa801b 442 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
xosuuu 0:1a6e8ffa801b 443 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
xosuuu 0:1a6e8ffa801b 444 }
xosuuu 0:1a6e8ffa801b 445
xosuuu 0:1a6e8ffa801b 446 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
xosuuu 0:1a6e8ffa801b 447 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
xosuuu 0:1a6e8ffa801b 448 void calibrateMPU9250(float * dest1, float * dest2)
xosuuu 0:1a6e8ffa801b 449 {
xosuuu 0:1a6e8ffa801b 450 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
xosuuu 0:1a6e8ffa801b 451 uint16_t ii, packet_count, fifo_count;
xosuuu 0:1a6e8ffa801b 452 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
xosuuu 0:1a6e8ffa801b 453
xosuuu 0:1a6e8ffa801b 454 // reset device, reset all registers, clear gyro and accelerometer bias registers
xosuuu 0:1a6e8ffa801b 455 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
xosuuu 2:bb20d5091065 456 wait(0.1);
xosuuu 0:1a6e8ffa801b 457
xosuuu 0:1a6e8ffa801b 458 // get stable time source
xosuuu 0:1a6e8ffa801b 459 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
xosuuu 0:1a6e8ffa801b 460 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
xosuuu 0:1a6e8ffa801b 461 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
xosuuu 0:1a6e8ffa801b 462 wait(0.2);
xosuuu 0:1a6e8ffa801b 463
xosuuu 0:1a6e8ffa801b 464 // Configure device for bias calculation
xosuuu 0:1a6e8ffa801b 465 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
xosuuu 0:1a6e8ffa801b 466 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
xosuuu 0:1a6e8ffa801b 467 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
xosuuu 0:1a6e8ffa801b 468 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
xosuuu 0:1a6e8ffa801b 469 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
xosuuu 0:1a6e8ffa801b 470 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
xosuuu 0:1a6e8ffa801b 471 wait(0.015);
xosuuu 0:1a6e8ffa801b 472
xosuuu 0:1a6e8ffa801b 473 // Configure MPU9250 gyro and accelerometer for bias calculation
xosuuu 0:1a6e8ffa801b 474 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
xosuuu 0:1a6e8ffa801b 475 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
xosuuu 0:1a6e8ffa801b 476 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
xosuuu 0:1a6e8ffa801b 477 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
xosuuu 0:1a6e8ffa801b 478
xosuuu 0:1a6e8ffa801b 479 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
xosuuu 0:1a6e8ffa801b 480 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
xosuuu 0:1a6e8ffa801b 481
xosuuu 2:bb20d5091065 482 // Configure FIFO to capture accelerometer and gyro data. This data will be used for bias calculation
xosuuu 0:1a6e8ffa801b 483 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
xosuuu 0:1a6e8ffa801b 484 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
xosuuu 0:1a6e8ffa801b 485 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
xosuuu 0:1a6e8ffa801b 486
xosuuu 0:1a6e8ffa801b 487 // At end of sample accumulation, turn off FIFO sensor read
xosuuu 0:1a6e8ffa801b 488 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
xosuuu 0:1a6e8ffa801b 489 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
xosuuu 0:1a6e8ffa801b 490 fifo_count = ((uint16_t)data[0] << 8) | data[1];
xosuuu 0:1a6e8ffa801b 491 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
xosuuu 0:1a6e8ffa801b 492
xosuuu 0:1a6e8ffa801b 493 for (ii = 0; ii < packet_count; ii++) {
xosuuu 0:1a6e8ffa801b 494 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
xosuuu 0:1a6e8ffa801b 495 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
xosuuu 0:1a6e8ffa801b 496 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 497 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
xosuuu 0:1a6e8ffa801b 498 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
xosuuu 0:1a6e8ffa801b 499 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
xosuuu 0:1a6e8ffa801b 500 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
xosuuu 0:1a6e8ffa801b 501 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
xosuuu 0:1a6e8ffa801b 502
xosuuu 0:1a6e8ffa801b 503 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
xosuuu 0:1a6e8ffa801b 504 accel_bias[1] += (int32_t) accel_temp[1];
xosuuu 0:1a6e8ffa801b 505 accel_bias[2] += (int32_t) accel_temp[2];
xosuuu 0:1a6e8ffa801b 506 gyro_bias[0] += (int32_t) gyro_temp[0];
xosuuu 0:1a6e8ffa801b 507 gyro_bias[1] += (int32_t) gyro_temp[1];
xosuuu 0:1a6e8ffa801b 508 gyro_bias[2] += (int32_t) gyro_temp[2];
xosuuu 0:1a6e8ffa801b 509
xosuuu 0:1a6e8ffa801b 510 }
xosuuu 0:1a6e8ffa801b 511 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
xosuuu 0:1a6e8ffa801b 512 accel_bias[1] /= (int32_t) packet_count;
xosuuu 0:1a6e8ffa801b 513 accel_bias[2] /= (int32_t) packet_count;
xosuuu 0:1a6e8ffa801b 514 gyro_bias[0] /= (int32_t) packet_count;
xosuuu 0:1a6e8ffa801b 515 gyro_bias[1] /= (int32_t) packet_count;
xosuuu 0:1a6e8ffa801b 516 gyro_bias[2] /= (int32_t) packet_count;
xosuuu 0:1a6e8ffa801b 517
xosuuu 0:1a6e8ffa801b 518 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
xosuuu 0:1a6e8ffa801b 519 else {accel_bias[2] += (int32_t) accelsensitivity;}
xosuuu 0:1a6e8ffa801b 520
xosuuu 0:1a6e8ffa801b 521 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
xosuuu 0:1a6e8ffa801b 522 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 523 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
xosuuu 0:1a6e8ffa801b 524 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
xosuuu 0:1a6e8ffa801b 525 data[3] = (-gyro_bias[1]/4) & 0xFF;
xosuuu 0:1a6e8ffa801b 526 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
xosuuu 0:1a6e8ffa801b 527 data[5] = (-gyro_bias[2]/4) & 0xFF;
xosuuu 0:1a6e8ffa801b 528
xosuuu 0:1a6e8ffa801b 529 /// Push gyro biases to hardware registers
xosuuu 0:1a6e8ffa801b 530 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
xosuuu 0:1a6e8ffa801b 531 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
xosuuu 0:1a6e8ffa801b 532 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
xosuuu 0:1a6e8ffa801b 533 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
xosuuu 0:1a6e8ffa801b 534 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
xosuuu 0:1a6e8ffa801b 535 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
xosuuu 0:1a6e8ffa801b 536 */
xosuuu 0:1a6e8ffa801b 537 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
xosuuu 0:1a6e8ffa801b 538 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
xosuuu 0:1a6e8ffa801b 539 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
xosuuu 0:1a6e8ffa801b 540
xosuuu 0:1a6e8ffa801b 541 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
xosuuu 0:1a6e8ffa801b 542 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
xosuuu 0:1a6e8ffa801b 543 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
xosuuu 0:1a6e8ffa801b 544 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
xosuuu 0:1a6e8ffa801b 545 // the accelerometer biases calculated above must be divided by 8.
xosuuu 0:1a6e8ffa801b 546
xosuuu 0:1a6e8ffa801b 547 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
xosuuu 0:1a6e8ffa801b 548 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
xosuuu 0:1a6e8ffa801b 549 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
xosuuu 0:1a6e8ffa801b 550 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
xosuuu 0:1a6e8ffa801b 551 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
xosuuu 0:1a6e8ffa801b 552 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
xosuuu 0:1a6e8ffa801b 553 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
xosuuu 0:1a6e8ffa801b 554
xosuuu 0:1a6e8ffa801b 555 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
xosuuu 0:1a6e8ffa801b 556 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
xosuuu 0:1a6e8ffa801b 557
xosuuu 0:1a6e8ffa801b 558 for(ii = 0; ii < 3; ii++) {
xosuuu 0:1a6e8ffa801b 559 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 560 }
xosuuu 0:1a6e8ffa801b 561
xosuuu 0:1a6e8ffa801b 562 // Construct total accelerometer bias, including calculated average accelerometer bias from above
xosuuu 0:1a6e8ffa801b 563 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 564 accel_bias_reg[1] -= (accel_bias[1]/8);
xosuuu 0:1a6e8ffa801b 565 accel_bias_reg[2] -= (accel_bias[2]/8);
xosuuu 0:1a6e8ffa801b 566
xosuuu 0:1a6e8ffa801b 567 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
xosuuu 0:1a6e8ffa801b 568 data[1] = (accel_bias_reg[0]) & 0xFF;
xosuuu 0:1a6e8ffa801b 569 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
xosuuu 0:1a6e8ffa801b 570 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
xosuuu 0:1a6e8ffa801b 571 data[3] = (accel_bias_reg[1]) & 0xFF;
xosuuu 0:1a6e8ffa801b 572 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
xosuuu 0:1a6e8ffa801b 573 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
xosuuu 0:1a6e8ffa801b 574 data[5] = (accel_bias_reg[2]) & 0xFF;
xosuuu 0:1a6e8ffa801b 575 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
xosuuu 0:1a6e8ffa801b 576
xosuuu 0:1a6e8ffa801b 577 // Apparently this is not working for the acceleration biases in the MPU-9250
xosuuu 0:1a6e8ffa801b 578 // Are we handling the temperature correction bit properly?
xosuuu 0:1a6e8ffa801b 579 // Push accelerometer biases to hardware registers
xosuuu 0:1a6e8ffa801b 580 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
xosuuu 0:1a6e8ffa801b 581 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
xosuuu 0:1a6e8ffa801b 582 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
xosuuu 0:1a6e8ffa801b 583 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
xosuuu 0:1a6e8ffa801b 584 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
xosuuu 0:1a6e8ffa801b 585 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
xosuuu 0:1a6e8ffa801b 586 */
xosuuu 0:1a6e8ffa801b 587 // Output scaled accelerometer biases for manual subtraction in the main program
xosuuu 0:1a6e8ffa801b 588 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
xosuuu 0:1a6e8ffa801b 589 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
xosuuu 0:1a6e8ffa801b 590 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
xosuuu 0:1a6e8ffa801b 591 }
xosuuu 0:1a6e8ffa801b 592
xosuuu 0:1a6e8ffa801b 593
xosuuu 0:1a6e8ffa801b 594 // Accelerometer and gyroscope self test; check calibration wrt factory settings
xosuuu 0:1a6e8ffa801b 595 void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
xosuuu 0:1a6e8ffa801b 596 {
xosuuu 0:1a6e8ffa801b 597 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
xosuuu 0:1a6e8ffa801b 598 uint8_t selfTest[6];
xosuuu 0:1a6e8ffa801b 599 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
xosuuu 0:1a6e8ffa801b 600 float factoryTrim[6];
xosuuu 0:1a6e8ffa801b 601 uint8_t FS = 0;
xosuuu 0:1a6e8ffa801b 602
xosuuu 0:1a6e8ffa801b 603 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
xosuuu 0:1a6e8ffa801b 604 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
xosuuu 0:1a6e8ffa801b 605 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
xosuuu 0:1a6e8ffa801b 606 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
xosuuu 0:1a6e8ffa801b 607 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
xosuuu 0:1a6e8ffa801b 608
xosuuu 0:1a6e8ffa801b 609 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
xosuuu 0:1a6e8ffa801b 610
xosuuu 0:1a6e8ffa801b 611 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
xosuuu 0:1a6e8ffa801b 612 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 613 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
xosuuu 0:1a6e8ffa801b 614 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
xosuuu 0:1a6e8ffa801b 615
xosuuu 0:1a6e8ffa801b 616 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
xosuuu 0:1a6e8ffa801b 617 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 618 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
xosuuu 0:1a6e8ffa801b 619 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
xosuuu 0:1a6e8ffa801b 620 }
xosuuu 0:1a6e8ffa801b 621
xosuuu 0:1a6e8ffa801b 622 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
xosuuu 0:1a6e8ffa801b 623 aAvg[ii] /= 200;
xosuuu 0:1a6e8ffa801b 624 gAvg[ii] /= 200;
xosuuu 0:1a6e8ffa801b 625 }
xosuuu 0:1a6e8ffa801b 626
xosuuu 0:1a6e8ffa801b 627 // Configure the accelerometer for self-test
xosuuu 0:1a6e8ffa801b 628 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
xosuuu 0:1a6e8ffa801b 629 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
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
xosuuu 1:61bf659e4a1f 690 // First of all measure 3 axis magnetometer values (only X and Y axis is used):
xosuuu 1:61bf659e4a1f 691 readMagData(magCount); // Read the x/y/z adc values
xosuuu 1:61bf659e4a1f 692 // Calculate the magnetometer values in milliGauss
xosuuu 1:61bf659e4a1f 693 // Include factory calibration per data sheet and user environmental corrections
xosuuu 1:61bf659e4a1f 694 if (I2Cstate == 0){ // no error on I2C
xosuuu 1:61bf659e4a1f 695 I2Cstate = 1;
xosuuu 1:61bf659e4a1f 696 magn_x = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
xosuuu 1:61bf659e4a1f 697 magn_y = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
xosuuu 1:61bf659e4a1f 698 }
xosuuu 1:61bf659e4a1f 699
xosuuu 1:61bf659e4a1f 700 // Now obtains the orientation value:
xosuuu 1:61bf659e4a1f 701 if (magn_y>0)
xosuuu 1:61bf659e4a1f 702 orient[0] = 90.0 - (float) ( atan(magn_x/magn_y)*180/M_PI );
xosuuu 1:61bf659e4a1f 703 else if (magn_y<0)
xosuuu 1:61bf659e4a1f 704 orient[0] = 270.0 - (float) ( atan(magn_x/magn_y)*180/M_PI );
xosuuu 1:61bf659e4a1f 705 else if (magn_y == 0){
xosuuu 1:61bf659e4a1f 706 if (magn_x<0)
xosuuu 1:61bf659e4a1f 707 orient[0] = 180.0;
xosuuu 1:61bf659e4a1f 708 else
xosuuu 1:61bf659e4a1f 709 orient[0] = 0.0;
xosuuu 1:61bf659e4a1f 710 }
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 0:1a6e8ffa801b 717
xosuuu 0:1a6e8ffa801b 718 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
xosuuu 0:1a6e8ffa801b 719 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
xosuuu 0:1a6e8ffa801b 720 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
xosuuu 0:1a6e8ffa801b 721 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
xosuuu 0:1a6e8ffa801b 722 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
xosuuu 0:1a6e8ffa801b 723 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
xosuuu 0:1a6e8ffa801b 724 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
xosuuu 0:1a6e8ffa801b 725 {
xosuuu 0:1a6e8ffa801b 726 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
xosuuu 0:1a6e8ffa801b 727 float norm;
xosuuu 0:1a6e8ffa801b 728 float hx, hy, _2bx, _2bz;
xosuuu 0:1a6e8ffa801b 729 float s1, s2, s3, s4;
xosuuu 0:1a6e8ffa801b 730 float qDot1, qDot2, qDot3, qDot4;
xosuuu 0:1a6e8ffa801b 731
xosuuu 0:1a6e8ffa801b 732 // Auxiliary variables to avoid repeated arithmetic
xosuuu 0:1a6e8ffa801b 733 float _2q1mx;
xosuuu 0:1a6e8ffa801b 734 float _2q1my;
xosuuu 0:1a6e8ffa801b 735 float _2q1mz;
xosuuu 0:1a6e8ffa801b 736 float _2q2mx;
xosuuu 0:1a6e8ffa801b 737 float _4bx;
xosuuu 0:1a6e8ffa801b 738 float _4bz;
xosuuu 0:1a6e8ffa801b 739 float _2q1 = 2.0f * q1;
xosuuu 0:1a6e8ffa801b 740 float _2q2 = 2.0f * q2;
xosuuu 0:1a6e8ffa801b 741 float _2q3 = 2.0f * q3;
xosuuu 0:1a6e8ffa801b 742 float _2q4 = 2.0f * q4;
xosuuu 0:1a6e8ffa801b 743 float _2q1q3 = 2.0f * q1 * q3;
xosuuu 0:1a6e8ffa801b 744 float _2q3q4 = 2.0f * q3 * q4;
xosuuu 0:1a6e8ffa801b 745 float q1q1 = q1 * q1;
xosuuu 0:1a6e8ffa801b 746 float q1q2 = q1 * q2;
xosuuu 0:1a6e8ffa801b 747 float q1q3 = q1 * q3;
xosuuu 0:1a6e8ffa801b 748 float q1q4 = q1 * q4;
xosuuu 0:1a6e8ffa801b 749 float q2q2 = q2 * q2;
xosuuu 0:1a6e8ffa801b 750 float q2q3 = q2 * q3;
xosuuu 0:1a6e8ffa801b 751 float q2q4 = q2 * q4;
xosuuu 0:1a6e8ffa801b 752 float q3q3 = q3 * q3;
xosuuu 0:1a6e8ffa801b 753 float q3q4 = q3 * q4;
xosuuu 0:1a6e8ffa801b 754 float q4q4 = q4 * q4;
xosuuu 0:1a6e8ffa801b 755
xosuuu 0:1a6e8ffa801b 756 // Normalise accelerometer measurement
xosuuu 0:1a6e8ffa801b 757 norm = sqrt(ax * ax + ay * ay + az * az);
xosuuu 0:1a6e8ffa801b 758 if (norm == 0.0f) return; // handle NaN
xosuuu 0:1a6e8ffa801b 759 norm = 1.0f/norm;
xosuuu 0:1a6e8ffa801b 760 ax *= norm;
xosuuu 0:1a6e8ffa801b 761 ay *= norm;
xosuuu 0:1a6e8ffa801b 762 az *= norm;
xosuuu 0:1a6e8ffa801b 763
xosuuu 0:1a6e8ffa801b 764 // Normalise magnetometer measurement
xosuuu 0:1a6e8ffa801b 765 norm = sqrt(mx * mx + my * my + mz * mz);
xosuuu 0:1a6e8ffa801b 766 if (norm == 0.0f) return; // handle NaN
xosuuu 0:1a6e8ffa801b 767 norm = 1.0f/norm;
xosuuu 0:1a6e8ffa801b 768 mx *= norm;
xosuuu 0:1a6e8ffa801b 769 my *= norm;
xosuuu 0:1a6e8ffa801b 770 mz *= norm;
xosuuu 0:1a6e8ffa801b 771
xosuuu 0:1a6e8ffa801b 772 // Reference direction of Earth's magnetic field
xosuuu 0:1a6e8ffa801b 773 _2q1mx = 2.0f * q1 * mx;
xosuuu 0:1a6e8ffa801b 774 _2q1my = 2.0f * q1 * my;
xosuuu 0:1a6e8ffa801b 775 _2q1mz = 2.0f * q1 * mz;
xosuuu 0:1a6e8ffa801b 776 _2q2mx = 2.0f * q2 * mx;
xosuuu 0:1a6e8ffa801b 777 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
xosuuu 0:1a6e8ffa801b 778 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
xosuuu 0:1a6e8ffa801b 779 _2bx = sqrt(hx * hx + hy * hy);
xosuuu 0:1a6e8ffa801b 780 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
xosuuu 0:1a6e8ffa801b 781 _4bx = 2.0f * _2bx;
xosuuu 0:1a6e8ffa801b 782 _4bz = 2.0f * _2bz;
xosuuu 0:1a6e8ffa801b 783
xosuuu 0:1a6e8ffa801b 784 // Gradient decent algorithm corrective step
xosuuu 0:1a6e8ffa801b 785 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 786 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 787 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 788 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 789 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
xosuuu 0:1a6e8ffa801b 790 norm = 1.0f/norm;
xosuuu 0:1a6e8ffa801b 791 s1 *= norm;
xosuuu 0:1a6e8ffa801b 792 s2 *= norm;
xosuuu 0:1a6e8ffa801b 793 s3 *= norm;
xosuuu 0:1a6e8ffa801b 794 s4 *= norm;
xosuuu 0:1a6e8ffa801b 795
xosuuu 0:1a6e8ffa801b 796 // Compute rate of change of quaternion
xosuuu 0:1a6e8ffa801b 797 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
xosuuu 0:1a6e8ffa801b 798 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
xosuuu 0:1a6e8ffa801b 799 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
xosuuu 0:1a6e8ffa801b 800 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
xosuuu 0:1a6e8ffa801b 801
xosuuu 0:1a6e8ffa801b 802 // Integrate to yield quaternion
xosuuu 0:1a6e8ffa801b 803 q1 += qDot1 * deltat;
xosuuu 0:1a6e8ffa801b 804 q2 += qDot2 * deltat;
xosuuu 0:1a6e8ffa801b 805 q3 += qDot3 * deltat;
xosuuu 0:1a6e8ffa801b 806 q4 += qDot4 * deltat;
xosuuu 0:1a6e8ffa801b 807 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
xosuuu 0:1a6e8ffa801b 808 norm = 1.0f/norm;
xosuuu 0:1a6e8ffa801b 809 q[0] = q1 * norm;
xosuuu 0:1a6e8ffa801b 810 q[1] = q2 * norm;
xosuuu 0:1a6e8ffa801b 811 q[2] = q3 * norm;
xosuuu 0:1a6e8ffa801b 812 q[3] = q4 * norm;
xosuuu 0:1a6e8ffa801b 813
xosuuu 0:1a6e8ffa801b 814 }
xosuuu 0:1a6e8ffa801b 815
xosuuu 0:1a6e8ffa801b 816
xosuuu 0:1a6e8ffa801b 817
xosuuu 0:1a6e8ffa801b 818 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
xosuuu 0:1a6e8ffa801b 819 // measured ones.
xosuuu 0:1a6e8ffa801b 820 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
xosuuu 0:1a6e8ffa801b 821 {
xosuuu 0:1a6e8ffa801b 822 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
xosuuu 0:1a6e8ffa801b 823 float norm;
xosuuu 0:1a6e8ffa801b 824 float hx, hy, bx, bz;
xosuuu 0:1a6e8ffa801b 825 float vx, vy, vz, wx, wy, wz;
xosuuu 0:1a6e8ffa801b 826 float ex, ey, ez;
xosuuu 0:1a6e8ffa801b 827 float pa, pb, pc;
xosuuu 0:1a6e8ffa801b 828
xosuuu 0:1a6e8ffa801b 829 // Auxiliary variables to avoid repeated arithmetic
xosuuu 0:1a6e8ffa801b 830 float q1q1 = q1 * q1;
xosuuu 0:1a6e8ffa801b 831 float q1q2 = q1 * q2;
xosuuu 0:1a6e8ffa801b 832 float q1q3 = q1 * q3;
xosuuu 0:1a6e8ffa801b 833 float q1q4 = q1 * q4;
xosuuu 0:1a6e8ffa801b 834 float q2q2 = q2 * q2;
xosuuu 0:1a6e8ffa801b 835 float q2q3 = q2 * q3;
xosuuu 0:1a6e8ffa801b 836 float q2q4 = q2 * q4;
xosuuu 0:1a6e8ffa801b 837 float q3q3 = q3 * q3;
xosuuu 0:1a6e8ffa801b 838 float q3q4 = q3 * q4;
xosuuu 0:1a6e8ffa801b 839 float q4q4 = q4 * q4;
xosuuu 0:1a6e8ffa801b 840
xosuuu 0:1a6e8ffa801b 841 // Normalise accelerometer measurement
xosuuu 0:1a6e8ffa801b 842 norm = sqrt(ax * ax + ay * ay + az * az);
xosuuu 0:1a6e8ffa801b 843 if (norm == 0.0f) return; // handle NaN
xosuuu 0:1a6e8ffa801b 844 norm = 1.0f / norm; // use reciprocal for division
xosuuu 0:1a6e8ffa801b 845 ax *= norm;
xosuuu 0:1a6e8ffa801b 846 ay *= norm;
xosuuu 0:1a6e8ffa801b 847 az *= norm;
xosuuu 0:1a6e8ffa801b 848
xosuuu 0:1a6e8ffa801b 849 // Normalise magnetometer measurement
xosuuu 0:1a6e8ffa801b 850 norm = sqrt(mx * mx + my * my + mz * mz);
xosuuu 0:1a6e8ffa801b 851 if (norm == 0.0f) return; // handle NaN
xosuuu 0:1a6e8ffa801b 852 norm = 1.0f / norm; // use reciprocal for division
xosuuu 0:1a6e8ffa801b 853 mx *= norm;
xosuuu 0:1a6e8ffa801b 854 my *= norm;
xosuuu 0:1a6e8ffa801b 855 mz *= norm;
xosuuu 0:1a6e8ffa801b 856
xosuuu 0:1a6e8ffa801b 857 // Reference direction of Earth's magnetic field
xosuuu 0:1a6e8ffa801b 858 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
xosuuu 0:1a6e8ffa801b 859 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
xosuuu 0:1a6e8ffa801b 860 bx = sqrt((hx * hx) + (hy * hy));
xosuuu 0:1a6e8ffa801b 861 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
xosuuu 0:1a6e8ffa801b 862
xosuuu 0:1a6e8ffa801b 863 // Estimated direction of gravity and magnetic field
xosuuu 0:1a6e8ffa801b 864 vx = 2.0f * (q2q4 - q1q3);
xosuuu 0:1a6e8ffa801b 865 vy = 2.0f * (q1q2 + q3q4);
xosuuu 0:1a6e8ffa801b 866 vz = q1q1 - q2q2 - q3q3 + q4q4;
xosuuu 0:1a6e8ffa801b 867 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
xosuuu 0:1a6e8ffa801b 868 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
xosuuu 0:1a6e8ffa801b 869 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
xosuuu 0:1a6e8ffa801b 870
xosuuu 0:1a6e8ffa801b 871 // Error is cross product between estimated direction and measured direction of gravity
xosuuu 0:1a6e8ffa801b 872 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
xosuuu 0:1a6e8ffa801b 873 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
xosuuu 0:1a6e8ffa801b 874 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
xosuuu 0:1a6e8ffa801b 875 if (Ki > 0.0f)
xosuuu 0:1a6e8ffa801b 876 {
xosuuu 0:1a6e8ffa801b 877 eInt[0] += ex; // accumulate integral error
xosuuu 0:1a6e8ffa801b 878 eInt[1] += ey;
xosuuu 0:1a6e8ffa801b 879 eInt[2] += ez;
xosuuu 0:1a6e8ffa801b 880 }
xosuuu 0:1a6e8ffa801b 881 else
xosuuu 0:1a6e8ffa801b 882 {
xosuuu 0:1a6e8ffa801b 883 eInt[0] = 0.0f; // prevent integral wind up
xosuuu 0:1a6e8ffa801b 884 eInt[1] = 0.0f;
xosuuu 0:1a6e8ffa801b 885 eInt[2] = 0.0f;
xosuuu 0:1a6e8ffa801b 886 }
xosuuu 0:1a6e8ffa801b 887
xosuuu 0:1a6e8ffa801b 888 // Apply feedback terms
xosuuu 0:1a6e8ffa801b 889 gx = gx + Kp * ex + Ki * eInt[0];
xosuuu 0:1a6e8ffa801b 890 gy = gy + Kp * ey + Ki * eInt[1];
xosuuu 0:1a6e8ffa801b 891 gz = gz + Kp * ez + Ki * eInt[2];
xosuuu 0:1a6e8ffa801b 892
xosuuu 0:1a6e8ffa801b 893 // Integrate rate of change of quaternion
xosuuu 0:1a6e8ffa801b 894 pa = q2;
xosuuu 0:1a6e8ffa801b 895 pb = q3;
xosuuu 0:1a6e8ffa801b 896 pc = q4;
xosuuu 0:1a6e8ffa801b 897 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
xosuuu 0:1a6e8ffa801b 898 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
xosuuu 0:1a6e8ffa801b 899 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
xosuuu 0:1a6e8ffa801b 900 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
xosuuu 0:1a6e8ffa801b 901
xosuuu 0:1a6e8ffa801b 902 // Normalise quaternion
xosuuu 0:1a6e8ffa801b 903 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
xosuuu 0:1a6e8ffa801b 904 norm = 1.0f / norm;
xosuuu 0:1a6e8ffa801b 905 q[0] = q1 * norm;
xosuuu 0:1a6e8ffa801b 906 q[1] = q2 * norm;
xosuuu 0:1a6e8ffa801b 907 q[2] = q3 * norm;
xosuuu 0:1a6e8ffa801b 908 q[3] = q4 * norm;
xosuuu 0:1a6e8ffa801b 909
xosuuu 0:1a6e8ffa801b 910 }
xosuuu 0:1a6e8ffa801b 911 };
xosuuu 0:1a6e8ffa801b 912 #endif