mpu9250 - MPU9250 library fork

Users » rctaduio » Code » mpu9250

MPU9250 library fork

MPU9250.h@0:89967a01628c, 2016-10-06 (annotated)

Committer:: rctaduio
Date:: Thu Oct 06 22:19:07 2016 +0000
Revision:: 0:89967a01628c

Updated to include madgewick open source lib

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	06 Oct 2016
Imports:	28
Forks:	0
Commits:	1
Dependents:	1
Dependencies:	0
Followers:	1

MPU9250.h@0:89967a01628c, 2016-10-06 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning