MotionDriverv512 - InvenSense Motion Driver v5.1.2 ported

Users » oprospero » Code » MotionDriverv512

InvenSense Motion Driver v5.1.2 ported

inv_mpu.cpp@1:dbbd3dcd61c9, 2014-06-07 (annotated)

Committer:: oprospero
Date:: Sat Jun 07 21:05:23 2014 +0000
Revision:: 1:dbbd3dcd61c9
Parent:: 0:4dd021344a6b
Child:: 3:8ac73f9099ab

initialization returns where failure occurs

Who changed what in which revision?

User	Revision	Line number	New contents of line
oprospero	0:4dd021344a6b	1	/*
oprospero	0:4dd021344a6b	2	$License:
oprospero	0:4dd021344a6b	3	Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
oprospero	0:4dd021344a6b	4	See included License.txt for License information.
oprospero	0:4dd021344a6b	5	$
oprospero	0:4dd021344a6b	6	*/
oprospero	0:4dd021344a6b	7	/**
oprospero	0:4dd021344a6b	8	* @addtogroup DRIVERS Sensor Driver Layer
oprospero	0:4dd021344a6b	9	* @brief Hardware drivers to communicate with sensors via I2C.
oprospero	0:4dd021344a6b	10	*
oprospero	0:4dd021344a6b	11	* @{
oprospero	0:4dd021344a6b	12	* @file inv_mpu.c
oprospero	0:4dd021344a6b	13	* @brief An I2C-based driver for Invensense gyroscopes.
oprospero	0:4dd021344a6b	14	* @details This driver currently works for the following devices:
oprospero	0:4dd021344a6b	15	* MPU6050
oprospero	0:4dd021344a6b	16	* MPU6500
oprospero	0:4dd021344a6b	17	* MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)
oprospero	0:4dd021344a6b	18	* MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)
oprospero	0:4dd021344a6b	19	*/
oprospero	0:4dd021344a6b	20	#include <stdio.h>
oprospero	0:4dd021344a6b	21	#include <stdint.h>
oprospero	0:4dd021344a6b	22	#include <stdlib.h>
oprospero	0:4dd021344a6b	23	#include <string.h>
oprospero	0:4dd021344a6b	24	#include <math.h>
oprospero	0:4dd021344a6b	25	#include "inv_mpu.h"
oprospero	0:4dd021344a6b	26
oprospero	0:4dd021344a6b	27	#ifdef DEBUG_MPU
oprospero	0:4dd021344a6b	28	Serial debug_mpu(USBTX, USBRX);
oprospero	0:4dd021344a6b	29	#define LN "\r\n"
oprospero	0:4dd021344a6b	30	#define BREAK debug_mpu.printf(LN)
oprospero	0:4dd021344a6b	31	#define PRINTF debug_mpu.printf
oprospero	0:4dd021344a6b	32	#define PRINTLN(x) debug_mpu.printf(x);BREAK
oprospero	0:4dd021344a6b	33	#else
oprospero	0:4dd021344a6b	34	#define PRINTF(...)
oprospero	0:4dd021344a6b	35	#define BREAK
oprospero	0:4dd021344a6b	36	#define PRINTLN(x)
oprospero	0:4dd021344a6b	37	#endif
oprospero	0:4dd021344a6b	38
oprospero	0:4dd021344a6b	39
oprospero	0:4dd021344a6b	40	#define log_i(...)
oprospero	0:4dd021344a6b	41	#define log_e(...)
oprospero	0:4dd021344a6b	42	#define get_ms(...)
oprospero	0:4dd021344a6b	43	#define min(a,b) ((a<b)?a:b)
oprospero	0:4dd021344a6b	44
oprospero	0:4dd021344a6b	45
oprospero	0:4dd021344a6b	46	/* The following functions must be defined for this platform:
oprospero	0:4dd021344a6b	47	* i2Cdev.writeBytes(unsigned char slave_addr, unsigned char reg_addr,
oprospero	0:4dd021344a6b	48	* unsigned char length, unsigned char const *data)
oprospero	0:4dd021344a6b	49	* i2Cdev.readBytes(unsigned char slave_addr, unsigned char reg_addr,
oprospero	0:4dd021344a6b	50	* unsigned char length, unsigned char *data)
oprospero	0:4dd021344a6b	51	* wait_ms(unsigned long num_ms)
oprospero	0:4dd021344a6b	52	* get_ms(unsigned long *count)
oprospero	0:4dd021344a6b	53	* reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
oprospero	0:4dd021344a6b	54	* labs(long x)
oprospero	0:4dd021344a6b	55	* fabsf(float x)
oprospero	0:4dd021344a6b	56	* min(int a, int b)
oprospero	0:4dd021344a6b	57	*/
oprospero	0:4dd021344a6b	58	//#if defined MOTION_DRIVER_TARGET_MSP430
oprospero	0:4dd021344a6b	59	//#include "msp430.h"
oprospero	0:4dd021344a6b	60	//#include "msp430_i2c.h"
oprospero	0:4dd021344a6b	61	//#include "msp430_clock.h"
oprospero	0:4dd021344a6b	62	//#include "msp430_interrupt.h"
oprospero	0:4dd021344a6b	63	//#define i2Cdev.writeBytes msp430_i2Cdev.writeBytes
oprospero	0:4dd021344a6b	64	//#define i2Cdev.readBytes msp430_i2Cdev.readBytes
oprospero	0:4dd021344a6b	65	//#define wait_ms msp430_wait_ms
oprospero	0:4dd021344a6b	66	//#define get_ms msp430_get_clock_ms
oprospero	0:4dd021344a6b	67	//static inline int reg_int_cb(struct int_param_s *int_param)
oprospero	0:4dd021344a6b	68	//{
oprospero	0:4dd021344a6b	69	// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
oprospero	0:4dd021344a6b	70	// int_param->active_low);
oprospero	0:4dd021344a6b	71	//}
oprospero	0:4dd021344a6b	72	//#define log_i(...) do {} while (0)
oprospero	0:4dd021344a6b	73	//#define log_e(...) do {} while (0)
oprospero	0:4dd021344a6b	74	///* labs is already defined by TI's toolchain. */
oprospero	0:4dd021344a6b	75	///* fabs is for doubles. fabsf is for floats. */
oprospero	0:4dd021344a6b	76	//#define fabs fabsf
oprospero	0:4dd021344a6b	77	//#define min(a,b) ((a<b)?a:b)
oprospero	0:4dd021344a6b	78	//#elif defined EMPL_TARGET_MSP430
oprospero	0:4dd021344a6b	79	//#include "msp430.h"
oprospero	0:4dd021344a6b	80	//#include "msp430_i2c.h"
oprospero	0:4dd021344a6b	81	//#include "msp430_clock.h"
oprospero	0:4dd021344a6b	82	//#include "msp430_interrupt.h"
oprospero	0:4dd021344a6b	83	//#include "log.h"
oprospero	0:4dd021344a6b	84	//#define i2Cdev.writeBytes msp430_i2Cdev.writeBytes
oprospero	0:4dd021344a6b	85	//#define i2Cdev.readBytes msp430_i2Cdev.readBytes
oprospero	0:4dd021344a6b	86	//#define wait_ms msp430_wait_ms
oprospero	0:4dd021344a6b	87	//#define get_ms msp430_get_clock_ms
oprospero	0:4dd021344a6b	88	//static inline int reg_int_cb(struct int_param_s *int_param)
oprospero	0:4dd021344a6b	89	//{
oprospero	0:4dd021344a6b	90	// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
oprospero	0:4dd021344a6b	91	// int_param->active_low);
oprospero	0:4dd021344a6b	92	//}
oprospero	0:4dd021344a6b	93	//#define log_i MPL_LOGI
oprospero	0:4dd021344a6b	94	//#define log_e MPL_LOGE
oprospero	0:4dd021344a6b	95	///* labs is already defined by TI's toolchain. */
oprospero	0:4dd021344a6b	96	///* fabs is for doubles. fabsf is for floats. */
oprospero	0:4dd021344a6b	97	//#define fabs fabsf
oprospero	0:4dd021344a6b	98	//#define min(a,b) ((a<b)?a:b)
oprospero	0:4dd021344a6b	99	//#elif defined EMPL_TARGET_UC3L0
oprospero	0:4dd021344a6b	100	///* Instead of using the standard TWI driver from the ASF library, we're using
oprospero	0:4dd021344a6b	101	// * a TWI driver that follows the slave address + register address convention.
oprospero	0:4dd021344a6b	102	// */
oprospero	0:4dd021344a6b	103	//#include "twi.h"
oprospero	0:4dd021344a6b	104	//#include "delay.h"
oprospero	0:4dd021344a6b	105	//#include "sysclk.h"
oprospero	0:4dd021344a6b	106	//#include "log.h"
oprospero	0:4dd021344a6b	107	//#include "sensors_xplained.h"
oprospero	0:4dd021344a6b	108	//#include "uc3l0_clock.h"
oprospero	0:4dd021344a6b	109	//#define i2Cdev.writeBytes(a, b, c, d) twi_write(a, b, d, c)
oprospero	0:4dd021344a6b	110	//#define i2Cdev.readBytes(a, b, c, d) twi_read(a, b, d, c)
oprospero	0:4dd021344a6b	111	///* wait_ms is a function already defined in ASF. */
oprospero	0:4dd021344a6b	112	//#define get_ms uc3l0_get_clock_ms
oprospero	0:4dd021344a6b	113	//static inline int reg_int_cb(struct int_param_s *int_param)
oprospero	0:4dd021344a6b	114	//{
oprospero	0:4dd021344a6b	115	// sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
oprospero	0:4dd021344a6b	116	// return 0;
oprospero	0:4dd021344a6b	117	//}
oprospero	0:4dd021344a6b	118	//#define log_i MPL_LOGI
oprospero	0:4dd021344a6b	119	//#define log_e MPL_LOGE
oprospero	0:4dd021344a6b	120	///* UC3 is a 32-bit processor, so abs and labs are equivalent. */
oprospero	0:4dd021344a6b	121	//#define labs abs
oprospero	0:4dd021344a6b	122	//#define fabs(x) (((x)>0)?(x):-(x))
oprospero	0:4dd021344a6b	123	//#else
oprospero	0:4dd021344a6b	124	//#error Gyro driver is missing the system layer implementations.
oprospero	0:4dd021344a6b	125	//#endif
oprospero	0:4dd021344a6b	126	#define MPU9150
oprospero	0:4dd021344a6b	127	#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
oprospero	0:4dd021344a6b	128	#error Which gyro are you using? Define MPUxxxx in your compiler options.
oprospero	0:4dd021344a6b	129	#endif
oprospero	0:4dd021344a6b	130
oprospero	0:4dd021344a6b	131	/* Time for some messy macro work. =]
oprospero	0:4dd021344a6b	132	* #define MPU9150
oprospero	0:4dd021344a6b	133	* is equivalent to..
oprospero	0:4dd021344a6b	134	* #define MPU6050
oprospero	0:4dd021344a6b	135	* #define AK8975_SECONDARY
oprospero	0:4dd021344a6b	136	*
oprospero	0:4dd021344a6b	137	* #define MPU9250
oprospero	0:4dd021344a6b	138	* is equivalent to..
oprospero	0:4dd021344a6b	139	* #define MPU6500
oprospero	0:4dd021344a6b	140	* #define AK8963_SECONDARY
oprospero	0:4dd021344a6b	141	*/
oprospero	0:4dd021344a6b	142	#if defined MPU9150
oprospero	0:4dd021344a6b	143	#ifndef MPU6050
oprospero	0:4dd021344a6b	144	#define MPU6050
oprospero	0:4dd021344a6b	145	#endif /* #ifndef MPU6050 */
oprospero	0:4dd021344a6b	146	#if defined AK8963_SECONDARY
oprospero	0:4dd021344a6b	147	#error "MPU9150 and AK8963_SECONDARY cannot both be defined."
oprospero	0:4dd021344a6b	148	#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
oprospero	0:4dd021344a6b	149	#define AK8975_SECONDARY
oprospero	0:4dd021344a6b	150	#endif /* #if defined AK8963_SECONDARY */
oprospero	0:4dd021344a6b	151	#elif defined MPU9250 /* #if defined MPU9150 */
oprospero	0:4dd021344a6b	152	#ifndef MPU6500
oprospero	0:4dd021344a6b	153	#define MPU6500
oprospero	0:4dd021344a6b	154	#endif /* #ifndef MPU6500 */
oprospero	0:4dd021344a6b	155	#if defined AK8975_SECONDARY
oprospero	0:4dd021344a6b	156	#error "MPU9250 and AK8975_SECONDARY cannot both be defined."
oprospero	0:4dd021344a6b	157	#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
oprospero	0:4dd021344a6b	158	#define AK8963_SECONDARY
oprospero	0:4dd021344a6b	159	#endif /* #if defined AK8975_SECONDARY */
oprospero	0:4dd021344a6b	160	#endif /* #if defined MPU9150 */
oprospero	0:4dd021344a6b	161
oprospero	0:4dd021344a6b	162	#if defined AK8975_SECONDARY \|\| defined AK8963_SECONDARY
oprospero	0:4dd021344a6b	163	#define AK89xx_SECONDARY
oprospero	0:4dd021344a6b	164	#else
oprospero	0:4dd021344a6b	165	/* #warning "No compass = less profit for Invensense. Lame." */
oprospero	0:4dd021344a6b	166	#endif
oprospero	0:4dd021344a6b	167
oprospero	0:4dd021344a6b	168	//static int set_int_enable(unsigned char enable);
oprospero	0:4dd021344a6b	169
oprospero	0:4dd021344a6b	170	/* Hardware registers needed by driver. */
oprospero	0:4dd021344a6b	171	struct gyro_reg_s {
oprospero	0:4dd021344a6b	172	unsigned char who_am_i;
oprospero	0:4dd021344a6b	173	unsigned char rate_div;
oprospero	0:4dd021344a6b	174	unsigned char lpf;
oprospero	0:4dd021344a6b	175	unsigned char prod_id;
oprospero	0:4dd021344a6b	176	unsigned char user_ctrl;
oprospero	0:4dd021344a6b	177	unsigned char fifo_en;
oprospero	0:4dd021344a6b	178	unsigned char gyro_cfg;
oprospero	0:4dd021344a6b	179	unsigned char accel_cfg;
oprospero	0:4dd021344a6b	180	unsigned char accel_cfg2;
oprospero	0:4dd021344a6b	181	unsigned char lp_accel_odr;
oprospero	0:4dd021344a6b	182	unsigned char motion_thr;
oprospero	0:4dd021344a6b	183	unsigned char motion_dur;
oprospero	0:4dd021344a6b	184	unsigned char fifo_count_h;
oprospero	0:4dd021344a6b	185	unsigned char fifo_r_w;
oprospero	0:4dd021344a6b	186	unsigned char raw_gyro;
oprospero	0:4dd021344a6b	187	unsigned char raw_accel;
oprospero	0:4dd021344a6b	188	unsigned char temp;
oprospero	0:4dd021344a6b	189	unsigned char int_enable;
oprospero	0:4dd021344a6b	190	unsigned char dmp_int_status;
oprospero	0:4dd021344a6b	191	unsigned char int_status;
oprospero	0:4dd021344a6b	192	unsigned char accel_intel;
oprospero	0:4dd021344a6b	193	unsigned char pwr_mgmt_1;
oprospero	0:4dd021344a6b	194	unsigned char pwr_mgmt_2;
oprospero	0:4dd021344a6b	195	unsigned char int_pin_cfg;
oprospero	0:4dd021344a6b	196	unsigned char mem_r_w;
oprospero	0:4dd021344a6b	197	unsigned char accel_offs;
oprospero	0:4dd021344a6b	198	unsigned char i2c_mst;
oprospero	0:4dd021344a6b	199	unsigned char bank_sel;
oprospero	0:4dd021344a6b	200	unsigned char mem_start_addr;
oprospero	0:4dd021344a6b	201	unsigned char prgm_start_h;
oprospero	0:4dd021344a6b	202	#if defined AK89xx_SECONDARY
oprospero	0:4dd021344a6b	203	unsigned char s0_addr;
oprospero	0:4dd021344a6b	204	unsigned char s0_reg;
oprospero	0:4dd021344a6b	205	unsigned char s0_ctrl;
oprospero	0:4dd021344a6b	206	unsigned char s1_addr;
oprospero	0:4dd021344a6b	207	unsigned char s1_reg;
oprospero	0:4dd021344a6b	208	unsigned char s1_ctrl;
oprospero	0:4dd021344a6b	209	unsigned char s4_ctrl;
oprospero	0:4dd021344a6b	210	unsigned char s0_do;
oprospero	0:4dd021344a6b	211	unsigned char s1_do;
oprospero	0:4dd021344a6b	212	unsigned char i2c_delay_ctrl;
oprospero	0:4dd021344a6b	213	unsigned char raw_compass;
oprospero	0:4dd021344a6b	214	/* The I2C_MST_VDDIO bit is in this register. */
oprospero	0:4dd021344a6b	215	unsigned char yg_offs_tc;
oprospero	0:4dd021344a6b	216	#endif
oprospero	0:4dd021344a6b	217	};
oprospero	0:4dd021344a6b	218
oprospero	0:4dd021344a6b	219	/* Information specific to a particular device. */
oprospero	0:4dd021344a6b	220	struct hw_s {
oprospero	0:4dd021344a6b	221	unsigned char addr;
oprospero	0:4dd021344a6b	222	unsigned short max_fifo;
oprospero	0:4dd021344a6b	223	unsigned char num_reg;
oprospero	0:4dd021344a6b	224	unsigned short temp_sens;
oprospero	0:4dd021344a6b	225	short temp_offset;
oprospero	0:4dd021344a6b	226	unsigned short bank_size;
oprospero	0:4dd021344a6b	227	#if defined AK89xx_SECONDARY
oprospero	0:4dd021344a6b	228	unsigned short compass_fsr;
oprospero	0:4dd021344a6b	229	#endif
oprospero	0:4dd021344a6b	230	};
oprospero	0:4dd021344a6b	231
oprospero	0:4dd021344a6b	232	/* When entering motion interrupt mode, the driver keeps track of the
oprospero	0:4dd021344a6b	233	* previous state so that it can be restored at a later time.
oprospero	0:4dd021344a6b	234	* TODO: This is tacky. Fix it.
oprospero	0:4dd021344a6b	235	*/
oprospero	0:4dd021344a6b	236	struct motion_int_cache_s {
oprospero	0:4dd021344a6b	237	unsigned short gyro_fsr;
oprospero	0:4dd021344a6b	238	unsigned char accel_fsr;
oprospero	0:4dd021344a6b	239	unsigned short lpf;
oprospero	0:4dd021344a6b	240	unsigned short sample_rate;
oprospero	0:4dd021344a6b	241	unsigned char sensors_on;
oprospero	0:4dd021344a6b	242	unsigned char fifo_sensors;
oprospero	0:4dd021344a6b	243	unsigned char dmp_on;
oprospero	0:4dd021344a6b	244	};
oprospero	0:4dd021344a6b	245
oprospero	0:4dd021344a6b	246	/* Cached chip configuration data.
oprospero	0:4dd021344a6b	247	* TODO: A lot of these can be handled with a bitmask.
oprospero	0:4dd021344a6b	248	*/
oprospero	0:4dd021344a6b	249	struct chip_cfg_s {
oprospero	0:4dd021344a6b	250	/* Matches gyro_cfg >> 3 & 0x03 */
oprospero	0:4dd021344a6b	251	unsigned char gyro_fsr;
oprospero	0:4dd021344a6b	252	/* Matches accel_cfg >> 3 & 0x03 */
oprospero	0:4dd021344a6b	253	unsigned char accel_fsr;
oprospero	0:4dd021344a6b	254	/* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
oprospero	0:4dd021344a6b	255	unsigned char sensors;
oprospero	0:4dd021344a6b	256	/* Matches config register. */
oprospero	0:4dd021344a6b	257	unsigned char lpf;
oprospero	0:4dd021344a6b	258	unsigned char clk_src;
oprospero	0:4dd021344a6b	259	/* Sample rate, NOT rate divider. */
oprospero	0:4dd021344a6b	260	unsigned short sample_rate;
oprospero	0:4dd021344a6b	261	/* Matches fifo_en register. */
oprospero	0:4dd021344a6b	262	unsigned char fifo_enable;
oprospero	0:4dd021344a6b	263	/* Matches int enable register. */
oprospero	0:4dd021344a6b	264	unsigned char int_enable;
oprospero	0:4dd021344a6b	265	/* 1 if devices on auxiliary I2C bus appear on the primary. */
oprospero	0:4dd021344a6b	266	unsigned char bypass_mode;
oprospero	0:4dd021344a6b	267	/* 1 if half-sensitivity.
oprospero	0:4dd021344a6b	268	* NOTE: This doesn't belong here, but everything else in hw_s is const,
oprospero	0:4dd021344a6b	269	* and this allows us to save some precious RAM.
oprospero	0:4dd021344a6b	270	*/
oprospero	0:4dd021344a6b	271	unsigned char accel_half;
oprospero	0:4dd021344a6b	272	/* 1 if device in low-power accel-only mode. */
oprospero	0:4dd021344a6b	273	unsigned char lp_accel_mode;
oprospero	0:4dd021344a6b	274	/* 1 if interrupts are only triggered on motion events. */
oprospero	0:4dd021344a6b	275	unsigned char int_motion_only;
oprospero	0:4dd021344a6b	276	struct motion_int_cache_s cache;
oprospero	0:4dd021344a6b	277	/* 1 for active low interrupts. */
oprospero	0:4dd021344a6b	278	unsigned char active_low_int;
oprospero	0:4dd021344a6b	279	/* 1 for latched interrupts. */
oprospero	0:4dd021344a6b	280	unsigned char latched_int;
oprospero	0:4dd021344a6b	281	/* 1 if DMP is enabled. */
oprospero	0:4dd021344a6b	282	unsigned char dmp_on;
oprospero	0:4dd021344a6b	283	/* Ensures that DMP will only be loaded once. */
oprospero	0:4dd021344a6b	284	unsigned char dmp_loaded;
oprospero	0:4dd021344a6b	285	/* Sampling rate used when DMP is enabled. */
oprospero	0:4dd021344a6b	286	unsigned short dmp_sample_rate;
oprospero	0:4dd021344a6b	287	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	288	/* Compass sample rate. */
oprospero	0:4dd021344a6b	289	unsigned short compass_sample_rate;
oprospero	0:4dd021344a6b	290	unsigned char compass_addr;
oprospero	0:4dd021344a6b	291	short mag_sens_adj[3];
oprospero	0:4dd021344a6b	292	#endif
oprospero	0:4dd021344a6b	293	};
oprospero	0:4dd021344a6b	294
oprospero	0:4dd021344a6b	295	/* Information for self-test. */
oprospero	0:4dd021344a6b	296	struct test_s {
oprospero	0:4dd021344a6b	297	unsigned long gyro_sens;
oprospero	0:4dd021344a6b	298	unsigned long accel_sens;
oprospero	0:4dd021344a6b	299	unsigned char reg_rate_div;
oprospero	0:4dd021344a6b	300	unsigned char reg_lpf;
oprospero	0:4dd021344a6b	301	unsigned char reg_gyro_fsr;
oprospero	0:4dd021344a6b	302	unsigned char reg_accel_fsr;
oprospero	0:4dd021344a6b	303	unsigned short wait_ms;
oprospero	0:4dd021344a6b	304	unsigned char packet_thresh;
oprospero	0:4dd021344a6b	305	float min_dps;
oprospero	0:4dd021344a6b	306	float max_dps;
oprospero	0:4dd021344a6b	307	float max_gyro_var;
oprospero	0:4dd021344a6b	308	float min_g;
oprospero	0:4dd021344a6b	309	float max_g;
oprospero	0:4dd021344a6b	310	float max_accel_var;
oprospero	0:4dd021344a6b	311	#ifdef MPU6500
oprospero	0:4dd021344a6b	312	float max_g_offset;
oprospero	0:4dd021344a6b	313	unsigned short sample_wait_ms;
oprospero	0:4dd021344a6b	314	#endif
oprospero	0:4dd021344a6b	315	};
oprospero	0:4dd021344a6b	316
oprospero	0:4dd021344a6b	317	/* Gyro driver state variables. */
oprospero	0:4dd021344a6b	318	struct gyro_state_s {
oprospero	0:4dd021344a6b	319	const struct gyro_reg_s *reg;
oprospero	0:4dd021344a6b	320	const struct hw_s *hw;
oprospero	0:4dd021344a6b	321	struct chip_cfg_s chip_cfg;
oprospero	0:4dd021344a6b	322	const struct test_s *test;
oprospero	0:4dd021344a6b	323	};
oprospero	0:4dd021344a6b	324
oprospero	0:4dd021344a6b	325	/* Filter configurations. */
oprospero	0:4dd021344a6b	326	enum lpf_e {
oprospero	0:4dd021344a6b	327	INV_FILTER_256HZ_NOLPF2 = 0,
oprospero	0:4dd021344a6b	328	INV_FILTER_188HZ,
oprospero	0:4dd021344a6b	329	INV_FILTER_98HZ,
oprospero	0:4dd021344a6b	330	INV_FILTER_42HZ,
oprospero	0:4dd021344a6b	331	INV_FILTER_20HZ,
oprospero	0:4dd021344a6b	332	INV_FILTER_10HZ,
oprospero	0:4dd021344a6b	333	INV_FILTER_5HZ,
oprospero	0:4dd021344a6b	334	INV_FILTER_2100HZ_NOLPF,
oprospero	0:4dd021344a6b	335	NUM_FILTER
oprospero	0:4dd021344a6b	336	};
oprospero	0:4dd021344a6b	337
oprospero	0:4dd021344a6b	338	/* Full scale ranges. */
oprospero	0:4dd021344a6b	339	enum gyro_fsr_e {
oprospero	0:4dd021344a6b	340	INV_FSR_250DPS = 0,
oprospero	0:4dd021344a6b	341	INV_FSR_500DPS,
oprospero	0:4dd021344a6b	342	INV_FSR_1000DPS,
oprospero	0:4dd021344a6b	343	INV_FSR_2000DPS,
oprospero	0:4dd021344a6b	344	NUM_GYRO_FSR
oprospero	0:4dd021344a6b	345	};
oprospero	0:4dd021344a6b	346
oprospero	0:4dd021344a6b	347	/* Full scale ranges. */
oprospero	0:4dd021344a6b	348	enum accel_fsr_e {
oprospero	0:4dd021344a6b	349	INV_FSR_2G = 0,
oprospero	0:4dd021344a6b	350	INV_FSR_4G,
oprospero	0:4dd021344a6b	351	INV_FSR_8G,
oprospero	0:4dd021344a6b	352	INV_FSR_16G,
oprospero	0:4dd021344a6b	353	NUM_ACCEL_FSR
oprospero	0:4dd021344a6b	354	};
oprospero	0:4dd021344a6b	355
oprospero	0:4dd021344a6b	356	/* Clock sources. */
oprospero	0:4dd021344a6b	357	enum clock_sel_e {
oprospero	0:4dd021344a6b	358	INV_CLK_INTERNAL = 0,
oprospero	0:4dd021344a6b	359	INV_CLK_PLL,
oprospero	0:4dd021344a6b	360	NUM_CLK
oprospero	0:4dd021344a6b	361	};
oprospero	0:4dd021344a6b	362
oprospero	0:4dd021344a6b	363	/* Low-power accel wakeup rates. */
oprospero	0:4dd021344a6b	364	enum lp_accel_rate_e {
oprospero	0:4dd021344a6b	365	#if defined MPU6050
oprospero	0:4dd021344a6b	366	INV_LPA_1_25HZ,
oprospero	0:4dd021344a6b	367	INV_LPA_5HZ,
oprospero	0:4dd021344a6b	368	INV_LPA_20HZ,
oprospero	0:4dd021344a6b	369	INV_LPA_40HZ
oprospero	0:4dd021344a6b	370	#elif defined MPU6500
oprospero	0:4dd021344a6b	371	INV_LPA_0_3125HZ,
oprospero	0:4dd021344a6b	372	INV_LPA_0_625HZ,
oprospero	0:4dd021344a6b	373	INV_LPA_1_25HZ,
oprospero	0:4dd021344a6b	374	INV_LPA_2_5HZ,
oprospero	0:4dd021344a6b	375	INV_LPA_5HZ,
oprospero	0:4dd021344a6b	376	INV_LPA_10HZ,
oprospero	0:4dd021344a6b	377	INV_LPA_20HZ,
oprospero	0:4dd021344a6b	378	INV_LPA_40HZ,
oprospero	0:4dd021344a6b	379	INV_LPA_80HZ,
oprospero	0:4dd021344a6b	380	INV_LPA_160HZ,
oprospero	0:4dd021344a6b	381	INV_LPA_320HZ,
oprospero	0:4dd021344a6b	382	INV_LPA_640HZ
oprospero	0:4dd021344a6b	383	#endif
oprospero	0:4dd021344a6b	384	};
oprospero	0:4dd021344a6b	385
oprospero	0:4dd021344a6b	386	#define BIT_I2C_MST_VDDIO (0x80)
oprospero	0:4dd021344a6b	387	#define BIT_FIFO_EN (0x40)
oprospero	0:4dd021344a6b	388	#define BIT_DMP_EN (0x80)
oprospero	0:4dd021344a6b	389	#define BIT_FIFO_RST (0x04)
oprospero	0:4dd021344a6b	390	#define BIT_DMP_RST (0x08)
oprospero	0:4dd021344a6b	391	#define BIT_FIFO_OVERFLOW (0x10)
oprospero	0:4dd021344a6b	392	#define BIT_DATA_RDY_EN (0x01)
oprospero	0:4dd021344a6b	393	#define BIT_DMP_INT_EN (0x02)
oprospero	0:4dd021344a6b	394	#define BIT_MOT_INT_EN (0x40)
oprospero	0:4dd021344a6b	395	#define BITS_FSR (0x18)
oprospero	0:4dd021344a6b	396	#define BITS_LPF (0x07)
oprospero	0:4dd021344a6b	397	#define BITS_HPF (0x07)
oprospero	0:4dd021344a6b	398	#define BITS_CLK (0x07)
oprospero	0:4dd021344a6b	399	#define BIT_FIFO_SIZE_1024 (0x40)
oprospero	0:4dd021344a6b	400	#define BIT_FIFO_SIZE_2048 (0x80)
oprospero	0:4dd021344a6b	401	#define BIT_FIFO_SIZE_4096 (0xC0)
oprospero	0:4dd021344a6b	402	#define BIT_RESET (0x80)
oprospero	0:4dd021344a6b	403	#define BIT_SLEEP (0x40)
oprospero	0:4dd021344a6b	404	#define BIT_S0_DELAY_EN (0x01)
oprospero	0:4dd021344a6b	405	#define BIT_S2_DELAY_EN (0x04)
oprospero	0:4dd021344a6b	406	#define BITS_SLAVE_LENGTH (0x0F)
oprospero	0:4dd021344a6b	407	#define BIT_SLAVE_BYTE_SW (0x40)
oprospero	0:4dd021344a6b	408	#define BIT_SLAVE_GROUP (0x10)
oprospero	0:4dd021344a6b	409	#define BIT_SLAVE_EN (0x80)
oprospero	0:4dd021344a6b	410	#define BIT_I2C_READ (0x80)
oprospero	0:4dd021344a6b	411	#define BITS_I2C_MASTER_DLY (0x1F)
oprospero	0:4dd021344a6b	412	#define BIT_AUX_IF_EN (0x20)
oprospero	0:4dd021344a6b	413	#define BIT_ACTL (0x80)
oprospero	0:4dd021344a6b	414	#define BIT_LATCH_EN (0x20)
oprospero	0:4dd021344a6b	415	#define BIT_ANY_RD_CLR (0x10)
oprospero	0:4dd021344a6b	416	#define BIT_BYPASS_EN (0x02)
oprospero	0:4dd021344a6b	417	#define BITS_WOM_EN (0xC0)
oprospero	0:4dd021344a6b	418	#define BIT_LPA_CYCLE (0x20)
oprospero	0:4dd021344a6b	419	#define BIT_STBY_XA (0x20)
oprospero	0:4dd021344a6b	420	#define BIT_STBY_YA (0x10)
oprospero	0:4dd021344a6b	421	#define BIT_STBY_ZA (0x08)
oprospero	0:4dd021344a6b	422	#define BIT_STBY_XG (0x04)
oprospero	0:4dd021344a6b	423	#define BIT_STBY_YG (0x02)
oprospero	0:4dd021344a6b	424	#define BIT_STBY_ZG (0x01)
oprospero	0:4dd021344a6b	425	#define BIT_STBY_XYZA (BIT_STBY_XA \| BIT_STBY_YA \| BIT_STBY_ZA)
oprospero	0:4dd021344a6b	426	#define BIT_STBY_XYZG (BIT_STBY_XG \| BIT_STBY_YG \| BIT_STBY_ZG)
oprospero	0:4dd021344a6b	427
oprospero	0:4dd021344a6b	428	#if defined AK8975_SECONDARY
oprospero	0:4dd021344a6b	429	#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
oprospero	0:4dd021344a6b	430	#define AK89xx_FSR (9830)
oprospero	0:4dd021344a6b	431	#elif defined AK8963_SECONDARY
oprospero	0:4dd021344a6b	432	#define SUPPORTS_AK89xx_HIGH_SENS (0x10)
oprospero	0:4dd021344a6b	433	#define AK89xx_FSR (4915)
oprospero	0:4dd021344a6b	434	#endif
oprospero	0:4dd021344a6b	435
oprospero	0:4dd021344a6b	436	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	437	#define AKM_REG_WHOAMI (0x00)
oprospero	0:4dd021344a6b	438
oprospero	0:4dd021344a6b	439	#define AKM_REG_ST1 (0x02)
oprospero	0:4dd021344a6b	440	#define AKM_REG_HXL (0x03)
oprospero	0:4dd021344a6b	441	#define AKM_REG_ST2 (0x09)
oprospero	0:4dd021344a6b	442
oprospero	0:4dd021344a6b	443	#define AKM_REG_CNTL (0x0A)
oprospero	0:4dd021344a6b	444	#define AKM_REG_ASTC (0x0C)
oprospero	0:4dd021344a6b	445	#define AKM_REG_ASAX (0x10)
oprospero	0:4dd021344a6b	446	#define AKM_REG_ASAY (0x11)
oprospero	0:4dd021344a6b	447	#define AKM_REG_ASAZ (0x12)
oprospero	0:4dd021344a6b	448
oprospero	0:4dd021344a6b	449	#define AKM_DATA_READY (0x01)
oprospero	0:4dd021344a6b	450	#define AKM_DATA_OVERRUN (0x02)
oprospero	0:4dd021344a6b	451	#define AKM_OVERFLOW (0x80)
oprospero	0:4dd021344a6b	452	#define AKM_DATA_ERROR (0x40)
oprospero	0:4dd021344a6b	453
oprospero	0:4dd021344a6b	454	#define AKM_BIT_SELF_TEST (0x40)
oprospero	0:4dd021344a6b	455
oprospero	0:4dd021344a6b	456	#define AKM_POWER_DOWN (0x00 \| SUPPORTS_AK89xx_HIGH_SENS)
oprospero	0:4dd021344a6b	457	#define AKM_SINGLE_MEASUREMENT (0x01 \| SUPPORTS_AK89xx_HIGH_SENS)
oprospero	0:4dd021344a6b	458	#define AKM_FUSE_ROM_ACCESS (0x0F \| SUPPORTS_AK89xx_HIGH_SENS)
oprospero	0:4dd021344a6b	459	#define AKM_MODE_SELF_TEST (0x08 \| SUPPORTS_AK89xx_HIGH_SENS)
oprospero	0:4dd021344a6b	460
oprospero	0:4dd021344a6b	461	#define AKM_WHOAMI (0x48)
oprospero	0:4dd021344a6b	462	#endif
oprospero	0:4dd021344a6b	463
oprospero	0:4dd021344a6b	464	#if defined MPU6050
oprospero	0:4dd021344a6b	465	const struct gyro_reg_s reg = {
oprospero	0:4dd021344a6b	466	.who_am_i = 0x75,
oprospero	0:4dd021344a6b	467	.rate_div = 0x19,
oprospero	0:4dd021344a6b	468	.lpf = 0x1A,
oprospero	0:4dd021344a6b	469	.prod_id = 0x0C,
oprospero	0:4dd021344a6b	470	.user_ctrl = 0x6A,
oprospero	0:4dd021344a6b	471	.fifo_en = 0x23,
oprospero	0:4dd021344a6b	472	.gyro_cfg = 0x1B,
oprospero	0:4dd021344a6b	473	.accel_cfg = 0x1C,
oprospero	0:4dd021344a6b	474	.motion_thr = 0x1F,
oprospero	0:4dd021344a6b	475	.motion_dur = 0x20,
oprospero	0:4dd021344a6b	476	.fifo_count_h = 0x72,
oprospero	0:4dd021344a6b	477	.fifo_r_w = 0x74,
oprospero	0:4dd021344a6b	478	.raw_gyro = 0x43,
oprospero	0:4dd021344a6b	479	.raw_accel = 0x3B,
oprospero	0:4dd021344a6b	480	.temp = 0x41,
oprospero	0:4dd021344a6b	481	.int_enable = 0x38,
oprospero	0:4dd021344a6b	482	.dmp_int_status = 0x39,
oprospero	0:4dd021344a6b	483	.int_status = 0x3A,
oprospero	0:4dd021344a6b	484	.pwr_mgmt_1 = 0x6B,
oprospero	0:4dd021344a6b	485	.pwr_mgmt_2 = 0x6C,
oprospero	0:4dd021344a6b	486	.int_pin_cfg = 0x37,
oprospero	0:4dd021344a6b	487	.mem_r_w = 0x6F,
oprospero	0:4dd021344a6b	488	.accel_offs = 0x06,
oprospero	0:4dd021344a6b	489	.i2c_mst = 0x24,
oprospero	0:4dd021344a6b	490	.bank_sel = 0x6D,
oprospero	0:4dd021344a6b	491	.mem_start_addr = 0x6E,
oprospero	0:4dd021344a6b	492	.prgm_start_h = 0x70
oprospero	0:4dd021344a6b	493	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	494	,.raw_compass = 0x49,
oprospero	0:4dd021344a6b	495	.yg_offs_tc = 0x01,
oprospero	0:4dd021344a6b	496	.s0_addr = 0x25,
oprospero	0:4dd021344a6b	497	.s0_reg = 0x26,
oprospero	0:4dd021344a6b	498	.s0_ctrl = 0x27,
oprospero	0:4dd021344a6b	499	.s1_addr = 0x28,
oprospero	0:4dd021344a6b	500	.s1_reg = 0x29,
oprospero	0:4dd021344a6b	501	.s1_ctrl = 0x2A,
oprospero	0:4dd021344a6b	502	.s4_ctrl = 0x34,
oprospero	0:4dd021344a6b	503	.s0_do = 0x63,
oprospero	0:4dd021344a6b	504	.s1_do = 0x64,
oprospero	0:4dd021344a6b	505	.i2c_delay_ctrl = 0x67
oprospero	0:4dd021344a6b	506	#endif
oprospero	0:4dd021344a6b	507	};
oprospero	0:4dd021344a6b	508	const struct hw_s hw = {
oprospero	0:4dd021344a6b	509	.addr = 0x68,
oprospero	0:4dd021344a6b	510	.max_fifo = 1024,
oprospero	0:4dd021344a6b	511	.num_reg = 118,
oprospero	0:4dd021344a6b	512	.temp_sens = 340,
oprospero	0:4dd021344a6b	513	.temp_offset = -521,
oprospero	0:4dd021344a6b	514	.bank_size = 256
oprospero	0:4dd021344a6b	515	#if defined AK89xx_SECONDARY
oprospero	0:4dd021344a6b	516	,.compass_fsr = AK89xx_FSR
oprospero	0:4dd021344a6b	517	#endif
oprospero	0:4dd021344a6b	518	};
oprospero	0:4dd021344a6b	519
oprospero	0:4dd021344a6b	520	const struct test_s test = {
oprospero	0:4dd021344a6b	521	.gyro_sens = 32768/250,
oprospero	0:4dd021344a6b	522	.accel_sens = 32768/16,
oprospero	0:4dd021344a6b	523	.reg_rate_div = 0, /* 1kHz. */
oprospero	0:4dd021344a6b	524	.reg_lpf = 1, /* 188Hz. */
oprospero	0:4dd021344a6b	525	.reg_gyro_fsr = 0, /* 250dps. */
oprospero	0:4dd021344a6b	526	.reg_accel_fsr = 0x18, /* 16g. */
oprospero	0:4dd021344a6b	527	.wait_ms = 50,
oprospero	0:4dd021344a6b	528	.packet_thresh = 5, /* 5% */
oprospero	0:4dd021344a6b	529	.min_dps = 10.f,
oprospero	0:4dd021344a6b	530	.max_dps = 105.f,
oprospero	0:4dd021344a6b	531	.max_gyro_var = 0.14f,
oprospero	0:4dd021344a6b	532	.min_g = 0.3f,
oprospero	0:4dd021344a6b	533	.max_g = 0.95f,
oprospero	0:4dd021344a6b	534	.max_accel_var = 0.14f
oprospero	0:4dd021344a6b	535	};
oprospero	0:4dd021344a6b	536
oprospero	0:4dd021344a6b	537	static struct gyro_state_s st = {
oprospero	0:4dd021344a6b	538	.reg = &reg,
oprospero	0:4dd021344a6b	539	.hw = &hw,
oprospero	0:4dd021344a6b	540	.test = &test
oprospero	0:4dd021344a6b	541	};
oprospero	0:4dd021344a6b	542	#elif defined MPU6500
oprospero	0:4dd021344a6b	543	const struct gyro_reg_s reg = {
oprospero	0:4dd021344a6b	544	.who_am_i = 0x75,
oprospero	0:4dd021344a6b	545	.rate_div = 0x19,
oprospero	0:4dd021344a6b	546	.lpf = 0x1A,
oprospero	0:4dd021344a6b	547	.prod_id = 0x0C,
oprospero	0:4dd021344a6b	548	.user_ctrl = 0x6A,
oprospero	0:4dd021344a6b	549	.fifo_en = 0x23,
oprospero	0:4dd021344a6b	550	.gyro_cfg = 0x1B,
oprospero	0:4dd021344a6b	551	.accel_cfg = 0x1C,
oprospero	0:4dd021344a6b	552	.accel_cfg2 = 0x1D,
oprospero	0:4dd021344a6b	553	.lp_accel_odr = 0x1E,
oprospero	0:4dd021344a6b	554	.motion_thr = 0x1F,
oprospero	0:4dd021344a6b	555	.motion_dur = 0x20,
oprospero	0:4dd021344a6b	556	.fifo_count_h = 0x72,
oprospero	0:4dd021344a6b	557	.fifo_r_w = 0x74,
oprospero	0:4dd021344a6b	558	.raw_gyro = 0x43,
oprospero	0:4dd021344a6b	559	.raw_accel = 0x3B,
oprospero	0:4dd021344a6b	560	.temp = 0x41,
oprospero	0:4dd021344a6b	561	.int_enable = 0x38,
oprospero	0:4dd021344a6b	562	.dmp_int_status = 0x39,
oprospero	0:4dd021344a6b	563	.int_status = 0x3A,
oprospero	0:4dd021344a6b	564	.accel_intel = 0x69,
oprospero	0:4dd021344a6b	565	.pwr_mgmt_1 = 0x6B,
oprospero	0:4dd021344a6b	566	.pwr_mgmt_2 = 0x6C,
oprospero	0:4dd021344a6b	567	.int_pin_cfg = 0x37,
oprospero	0:4dd021344a6b	568	.mem_r_w = 0x6F,
oprospero	0:4dd021344a6b	569	.accel_offs = 0x77,
oprospero	0:4dd021344a6b	570	.i2c_mst = 0x24,
oprospero	0:4dd021344a6b	571	.bank_sel = 0x6D,
oprospero	0:4dd021344a6b	572	.mem_start_addr = 0x6E,
oprospero	0:4dd021344a6b	573	.prgm_start_h = 0x70
oprospero	0:4dd021344a6b	574	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	575	,.raw_compass = 0x49,
oprospero	0:4dd021344a6b	576	.s0_addr = 0x25,
oprospero	0:4dd021344a6b	577	.s0_reg = 0x26,
oprospero	0:4dd021344a6b	578	.s0_ctrl = 0x27,
oprospero	0:4dd021344a6b	579	.s1_addr = 0x28,
oprospero	0:4dd021344a6b	580	.s1_reg = 0x29,
oprospero	0:4dd021344a6b	581	.s1_ctrl = 0x2A,
oprospero	0:4dd021344a6b	582	.s4_ctrl = 0x34,
oprospero	0:4dd021344a6b	583	.s0_do = 0x63,
oprospero	0:4dd021344a6b	584	.s1_do = 0x64,
oprospero	0:4dd021344a6b	585	.i2c_delay_ctrl = 0x67
oprospero	0:4dd021344a6b	586	#endif
oprospero	0:4dd021344a6b	587	};
oprospero	0:4dd021344a6b	588	const struct hw_s hw = {
oprospero	0:4dd021344a6b	589	.addr = 0x68,
oprospero	0:4dd021344a6b	590	.max_fifo = 1024,
oprospero	0:4dd021344a6b	591	.num_reg = 128,
oprospero	0:4dd021344a6b	592	.temp_sens = 321,
oprospero	0:4dd021344a6b	593	.temp_offset = 0,
oprospero	0:4dd021344a6b	594	.bank_size = 256
oprospero	0:4dd021344a6b	595	#if defined AK89xx_SECONDARY
oprospero	0:4dd021344a6b	596	,.compass_fsr = AK89xx_FSR
oprospero	0:4dd021344a6b	597	#endif
oprospero	0:4dd021344a6b	598	};
oprospero	0:4dd021344a6b	599
oprospero	0:4dd021344a6b	600	const struct test_s test = {
oprospero	0:4dd021344a6b	601	.gyro_sens = 32768/250,
oprospero	0:4dd021344a6b	602	.accel_sens = 32768/2, //FSR = +-2G = 16384 LSB/G
oprospero	0:4dd021344a6b	603	.reg_rate_div = 0, /* 1kHz. */
oprospero	0:4dd021344a6b	604	.reg_lpf = 2, /* 92Hz low pass filter*/
oprospero	0:4dd021344a6b	605	.reg_gyro_fsr = 0, /* 250dps. */
oprospero	0:4dd021344a6b	606	.reg_accel_fsr = 0x0, /* Accel FSR setting = 2g. */
oprospero	0:4dd021344a6b	607	.wait_ms = 200, //200ms stabilization time
oprospero	0:4dd021344a6b	608	.packet_thresh = 200, /* 200 samples */
oprospero	0:4dd021344a6b	609	.min_dps = 20.f, //20 dps for Gyro Criteria C
oprospero	0:4dd021344a6b	610	.max_dps = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B
oprospero	0:4dd021344a6b	611	.max_gyro_var = .5f, //Must exceed +50% variation for Gyro Criteria A
oprospero	0:4dd021344a6b	612	.min_g = .225f, //Accel must exceed Min 225 mg for Criteria B
oprospero	0:4dd021344a6b	613	.max_g = .675f, //Accel cannot exceed Max 675 mg for Criteria B
oprospero	0:4dd021344a6b	614	.max_accel_var = .5f, //Accel must be within 50% variation for Criteria A
oprospero	0:4dd021344a6b	615	.max_g_offset = .5f, //500 mg for Accel Criteria C
oprospero	0:4dd021344a6b	616	.sample_wait_ms = 10 //10ms sample time wait
oprospero	0:4dd021344a6b	617	};
oprospero	0:4dd021344a6b	618
oprospero	0:4dd021344a6b	619	static struct gyro_state_s st = {
oprospero	0:4dd021344a6b	620	.reg = &reg,
oprospero	0:4dd021344a6b	621	.hw = &hw,
oprospero	0:4dd021344a6b	622	.test = &test
oprospero	0:4dd021344a6b	623	};
oprospero	0:4dd021344a6b	624	#endif
oprospero	0:4dd021344a6b	625
oprospero	0:4dd021344a6b	626	#define MAX_PACKET_LENGTH (12)
oprospero	0:4dd021344a6b	627	#ifdef MPU6500
oprospero	0:4dd021344a6b	628	#define HWST_MAX_PACKET_LENGTH (512)
oprospero	0:4dd021344a6b	629	#endif
oprospero	0:4dd021344a6b	630	//
oprospero	0:4dd021344a6b	631	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	632	//static int setup_compass(void);
oprospero	0:4dd021344a6b	633	#define MAX_COMPASS_SAMPLE_RATE (100)
oprospero	0:4dd021344a6b	634	#endif
oprospero	0:4dd021344a6b	635
oprospero	0:4dd021344a6b	636	/**
oprospero	0:4dd021344a6b	637	* @brief Enable/disable data ready interrupt.
oprospero	0:4dd021344a6b	638	* If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
oprospero	0:4dd021344a6b	639	* interrupt is used.
oprospero	0:4dd021344a6b	640	* @param[in] enable 1 to enable interrupt.
oprospero	0:4dd021344a6b	641	* @return 0 if successful.
oprospero	0:4dd021344a6b	642	*/
oprospero	0:4dd021344a6b	643	//static int set_int_enable(unsigned char enable)
oprospero	0:4dd021344a6b	644	int InvMpu::set_int_enable(unsigned char enable)
oprospero	0:4dd021344a6b	645	{
oprospero	0:4dd021344a6b	646	unsigned char tmp;
oprospero	0:4dd021344a6b	647
oprospero	0:4dd021344a6b	648	if (st.chip_cfg.dmp_on) {
oprospero	0:4dd021344a6b	649	if (enable)
oprospero	0:4dd021344a6b	650	tmp = BIT_DMP_INT_EN;
oprospero	0:4dd021344a6b	651	else
oprospero	0:4dd021344a6b	652	tmp = 0x00;
oprospero	0:4dd021344a6b	653	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, &tmp))
oprospero	0:4dd021344a6b	654	return -1;
oprospero	0:4dd021344a6b	655	st.chip_cfg.int_enable = tmp;
oprospero	0:4dd021344a6b	656	} else {
oprospero	0:4dd021344a6b	657	if (!st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	658	return -1;
oprospero	0:4dd021344a6b	659	if (enable && st.chip_cfg.int_enable)
oprospero	0:4dd021344a6b	660	return 0;
oprospero	0:4dd021344a6b	661	if (enable)
oprospero	0:4dd021344a6b	662	tmp = BIT_DATA_RDY_EN;
oprospero	0:4dd021344a6b	663	else
oprospero	0:4dd021344a6b	664	tmp = 0x00;
oprospero	0:4dd021344a6b	665	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, &tmp))
oprospero	0:4dd021344a6b	666	return -1;
oprospero	0:4dd021344a6b	667	st.chip_cfg.int_enable = tmp;
oprospero	0:4dd021344a6b	668	}
oprospero	0:4dd021344a6b	669	return 0;
oprospero	0:4dd021344a6b	670	}
oprospero	0:4dd021344a6b	671
oprospero	0:4dd021344a6b	672
oprospero	0:4dd021344a6b	673	InvMpu::InvMpu() : i2Cdev()
oprospero	0:4dd021344a6b	674	{
oprospero	0:4dd021344a6b	675	i2Cdev.frequency(400000);
oprospero	0:4dd021344a6b	676	}
oprospero	0:4dd021344a6b	677
oprospero	0:4dd021344a6b	678	int InvMpu::getDeviceID()
oprospero	0:4dd021344a6b	679	{
oprospero	0:4dd021344a6b	680	uint8_t buffer;
oprospero	0:4dd021344a6b	681	i2Cdev.readBits(st.hw->addr, reg.who_am_i, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &buffer);
oprospero	0:4dd021344a6b	682	// i2Cdev.readByte(st.hw->addr, reg.who_am_i, &buffer);
oprospero	0:4dd021344a6b	683	// uint8_t mask = ((1 << 6) - 1) << (1);
oprospero	0:4dd021344a6b	684	// buffer &= mask;
oprospero	0:4dd021344a6b	685	// buffer >>= 1;
oprospero	0:4dd021344a6b	686	// *data = b;
oprospero	0:4dd021344a6b	687	return buffer;
oprospero	0:4dd021344a6b	688	}
oprospero	0:4dd021344a6b	689
oprospero	0:4dd021344a6b	690	bool InvMpu::testConnection()
oprospero	0:4dd021344a6b	691	{
oprospero	0:4dd021344a6b	692	char deviceId = getDeviceID();
oprospero	0:4dd021344a6b	693	PRINTF("Device ID: %d 0x%X",deviceId,deviceId);
oprospero	0:4dd021344a6b	694	BREAK;
oprospero	0:4dd021344a6b	695	return deviceId == 0x34;
oprospero	0:4dd021344a6b	696	}
oprospero	0:4dd021344a6b	697	/**
oprospero	0:4dd021344a6b	698	* @brief Register dump for testing.
oprospero	0:4dd021344a6b	699	* @return 0 if successful.
oprospero	0:4dd021344a6b	700	*/
oprospero	0:4dd021344a6b	701	int InvMpu::mpu_reg_dump(void)
oprospero	0:4dd021344a6b	702	{
oprospero	0:4dd021344a6b	703	unsigned char ii;
oprospero	0:4dd021344a6b	704	unsigned char data;
oprospero	0:4dd021344a6b	705
oprospero	0:4dd021344a6b	706	for (ii = 0; ii < st.hw->num_reg; ii++) {
oprospero	0:4dd021344a6b	707	if (ii == st.reg->fifo_r_w \|\| ii == st.reg->mem_r_w)
oprospero	0:4dd021344a6b	708	continue;
oprospero	0:4dd021344a6b	709	if (i2Cdev.readBytes(st.hw->addr, ii, 1, &data))
oprospero	0:4dd021344a6b	710	return -1;
oprospero	0:4dd021344a6b	711	log_i("%#5x: %#5x\r\n", ii, data);
oprospero	0:4dd021344a6b	712	}
oprospero	0:4dd021344a6b	713	return 0;
oprospero	0:4dd021344a6b	714	}
oprospero	0:4dd021344a6b	715
oprospero	0:4dd021344a6b	716	/**
oprospero	0:4dd021344a6b	717	* @brief Read from a single register.
oprospero	0:4dd021344a6b	718	* NOTE: The memory and FIFO read/write registers cannot be accessed.
oprospero	0:4dd021344a6b	719	* @param[in] reg Register address.
oprospero	0:4dd021344a6b	720	* @param[out] data Register data.
oprospero	0:4dd021344a6b	721	* @return 0 if successful.
oprospero	0:4dd021344a6b	722	*/
oprospero	0:4dd021344a6b	723	int InvMpu::mpu_read_reg(unsigned char reg, unsigned char *data)
oprospero	0:4dd021344a6b	724	{
oprospero	0:4dd021344a6b	725	if (reg == st.reg->fifo_r_w \|\| reg == st.reg->mem_r_w)
oprospero	0:4dd021344a6b	726	return -1;
oprospero	0:4dd021344a6b	727	if (reg >= st.hw->num_reg)
oprospero	0:4dd021344a6b	728	return -1;
oprospero	0:4dd021344a6b	729	return i2Cdev.readBytes(st.hw->addr, reg, 1, data);
oprospero	0:4dd021344a6b	730	}
oprospero	0:4dd021344a6b	731
oprospero	0:4dd021344a6b	732	/**
oprospero	0:4dd021344a6b	733	* @brief Initialize hardware.
oprospero	0:4dd021344a6b	734	* Initial configuration:\n
oprospero	0:4dd021344a6b	735	* Gyro FSR: +/- 2000DPS\n
oprospero	0:4dd021344a6b	736	* Accel FSR +/- 2G\n
oprospero	0:4dd021344a6b	737	* DLPF: 42Hz\n
oprospero	0:4dd021344a6b	738	* FIFO rate: 50Hz\n
oprospero	0:4dd021344a6b	739	* Clock source: Gyro PLL\n
oprospero	0:4dd021344a6b	740	* FIFO: Disabled.\n
oprospero	0:4dd021344a6b	741	* Data ready interrupt: Disabled, active low, unlatched.
oprospero	0:4dd021344a6b	742	* @param[in] int_param Platform-specific parameters to interrupt API.
oprospero	0:4dd021344a6b	743	* @return 0 if successful.
oprospero	0:4dd021344a6b	744	*/
oprospero	0:4dd021344a6b	745	int InvMpu::mpu_init(void)
oprospero	0:4dd021344a6b	746	{
oprospero	0:4dd021344a6b	747	unsigned char data[6];
oprospero	0:4dd021344a6b	748
oprospero	1:dbbd3dcd61c9	749	PRINTLN("mpu_init");
oprospero	0:4dd021344a6b	750	/* Reset device. */
oprospero	0:4dd021344a6b	751	data[0] = BIT_RESET;
oprospero	0:4dd021344a6b	752	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data) )
oprospero	0:4dd021344a6b	753	{
oprospero	0:4dd021344a6b	754	return -1;
oprospero	0:4dd021344a6b	755	}
oprospero	1:dbbd3dcd61c9	756	PRINTLN("Device Reset");
oprospero	0:4dd021344a6b	757	wait_ms(100);
oprospero	0:4dd021344a6b	758
oprospero	0:4dd021344a6b	759	/* Wake up chip. */
oprospero	0:4dd021344a6b	760	data[0] = 0x00;
oprospero	0:4dd021344a6b	761	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
oprospero	0:4dd021344a6b	762	return -1;
oprospero	1:dbbd3dcd61c9	763	PRINTLN("Wake up Chip");
oprospero	0:4dd021344a6b	764
oprospero	0:4dd021344a6b	765	st.chip_cfg.accel_half = 0;
oprospero	0:4dd021344a6b	766
oprospero	0:4dd021344a6b	767	#ifdef MPU6500
oprospero	0:4dd021344a6b	768	/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
oprospero	0:4dd021344a6b	769	* first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
oprospero	0:4dd021344a6b	770	*/
oprospero	0:4dd021344a6b	771	data[0] = BIT_FIFO_SIZE_1024 \| 0x8;
oprospero	0:4dd021344a6b	772	if (i2Cdev.writeBytes(st.hw->addr, st.reg->accel_cfg2, 1, data))
oprospero	0:4dd021344a6b	773	return -1;
oprospero	0:4dd021344a6b	774	#endif
oprospero	0:4dd021344a6b	775
oprospero	0:4dd021344a6b	776	/* Set to invalid values to ensure no I2C writes are skipped. */
oprospero	0:4dd021344a6b	777	st.chip_cfg.sensors = 0xFF;
oprospero	0:4dd021344a6b	778	st.chip_cfg.gyro_fsr = 0xFF;
oprospero	0:4dd021344a6b	779	st.chip_cfg.accel_fsr = 0xFF;
oprospero	0:4dd021344a6b	780	st.chip_cfg.lpf = 0xFF;
oprospero	0:4dd021344a6b	781	st.chip_cfg.sample_rate = 0xFFFF;
oprospero	0:4dd021344a6b	782	st.chip_cfg.fifo_enable = 0xFF;
oprospero	0:4dd021344a6b	783	st.chip_cfg.bypass_mode = 0xFF;
oprospero	0:4dd021344a6b	784	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	785	st.chip_cfg.compass_sample_rate = 0xFFFF;
oprospero	0:4dd021344a6b	786	#endif
oprospero	0:4dd021344a6b	787	/* mpu_set_sensors always preserves this setting. */
oprospero	0:4dd021344a6b	788	st.chip_cfg.clk_src = INV_CLK_PLL;
oprospero	0:4dd021344a6b	789	/* Handled in next call to mpu_set_bypass. */
oprospero	0:4dd021344a6b	790	st.chip_cfg.active_low_int = 1;
oprospero	0:4dd021344a6b	791	st.chip_cfg.latched_int = 0;
oprospero	0:4dd021344a6b	792	st.chip_cfg.int_motion_only = 0;
oprospero	0:4dd021344a6b	793	st.chip_cfg.lp_accel_mode = 0;
oprospero	0:4dd021344a6b	794	memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
oprospero	0:4dd021344a6b	795	st.chip_cfg.dmp_on = 0;
oprospero	0:4dd021344a6b	796	st.chip_cfg.dmp_loaded = 0;
oprospero	0:4dd021344a6b	797	st.chip_cfg.dmp_sample_rate = 0;
oprospero	0:4dd021344a6b	798
oprospero	0:4dd021344a6b	799	if (mpu_set_gyro_fsr(2000))
oprospero	0:4dd021344a6b	800	return -1;
oprospero	1:dbbd3dcd61c9	801	PRINTLN("mpu_set_gyro_fsr");
oprospero	0:4dd021344a6b	802	if (mpu_set_accel_fsr(2))
oprospero	0:4dd021344a6b	803	return -1;
oprospero	1:dbbd3dcd61c9	804	PRINTLN("mpu_set_accel_fsr");
oprospero	0:4dd021344a6b	805	if (mpu_set_lpf(42))
oprospero	0:4dd021344a6b	806	return -1;
oprospero	1:dbbd3dcd61c9	807	PRINTLN("mpu_set_lpf");
oprospero	0:4dd021344a6b	808	if (mpu_set_sample_rate(50))
oprospero	0:4dd021344a6b	809	return -1;
oprospero	1:dbbd3dcd61c9	810	PRINTLN("mpu_set_sample_rate");
oprospero	0:4dd021344a6b	811	if (mpu_configure_fifo(0))
oprospero	0:4dd021344a6b	812	return -1;
oprospero	1:dbbd3dcd61c9	813	PRINTLN("mpu_configure_fifo");
oprospero	0:4dd021344a6b	814
oprospero	0:4dd021344a6b	815	// if (int_param)
oprospero	0:4dd021344a6b	816	// reg_int_cb(int_param);
oprospero	0:4dd021344a6b	817
oprospero	0:4dd021344a6b	818	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	819	setup_compass();
oprospero	0:4dd021344a6b	820	if (mpu_set_compass_sample_rate(10))
oprospero	0:4dd021344a6b	821	return -1;
oprospero	0:4dd021344a6b	822	#else
oprospero	0:4dd021344a6b	823	/* Already disabled by setup_compass. */
oprospero	0:4dd021344a6b	824	if (mpu_set_bypass(0))
oprospero	0:4dd021344a6b	825	return -1;
oprospero	0:4dd021344a6b	826	#endif
oprospero	1:dbbd3dcd61c9	827	PRINTLN("mpu_set_compass_sample_rate");
oprospero	0:4dd021344a6b	828
oprospero	0:4dd021344a6b	829	mpu_set_sensors(0);
oprospero	0:4dd021344a6b	830	return 0;
oprospero	0:4dd021344a6b	831	}
oprospero	0:4dd021344a6b	832
oprospero	0:4dd021344a6b	833	/**
oprospero	0:4dd021344a6b	834	* @brief Enter low-power accel-only mode.
oprospero	0:4dd021344a6b	835	* In low-power accel mode, the chip goes to sleep and only wakes up to sample
oprospero	0:4dd021344a6b	836	* the accelerometer at one of the following frequencies:
oprospero	0:4dd021344a6b	837	* \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
oprospero	0:4dd021344a6b	838	* \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
oprospero	0:4dd021344a6b	839	* \n If the requested rate is not one listed above, the device will be set to
oprospero	0:4dd021344a6b	840	* the next highest rate. Requesting a rate above the maximum supported
oprospero	0:4dd021344a6b	841	* frequency will result in an error.
oprospero	0:4dd021344a6b	842	* \n To select a fractional wake-up frequency, round down the value passed to
oprospero	0:4dd021344a6b	843	* @e rate.
oprospero	0:4dd021344a6b	844	* @param[in] rate Minimum sampling rate, or zero to disable LP
oprospero	0:4dd021344a6b	845	* accel mode.
oprospero	0:4dd021344a6b	846	* @return 0 if successful.
oprospero	0:4dd021344a6b	847	*/
oprospero	0:4dd021344a6b	848	int InvMpu::mpu_lp_accel_mode(unsigned char rate)
oprospero	0:4dd021344a6b	849	{
oprospero	0:4dd021344a6b	850	unsigned char tmp[2];
oprospero	0:4dd021344a6b	851
oprospero	0:4dd021344a6b	852	if (rate > 40)
oprospero	0:4dd021344a6b	853	return -1;
oprospero	0:4dd021344a6b	854
oprospero	0:4dd021344a6b	855	if (!rate) {
oprospero	0:4dd021344a6b	856	mpu_set_int_latched(0);
oprospero	0:4dd021344a6b	857	tmp[0] = 0;
oprospero	0:4dd021344a6b	858	tmp[1] = BIT_STBY_XYZG;
oprospero	0:4dd021344a6b	859	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
oprospero	0:4dd021344a6b	860	return -1;
oprospero	0:4dd021344a6b	861	st.chip_cfg.lp_accel_mode = 0;
oprospero	0:4dd021344a6b	862	return 0;
oprospero	0:4dd021344a6b	863	}
oprospero	0:4dd021344a6b	864	/* For LP accel, we automatically configure the hardware to produce latched
oprospero	0:4dd021344a6b	865	* interrupts. In LP accel mode, the hardware cycles into sleep mode before
oprospero	0:4dd021344a6b	866	* it gets a chance to deassert the interrupt pin; therefore, we shift this
oprospero	0:4dd021344a6b	867	* responsibility over to the MCU.
oprospero	0:4dd021344a6b	868	*
oprospero	0:4dd021344a6b	869	* Any register read will clear the interrupt.
oprospero	0:4dd021344a6b	870	*/
oprospero	0:4dd021344a6b	871	mpu_set_int_latched(1);
oprospero	0:4dd021344a6b	872	#if defined MPU6050
oprospero	0:4dd021344a6b	873	tmp[0] = BIT_LPA_CYCLE;
oprospero	0:4dd021344a6b	874	if (rate == 1) {
oprospero	0:4dd021344a6b	875	tmp[1] = INV_LPA_1_25HZ;
oprospero	0:4dd021344a6b	876	mpu_set_lpf(5);
oprospero	0:4dd021344a6b	877	} else if (rate <= 5) {
oprospero	0:4dd021344a6b	878	tmp[1] = INV_LPA_5HZ;
oprospero	0:4dd021344a6b	879	mpu_set_lpf(5);
oprospero	0:4dd021344a6b	880	} else if (rate <= 20) {
oprospero	0:4dd021344a6b	881	tmp[1] = INV_LPA_20HZ;
oprospero	0:4dd021344a6b	882	mpu_set_lpf(10);
oprospero	0:4dd021344a6b	883	} else {
oprospero	0:4dd021344a6b	884	tmp[1] = INV_LPA_40HZ;
oprospero	0:4dd021344a6b	885	mpu_set_lpf(20);
oprospero	0:4dd021344a6b	886	}
oprospero	0:4dd021344a6b	887	tmp[1] = (tmp[1] << 6) \| BIT_STBY_XYZG;
oprospero	0:4dd021344a6b	888	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
oprospero	0:4dd021344a6b	889	return -1;
oprospero	0:4dd021344a6b	890	#elif defined MPU6500
oprospero	0:4dd021344a6b	891	/* Set wake frequency. */
oprospero	0:4dd021344a6b	892	if (rate == 1)
oprospero	0:4dd021344a6b	893	tmp[0] = INV_LPA_1_25HZ;
oprospero	0:4dd021344a6b	894	else if (rate == 2)
oprospero	0:4dd021344a6b	895	tmp[0] = INV_LPA_2_5HZ;
oprospero	0:4dd021344a6b	896	else if (rate <= 5)
oprospero	0:4dd021344a6b	897	tmp[0] = INV_LPA_5HZ;
oprospero	0:4dd021344a6b	898	else if (rate <= 10)
oprospero	0:4dd021344a6b	899	tmp[0] = INV_LPA_10HZ;
oprospero	0:4dd021344a6b	900	else if (rate <= 20)
oprospero	0:4dd021344a6b	901	tmp[0] = INV_LPA_20HZ;
oprospero	0:4dd021344a6b	902	else if (rate <= 40)
oprospero	0:4dd021344a6b	903	tmp[0] = INV_LPA_40HZ;
oprospero	0:4dd021344a6b	904	else if (rate <= 80)
oprospero	0:4dd021344a6b	905	tmp[0] = INV_LPA_80HZ;
oprospero	0:4dd021344a6b	906	else if (rate <= 160)
oprospero	0:4dd021344a6b	907	tmp[0] = INV_LPA_160HZ;
oprospero	0:4dd021344a6b	908	else if (rate <= 320)
oprospero	0:4dd021344a6b	909	tmp[0] = INV_LPA_320HZ;
oprospero	0:4dd021344a6b	910	else
oprospero	0:4dd021344a6b	911	tmp[0] = INV_LPA_640HZ;
oprospero	0:4dd021344a6b	912	if (i2Cdev.writeBytes(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
oprospero	0:4dd021344a6b	913	return -1;
oprospero	0:4dd021344a6b	914	tmp[0] = BIT_LPA_CYCLE;
oprospero	0:4dd021344a6b	915	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
oprospero	0:4dd021344a6b	916	return -1;
oprospero	0:4dd021344a6b	917	#endif
oprospero	0:4dd021344a6b	918	st.chip_cfg.sensors = INV_XYZ_ACCEL;
oprospero	0:4dd021344a6b	919	st.chip_cfg.clk_src = 0;
oprospero	0:4dd021344a6b	920	st.chip_cfg.lp_accel_mode = 1;
oprospero	0:4dd021344a6b	921	mpu_configure_fifo(0);
oprospero	0:4dd021344a6b	922
oprospero	0:4dd021344a6b	923	return 0;
oprospero	0:4dd021344a6b	924	}
oprospero	0:4dd021344a6b	925
oprospero	0:4dd021344a6b	926	/**
oprospero	0:4dd021344a6b	927	* @brief Read raw gyro data directly from the registers.
oprospero	0:4dd021344a6b	928	* @param[out] data Raw data in hardware units.
oprospero	0:4dd021344a6b	929	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
oprospero	0:4dd021344a6b	930	* @return 0 if successful.
oprospero	0:4dd021344a6b	931	*/
oprospero	0:4dd021344a6b	932	int InvMpu::mpu_get_gyro_reg(short data, unsigned long timestamp)
oprospero	0:4dd021344a6b	933	{
oprospero	0:4dd021344a6b	934	unsigned char tmp[6];
oprospero	0:4dd021344a6b	935
oprospero	0:4dd021344a6b	936	if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
oprospero	0:4dd021344a6b	937	return -1;
oprospero	0:4dd021344a6b	938
oprospero	0:4dd021344a6b	939	if (i2Cdev.readBytes(st.hw->addr, st.reg->raw_gyro, 6, tmp) )
oprospero	0:4dd021344a6b	940	return -1;
oprospero	0:4dd021344a6b	941	data[0] = (tmp[0] << 8) \| tmp[1];
oprospero	0:4dd021344a6b	942	data[1] = (tmp[2] << 8) \| tmp[3];
oprospero	0:4dd021344a6b	943	data[2] = (tmp[4] << 8) \| tmp[5];
oprospero	0:4dd021344a6b	944	if (timestamp)
oprospero	0:4dd021344a6b	945	get_ms(timestamp);
oprospero	0:4dd021344a6b	946	return 0;
oprospero	0:4dd021344a6b	947	}
oprospero	0:4dd021344a6b	948
oprospero	0:4dd021344a6b	949	/**
oprospero	0:4dd021344a6b	950	* @brief Read raw accel data directly from the registers.
oprospero	0:4dd021344a6b	951	* @param[out] data Raw data in hardware units.
oprospero	0:4dd021344a6b	952	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
oprospero	0:4dd021344a6b	953	* @return 0 if successful.
oprospero	0:4dd021344a6b	954	*/
oprospero	0:4dd021344a6b	955	int InvMpu::mpu_get_accel_reg(short data, unsigned long timestamp)
oprospero	0:4dd021344a6b	956	{
oprospero	0:4dd021344a6b	957	unsigned char tmp[6];
oprospero	0:4dd021344a6b	958
oprospero	0:4dd021344a6b	959	if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
oprospero	0:4dd021344a6b	960	return -1;
oprospero	0:4dd021344a6b	961
oprospero	0:4dd021344a6b	962	if (i2Cdev.readBytes(st.hw->addr, st.reg->raw_accel, 6, tmp))
oprospero	0:4dd021344a6b	963	return -1;
oprospero	0:4dd021344a6b	964	data[0] = (tmp[0] << 8) \| tmp[1];
oprospero	0:4dd021344a6b	965	data[1] = (tmp[2] << 8) \| tmp[3];
oprospero	0:4dd021344a6b	966	data[2] = (tmp[4] << 8) \| tmp[5];
oprospero	0:4dd021344a6b	967	if (timestamp)
oprospero	0:4dd021344a6b	968	get_ms(timestamp);
oprospero	0:4dd021344a6b	969	return 0;
oprospero	0:4dd021344a6b	970	}
oprospero	0:4dd021344a6b	971
oprospero	0:4dd021344a6b	972	/**
oprospero	0:4dd021344a6b	973	* @brief Read temperature data directly from the registers.
oprospero	0:4dd021344a6b	974	* @param[out] data Data in q16 format.
oprospero	0:4dd021344a6b	975	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
oprospero	0:4dd021344a6b	976	* @return 0 if successful.
oprospero	0:4dd021344a6b	977	*/
oprospero	0:4dd021344a6b	978	int InvMpu::mpu_get_temperature(long data, unsigned long timestamp)
oprospero	0:4dd021344a6b	979	{
oprospero	0:4dd021344a6b	980	unsigned char tmp[2];
oprospero	0:4dd021344a6b	981	short raw;
oprospero	0:4dd021344a6b	982
oprospero	0:4dd021344a6b	983	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	984	return -1;
oprospero	0:4dd021344a6b	985
oprospero	0:4dd021344a6b	986	if (i2Cdev.readBytes(st.hw->addr, st.reg->temp, 2, tmp))
oprospero	0:4dd021344a6b	987	return -1;
oprospero	0:4dd021344a6b	988	raw = (tmp[0] << 8) \| tmp[1];
oprospero	0:4dd021344a6b	989	if (timestamp)
oprospero	0:4dd021344a6b	990	get_ms(timestamp);
oprospero	0:4dd021344a6b	991
oprospero	0:4dd021344a6b	992	data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
oprospero	0:4dd021344a6b	993	return 0;
oprospero	0:4dd021344a6b	994	}
oprospero	0:4dd021344a6b	995
oprospero	0:4dd021344a6b	996	/**
oprospero	0:4dd021344a6b	997	* @brief Read biases to the accel bias 6500 registers.
oprospero	0:4dd021344a6b	998	* This function reads from the MPU6500 accel offset cancellations registers.
oprospero	0:4dd021344a6b	999	* The format are G in +-8G format. The register is initialized with OTP
oprospero	0:4dd021344a6b	1000	* factory trim values.
oprospero	0:4dd021344a6b	1001	* @param[in] accel_bias returned structure with the accel bias
oprospero	0:4dd021344a6b	1002	* @return 0 if successful.
oprospero	0:4dd021344a6b	1003	*/
oprospero	0:4dd021344a6b	1004	int InvMpu::mpu_read_6500_accel_bias(long *accel_bias) {
oprospero	0:4dd021344a6b	1005	unsigned char data[6];
oprospero	0:4dd021344a6b	1006	if (i2Cdev.readBytes(st.hw->addr, 0x77, 2, &data[0]))
oprospero	0:4dd021344a6b	1007	return -1;
oprospero	0:4dd021344a6b	1008	if (i2Cdev.readBytes(st.hw->addr, 0x7A, 2, &data[2]))
oprospero	0:4dd021344a6b	1009	return -1;
oprospero	0:4dd021344a6b	1010	if (i2Cdev.readBytes(st.hw->addr, 0x7D, 2, &data[4]))
oprospero	0:4dd021344a6b	1011	return -1;
oprospero	0:4dd021344a6b	1012	accel_bias[0] = ((long)data[0]<<8) \| data[1];
oprospero	0:4dd021344a6b	1013	accel_bias[1] = ((long)data[2]<<8) \| data[3];
oprospero	0:4dd021344a6b	1014	accel_bias[2] = ((long)data[4]<<8) \| data[5];
oprospero	0:4dd021344a6b	1015	return 0;
oprospero	0:4dd021344a6b	1016	}
oprospero	0:4dd021344a6b	1017
oprospero	0:4dd021344a6b	1018	/**
oprospero	0:4dd021344a6b	1019	* @brief Read biases to the accel bias 6050 registers.
oprospero	0:4dd021344a6b	1020	* This function reads from the MPU6050 accel offset cancellations registers.
oprospero	0:4dd021344a6b	1021	* The format are G in +-8G format. The register is initialized with OTP
oprospero	0:4dd021344a6b	1022	* factory trim values.
oprospero	0:4dd021344a6b	1023	* @param[in] accel_bias returned structure with the accel bias
oprospero	0:4dd021344a6b	1024	* @return 0 if successful.
oprospero	0:4dd021344a6b	1025	*/
oprospero	0:4dd021344a6b	1026	int InvMpu::mpu_read_6050_accel_bias(long *accel_bias) {
oprospero	0:4dd021344a6b	1027	unsigned char data[6];
oprospero	0:4dd021344a6b	1028	if (i2Cdev.readBytes(st.hw->addr, 0x06, 2, &data[0]))
oprospero	0:4dd021344a6b	1029	return -1;
oprospero	0:4dd021344a6b	1030	if (i2Cdev.readBytes(st.hw->addr, 0x08, 2, &data[2]))
oprospero	0:4dd021344a6b	1031	return -1;
oprospero	0:4dd021344a6b	1032	if (i2Cdev.readBytes(st.hw->addr, 0x0A, 2, &data[4]))
oprospero	0:4dd021344a6b	1033	return -1;
oprospero	0:4dd021344a6b	1034	accel_bias[0] = ((long)data[0]<<8) \| data[1];
oprospero	0:4dd021344a6b	1035	accel_bias[1] = ((long)data[2]<<8) \| data[3];
oprospero	0:4dd021344a6b	1036	accel_bias[2] = ((long)data[4]<<8) \| data[5];
oprospero	0:4dd021344a6b	1037	return 0;
oprospero	0:4dd021344a6b	1038	}
oprospero	0:4dd021344a6b	1039
oprospero	0:4dd021344a6b	1040	int InvMpu::mpu_read_6500_gyro_bias(long *gyro_bias) {
oprospero	0:4dd021344a6b	1041	unsigned char data[6];
oprospero	0:4dd021344a6b	1042	if (i2Cdev.readBytes(st.hw->addr, 0x13, 2, &data[0]))
oprospero	0:4dd021344a6b	1043	return -1;
oprospero	0:4dd021344a6b	1044	if (i2Cdev.readBytes(st.hw->addr, 0x15, 2, &data[2]))
oprospero	0:4dd021344a6b	1045	return -1;
oprospero	0:4dd021344a6b	1046	if (i2Cdev.readBytes(st.hw->addr, 0x17, 2, &data[4]))
oprospero	0:4dd021344a6b	1047	return -1;
oprospero	0:4dd021344a6b	1048	gyro_bias[0] = ((long)data[0]<<8) \| data[1];
oprospero	0:4dd021344a6b	1049	gyro_bias[1] = ((long)data[2]<<8) \| data[3];
oprospero	0:4dd021344a6b	1050	gyro_bias[2] = ((long)data[4]<<8) \| data[5];
oprospero	0:4dd021344a6b	1051	return 0;
oprospero	0:4dd021344a6b	1052	}
oprospero	0:4dd021344a6b	1053
oprospero	0:4dd021344a6b	1054	/**
oprospero	0:4dd021344a6b	1055	* @brief Push biases to the gyro bias 6500/6050 registers.
oprospero	0:4dd021344a6b	1056	* This function expects biases relative to the current sensor output, and
oprospero	0:4dd021344a6b	1057	* these biases will be added to the factory-supplied values. Bias inputs are LSB
oprospero	0:4dd021344a6b	1058	* in +-1000dps format.
oprospero	0:4dd021344a6b	1059	* @param[in] gyro_bias New biases.
oprospero	0:4dd021344a6b	1060	* @return 0 if successful.
oprospero	0:4dd021344a6b	1061	*/
oprospero	0:4dd021344a6b	1062	int InvMpu::mpu_set_gyro_bias_reg(long *gyro_bias)
oprospero	0:4dd021344a6b	1063	{
oprospero	0:4dd021344a6b	1064	unsigned char data[6] = {0, 0, 0, 0, 0, 0};
oprospero	0:4dd021344a6b	1065	int i=0;
oprospero	0:4dd021344a6b	1066	for(i=0;i<3;i++) {
oprospero	0:4dd021344a6b	1067	gyro_bias[i]= (-gyro_bias[i]);
oprospero	0:4dd021344a6b	1068	}
oprospero	0:4dd021344a6b	1069	data[0] = (gyro_bias[0] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1070	data[1] = (gyro_bias[0]) & 0xff;
oprospero	0:4dd021344a6b	1071	data[2] = (gyro_bias[1] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1072	data[3] = (gyro_bias[1]) & 0xff;
oprospero	0:4dd021344a6b	1073	data[4] = (gyro_bias[2] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1074	data[5] = (gyro_bias[2]) & 0xff;
oprospero	0:4dd021344a6b	1075	if (i2Cdev.writeBytes(st.hw->addr, 0x13, 2, &data[0]))
oprospero	0:4dd021344a6b	1076	return -1;
oprospero	0:4dd021344a6b	1077	if (i2Cdev.writeBytes(st.hw->addr, 0x15, 2, &data[2]))
oprospero	0:4dd021344a6b	1078	return -1;
oprospero	0:4dd021344a6b	1079	if (i2Cdev.writeBytes(st.hw->addr, 0x17, 2, &data[4]))
oprospero	0:4dd021344a6b	1080	return -1;
oprospero	0:4dd021344a6b	1081	return 0;
oprospero	0:4dd021344a6b	1082	}
oprospero	0:4dd021344a6b	1083
oprospero	0:4dd021344a6b	1084	/**
oprospero	0:4dd021344a6b	1085	* @brief Push biases to the accel bias 6050 registers.
oprospero	0:4dd021344a6b	1086	* This function expects biases relative to the current sensor output, and
oprospero	0:4dd021344a6b	1087	* these biases will be added to the factory-supplied values. Bias inputs are LSB
oprospero	0:4dd021344a6b	1088	* in +-8G format.
oprospero	0:4dd021344a6b	1089	* @param[in] accel_bias New biases.
oprospero	0:4dd021344a6b	1090	* @return 0 if successful.
oprospero	0:4dd021344a6b	1091	*/
oprospero	0:4dd021344a6b	1092	int InvMpu::mpu_set_accel_bias_6050_reg(const long *accel_bias)
oprospero	0:4dd021344a6b	1093	{
oprospero	0:4dd021344a6b	1094	unsigned char data[6] = {0, 0, 0, 0, 0, 0};
oprospero	0:4dd021344a6b	1095	long accel_reg_bias[3] = {0, 0, 0};
oprospero	0:4dd021344a6b	1096	long mask = 0x0001;
oprospero	0:4dd021344a6b	1097	unsigned char mask_bit[3] = {0, 0, 0};
oprospero	0:4dd021344a6b	1098	unsigned char i = 0;
oprospero	0:4dd021344a6b	1099	if(mpu_read_6050_accel_bias(accel_reg_bias))
oprospero	0:4dd021344a6b	1100	return -1;
oprospero	0:4dd021344a6b	1101
oprospero	0:4dd021344a6b	1102	//bit 0 of the 2 byte bias is for temp comp
oprospero	0:4dd021344a6b	1103	//calculations need to compensate for this and not change it
oprospero	0:4dd021344a6b	1104	for(i=0; i<3; i++) {
oprospero	0:4dd021344a6b	1105	if(accel_reg_bias[i]&mask)
oprospero	0:4dd021344a6b	1106	mask_bit[i] = 0x01;
oprospero	0:4dd021344a6b	1107	}
oprospero	0:4dd021344a6b	1108
oprospero	0:4dd021344a6b	1109	accel_reg_bias[0] -= accel_bias[0];
oprospero	0:4dd021344a6b	1110	accel_reg_bias[1] -= accel_bias[1];
oprospero	0:4dd021344a6b	1111	accel_reg_bias[2] -= accel_bias[2];
oprospero	0:4dd021344a6b	1112
oprospero	0:4dd021344a6b	1113	data[0] = (accel_reg_bias[0] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1114	data[1] = (accel_reg_bias[0]) & 0xff;
oprospero	0:4dd021344a6b	1115	data[1] = data[1]\|mask_bit[0];
oprospero	0:4dd021344a6b	1116	data[2] = (accel_reg_bias[1] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1117	data[3] = (accel_reg_bias[1]) & 0xff;
oprospero	0:4dd021344a6b	1118	data[3] = data[3]\|mask_bit[1];
oprospero	0:4dd021344a6b	1119	data[4] = (accel_reg_bias[2] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1120	data[5] = (accel_reg_bias[2]) & 0xff;
oprospero	0:4dd021344a6b	1121	data[5] = data[5]\|mask_bit[2];
oprospero	0:4dd021344a6b	1122
oprospero	0:4dd021344a6b	1123	if (i2Cdev.writeBytes(st.hw->addr, 0x06, 2, &data[0]))
oprospero	0:4dd021344a6b	1124	return -1;
oprospero	0:4dd021344a6b	1125	if (i2Cdev.writeBytes(st.hw->addr, 0x08, 2, &data[2]))
oprospero	0:4dd021344a6b	1126	return -1;
oprospero	0:4dd021344a6b	1127	if (i2Cdev.writeBytes(st.hw->addr, 0x0A, 2, &data[4]))
oprospero	0:4dd021344a6b	1128	return -1;
oprospero	0:4dd021344a6b	1129
oprospero	0:4dd021344a6b	1130	return 0;
oprospero	0:4dd021344a6b	1131	}
oprospero	0:4dd021344a6b	1132
oprospero	0:4dd021344a6b	1133
oprospero	0:4dd021344a6b	1134	/**
oprospero	0:4dd021344a6b	1135	* @brief Push biases to the accel bias 6500 registers.
oprospero	0:4dd021344a6b	1136	* This function expects biases relative to the current sensor output, and
oprospero	0:4dd021344a6b	1137	* these biases will be added to the factory-supplied values. Bias inputs are LSB
oprospero	0:4dd021344a6b	1138	* in +-8G format.
oprospero	0:4dd021344a6b	1139	* @param[in] accel_bias New biases.
oprospero	0:4dd021344a6b	1140	* @return 0 if successful.
oprospero	0:4dd021344a6b	1141	*/
oprospero	0:4dd021344a6b	1142	int InvMpu::mpu_set_accel_bias_6500_reg(const long *accel_bias)
oprospero	0:4dd021344a6b	1143	{
oprospero	0:4dd021344a6b	1144	unsigned char data[6] = {0, 0, 0, 0, 0, 0};
oprospero	0:4dd021344a6b	1145	long accel_reg_bias[3] = {0, 0, 0};
oprospero	0:4dd021344a6b	1146	long mask = 0x0001;
oprospero	0:4dd021344a6b	1147	unsigned char mask_bit[3] = {0, 0, 0};
oprospero	0:4dd021344a6b	1148	unsigned char i = 0;
oprospero	0:4dd021344a6b	1149
oprospero	0:4dd021344a6b	1150	if(mpu_read_6500_accel_bias(accel_reg_bias))
oprospero	0:4dd021344a6b	1151	return -1;
oprospero	0:4dd021344a6b	1152
oprospero	0:4dd021344a6b	1153	//bit 0 of the 2 byte bias is for temp comp
oprospero	0:4dd021344a6b	1154	//calculations need to compensate for this
oprospero	0:4dd021344a6b	1155	for(i=0; i<3; i++) {
oprospero	0:4dd021344a6b	1156	if(accel_reg_bias[i]&mask)
oprospero	0:4dd021344a6b	1157	mask_bit[i] = 0x01;
oprospero	0:4dd021344a6b	1158	}
oprospero	0:4dd021344a6b	1159
oprospero	0:4dd021344a6b	1160	accel_reg_bias[0] -= accel_bias[0];
oprospero	0:4dd021344a6b	1161	accel_reg_bias[1] -= accel_bias[1];
oprospero	0:4dd021344a6b	1162	accel_reg_bias[2] -= accel_bias[2];
oprospero	0:4dd021344a6b	1163
oprospero	0:4dd021344a6b	1164	data[0] = (accel_reg_bias[0] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1165	data[1] = (accel_reg_bias[0]) & 0xff;
oprospero	0:4dd021344a6b	1166	data[1] = data[1]\|mask_bit[0];
oprospero	0:4dd021344a6b	1167	data[2] = (accel_reg_bias[1] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1168	data[3] = (accel_reg_bias[1]) & 0xff;
oprospero	0:4dd021344a6b	1169	data[3] = data[3]\|mask_bit[1];
oprospero	0:4dd021344a6b	1170	data[4] = (accel_reg_bias[2] >> 8) & 0xff;
oprospero	0:4dd021344a6b	1171	data[5] = (accel_reg_bias[2]) & 0xff;
oprospero	0:4dd021344a6b	1172	data[5] = data[5]\|mask_bit[2];
oprospero	0:4dd021344a6b	1173
oprospero	0:4dd021344a6b	1174	if (i2Cdev.writeBytes(st.hw->addr, 0x77, 2, &data[0]))
oprospero	0:4dd021344a6b	1175	return -1;
oprospero	0:4dd021344a6b	1176	if (i2Cdev.writeBytes(st.hw->addr, 0x7A, 2, &data[2]))
oprospero	0:4dd021344a6b	1177	return -1;
oprospero	0:4dd021344a6b	1178	if (i2Cdev.writeBytes(st.hw->addr, 0x7D, 2, &data[4]))
oprospero	0:4dd021344a6b	1179	return -1;
oprospero	0:4dd021344a6b	1180
oprospero	0:4dd021344a6b	1181	return 0;
oprospero	0:4dd021344a6b	1182	}
oprospero	0:4dd021344a6b	1183
oprospero	0:4dd021344a6b	1184	/**
oprospero	0:4dd021344a6b	1185	* @brief Reset FIFO read/write pointers.
oprospero	0:4dd021344a6b	1186	* @return 0 if successful.
oprospero	0:4dd021344a6b	1187	*/
oprospero	0:4dd021344a6b	1188	int InvMpu::mpu_reset_fifo(void)
oprospero	0:4dd021344a6b	1189	{
oprospero	0:4dd021344a6b	1190	unsigned char data;
oprospero	0:4dd021344a6b	1191
oprospero	0:4dd021344a6b	1192	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	1193	return -1;
oprospero	0:4dd021344a6b	1194
oprospero	0:4dd021344a6b	1195	data = 0;
oprospero	0:4dd021344a6b	1196	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, &data))
oprospero	0:4dd021344a6b	1197	return -1;
oprospero	0:4dd021344a6b	1198	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, &data))
oprospero	0:4dd021344a6b	1199	return -1;
oprospero	0:4dd021344a6b	1200	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &data))
oprospero	0:4dd021344a6b	1201	return -1;
oprospero	0:4dd021344a6b	1202
oprospero	0:4dd021344a6b	1203	if (st.chip_cfg.dmp_on) {
oprospero	0:4dd021344a6b	1204	data = BIT_FIFO_RST \| BIT_DMP_RST;
oprospero	0:4dd021344a6b	1205	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &data))
oprospero	0:4dd021344a6b	1206	return -1;
oprospero	0:4dd021344a6b	1207	wait_ms(1);
oprospero	0:4dd021344a6b	1208	data = BIT_DMP_EN \| BIT_FIFO_EN;
oprospero	0:4dd021344a6b	1209	if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
oprospero	0:4dd021344a6b	1210	data \|= BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1211	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &data))
oprospero	0:4dd021344a6b	1212	return -1;
oprospero	0:4dd021344a6b	1213	if (st.chip_cfg.int_enable)
oprospero	0:4dd021344a6b	1214	data = BIT_DMP_INT_EN;
oprospero	0:4dd021344a6b	1215	else
oprospero	0:4dd021344a6b	1216	data = 0;
oprospero	0:4dd021344a6b	1217	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, &data))
oprospero	0:4dd021344a6b	1218	return -1;
oprospero	0:4dd021344a6b	1219	data = 0;
oprospero	0:4dd021344a6b	1220	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, &data))
oprospero	0:4dd021344a6b	1221	return -1;
oprospero	0:4dd021344a6b	1222	} else {
oprospero	0:4dd021344a6b	1223	data = BIT_FIFO_RST;
oprospero	0:4dd021344a6b	1224	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &data))
oprospero	0:4dd021344a6b	1225	return -1;
oprospero	0:4dd021344a6b	1226	if (st.chip_cfg.bypass_mode \|\| !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
oprospero	0:4dd021344a6b	1227	data = BIT_FIFO_EN;
oprospero	0:4dd021344a6b	1228	else
oprospero	0:4dd021344a6b	1229	data = BIT_FIFO_EN \| BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1230	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &data))
oprospero	0:4dd021344a6b	1231	return -1;
oprospero	0:4dd021344a6b	1232	wait_ms(1);
oprospero	0:4dd021344a6b	1233	if (st.chip_cfg.int_enable)
oprospero	0:4dd021344a6b	1234	data = BIT_DATA_RDY_EN;
oprospero	0:4dd021344a6b	1235	else
oprospero	0:4dd021344a6b	1236	data = 0;
oprospero	0:4dd021344a6b	1237	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, &data))
oprospero	0:4dd021344a6b	1238	return -1;
oprospero	0:4dd021344a6b	1239	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
oprospero	0:4dd021344a6b	1240	return -1;
oprospero	0:4dd021344a6b	1241	}
oprospero	0:4dd021344a6b	1242	return 0;
oprospero	0:4dd021344a6b	1243	}
oprospero	0:4dd021344a6b	1244
oprospero	0:4dd021344a6b	1245	/**
oprospero	0:4dd021344a6b	1246	* @brief Get the gyro full-scale range.
oprospero	0:4dd021344a6b	1247	* @param[out] fsr Current full-scale range.
oprospero	0:4dd021344a6b	1248	* @return 0 if successful.
oprospero	0:4dd021344a6b	1249	*/
oprospero	0:4dd021344a6b	1250	int InvMpu::mpu_get_gyro_fsr(unsigned short *fsr)
oprospero	0:4dd021344a6b	1251	{
oprospero	0:4dd021344a6b	1252	switch (st.chip_cfg.gyro_fsr) {
oprospero	0:4dd021344a6b	1253	case INV_FSR_250DPS:
oprospero	0:4dd021344a6b	1254	fsr[0] = 250;
oprospero	0:4dd021344a6b	1255	break;
oprospero	0:4dd021344a6b	1256	case INV_FSR_500DPS:
oprospero	0:4dd021344a6b	1257	fsr[0] = 500;
oprospero	0:4dd021344a6b	1258	break;
oprospero	0:4dd021344a6b	1259	case INV_FSR_1000DPS:
oprospero	0:4dd021344a6b	1260	fsr[0] = 1000;
oprospero	0:4dd021344a6b	1261	break;
oprospero	0:4dd021344a6b	1262	case INV_FSR_2000DPS:
oprospero	0:4dd021344a6b	1263	fsr[0] = 2000;
oprospero	0:4dd021344a6b	1264	break;
oprospero	0:4dd021344a6b	1265	default:
oprospero	0:4dd021344a6b	1266	fsr[0] = 0;
oprospero	0:4dd021344a6b	1267	break;
oprospero	0:4dd021344a6b	1268	}
oprospero	0:4dd021344a6b	1269	return 0;
oprospero	0:4dd021344a6b	1270	}
oprospero	0:4dd021344a6b	1271
oprospero	0:4dd021344a6b	1272	/**
oprospero	0:4dd021344a6b	1273	* @brief Set the gyro full-scale range.
oprospero	0:4dd021344a6b	1274	* @param[in] fsr Desired full-scale range.
oprospero	0:4dd021344a6b	1275	* @return 0 if successful.
oprospero	0:4dd021344a6b	1276	*/
oprospero	0:4dd021344a6b	1277	int InvMpu::mpu_set_gyro_fsr(unsigned short fsr)
oprospero	0:4dd021344a6b	1278	{
oprospero	0:4dd021344a6b	1279	unsigned char data;
oprospero	0:4dd021344a6b	1280
oprospero	0:4dd021344a6b	1281	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	1282	return -1;
oprospero	0:4dd021344a6b	1283
oprospero	0:4dd021344a6b	1284	switch (fsr) {
oprospero	0:4dd021344a6b	1285	case 250:
oprospero	0:4dd021344a6b	1286	data = INV_FSR_250DPS << 3;
oprospero	0:4dd021344a6b	1287	break;
oprospero	0:4dd021344a6b	1288	case 500:
oprospero	0:4dd021344a6b	1289	data = INV_FSR_500DPS << 3;
oprospero	0:4dd021344a6b	1290	break;
oprospero	0:4dd021344a6b	1291	case 1000:
oprospero	0:4dd021344a6b	1292	data = INV_FSR_1000DPS << 3;
oprospero	0:4dd021344a6b	1293	break;
oprospero	0:4dd021344a6b	1294	case 2000:
oprospero	0:4dd021344a6b	1295	data = INV_FSR_2000DPS << 3;
oprospero	0:4dd021344a6b	1296	break;
oprospero	0:4dd021344a6b	1297	default:
oprospero	0:4dd021344a6b	1298	return -1;
oprospero	0:4dd021344a6b	1299	}
oprospero	0:4dd021344a6b	1300
oprospero	0:4dd021344a6b	1301	if (st.chip_cfg.gyro_fsr == (data >> 3))
oprospero	0:4dd021344a6b	1302	return 0;
oprospero	0:4dd021344a6b	1303	if (i2Cdev.writeBytes(st.hw->addr, st.reg->gyro_cfg, 1, &data))
oprospero	0:4dd021344a6b	1304	return -1;
oprospero	0:4dd021344a6b	1305	st.chip_cfg.gyro_fsr = data >> 3;
oprospero	0:4dd021344a6b	1306	return 0;
oprospero	0:4dd021344a6b	1307	}
oprospero	0:4dd021344a6b	1308
oprospero	0:4dd021344a6b	1309	/**
oprospero	0:4dd021344a6b	1310	* @brief Get the accel full-scale range.
oprospero	0:4dd021344a6b	1311	* @param[out] fsr Current full-scale range.
oprospero	0:4dd021344a6b	1312	* @return 0 if successful.
oprospero	0:4dd021344a6b	1313	*/
oprospero	0:4dd021344a6b	1314	int InvMpu::mpu_get_accel_fsr(unsigned char *fsr)
oprospero	0:4dd021344a6b	1315	{
oprospero	0:4dd021344a6b	1316	switch (st.chip_cfg.accel_fsr) {
oprospero	0:4dd021344a6b	1317	case INV_FSR_2G:
oprospero	0:4dd021344a6b	1318	fsr[0] = 2;
oprospero	0:4dd021344a6b	1319	break;
oprospero	0:4dd021344a6b	1320	case INV_FSR_4G:
oprospero	0:4dd021344a6b	1321	fsr[0] = 4;
oprospero	0:4dd021344a6b	1322	break;
oprospero	0:4dd021344a6b	1323	case INV_FSR_8G:
oprospero	0:4dd021344a6b	1324	fsr[0] = 8;
oprospero	0:4dd021344a6b	1325	break;
oprospero	0:4dd021344a6b	1326	case INV_FSR_16G:
oprospero	0:4dd021344a6b	1327	fsr[0] = 16;
oprospero	0:4dd021344a6b	1328	break;
oprospero	0:4dd021344a6b	1329	default:
oprospero	0:4dd021344a6b	1330	return -1;
oprospero	0:4dd021344a6b	1331	}
oprospero	0:4dd021344a6b	1332	if (st.chip_cfg.accel_half)
oprospero	0:4dd021344a6b	1333	fsr[0] <<= 1;
oprospero	0:4dd021344a6b	1334	return 0;
oprospero	0:4dd021344a6b	1335	}
oprospero	0:4dd021344a6b	1336
oprospero	0:4dd021344a6b	1337	/**
oprospero	0:4dd021344a6b	1338	* @brief Set the accel full-scale range.
oprospero	0:4dd021344a6b	1339	* @param[in] fsr Desired full-scale range.
oprospero	0:4dd021344a6b	1340	* @return 0 if successful.
oprospero	0:4dd021344a6b	1341	*/
oprospero	0:4dd021344a6b	1342	int InvMpu::mpu_set_accel_fsr(unsigned char fsr)
oprospero	0:4dd021344a6b	1343	{
oprospero	0:4dd021344a6b	1344	unsigned char data;
oprospero	0:4dd021344a6b	1345
oprospero	0:4dd021344a6b	1346	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	1347	return -1;
oprospero	0:4dd021344a6b	1348
oprospero	0:4dd021344a6b	1349	switch (fsr) {
oprospero	0:4dd021344a6b	1350	case 2:
oprospero	0:4dd021344a6b	1351	data = INV_FSR_2G << 3;
oprospero	0:4dd021344a6b	1352	break;
oprospero	0:4dd021344a6b	1353	case 4:
oprospero	0:4dd021344a6b	1354	data = INV_FSR_4G << 3;
oprospero	0:4dd021344a6b	1355	break;
oprospero	0:4dd021344a6b	1356	case 8:
oprospero	0:4dd021344a6b	1357	data = INV_FSR_8G << 3;
oprospero	0:4dd021344a6b	1358	break;
oprospero	0:4dd021344a6b	1359	case 16:
oprospero	0:4dd021344a6b	1360	data = INV_FSR_16G << 3;
oprospero	0:4dd021344a6b	1361	break;
oprospero	0:4dd021344a6b	1362	default:
oprospero	0:4dd021344a6b	1363	return -1;
oprospero	0:4dd021344a6b	1364	}
oprospero	0:4dd021344a6b	1365
oprospero	0:4dd021344a6b	1366	if (st.chip_cfg.accel_fsr == (data >> 3))
oprospero	0:4dd021344a6b	1367	return 0;
oprospero	0:4dd021344a6b	1368	if (i2Cdev.writeBytes(st.hw->addr, st.reg->accel_cfg, 1, &data))
oprospero	0:4dd021344a6b	1369	return -1;
oprospero	0:4dd021344a6b	1370	st.chip_cfg.accel_fsr = data >> 3;
oprospero	0:4dd021344a6b	1371	return 0;
oprospero	0:4dd021344a6b	1372	}
oprospero	0:4dd021344a6b	1373
oprospero	0:4dd021344a6b	1374	/**
oprospero	0:4dd021344a6b	1375	* @brief Get the current DLPF setting.
oprospero	0:4dd021344a6b	1376	* @param[out] lpf Current LPF setting.
oprospero	0:4dd021344a6b	1377	* 0 if successful.
oprospero	0:4dd021344a6b	1378	*/
oprospero	0:4dd021344a6b	1379	int InvMpu::mpu_get_lpf(unsigned short *lpf)
oprospero	0:4dd021344a6b	1380	{
oprospero	0:4dd021344a6b	1381	switch (st.chip_cfg.lpf) {
oprospero	0:4dd021344a6b	1382	case INV_FILTER_188HZ:
oprospero	0:4dd021344a6b	1383	lpf[0] = 188;
oprospero	0:4dd021344a6b	1384	break;
oprospero	0:4dd021344a6b	1385	case INV_FILTER_98HZ:
oprospero	0:4dd021344a6b	1386	lpf[0] = 98;
oprospero	0:4dd021344a6b	1387	break;
oprospero	0:4dd021344a6b	1388	case INV_FILTER_42HZ:
oprospero	0:4dd021344a6b	1389	lpf[0] = 42;
oprospero	0:4dd021344a6b	1390	break;
oprospero	0:4dd021344a6b	1391	case INV_FILTER_20HZ:
oprospero	0:4dd021344a6b	1392	lpf[0] = 20;
oprospero	0:4dd021344a6b	1393	break;
oprospero	0:4dd021344a6b	1394	case INV_FILTER_10HZ:
oprospero	0:4dd021344a6b	1395	lpf[0] = 10;
oprospero	0:4dd021344a6b	1396	break;
oprospero	0:4dd021344a6b	1397	case INV_FILTER_5HZ:
oprospero	0:4dd021344a6b	1398	lpf[0] = 5;
oprospero	0:4dd021344a6b	1399	break;
oprospero	0:4dd021344a6b	1400	case INV_FILTER_256HZ_NOLPF2:
oprospero	0:4dd021344a6b	1401	case INV_FILTER_2100HZ_NOLPF:
oprospero	0:4dd021344a6b	1402	default:
oprospero	0:4dd021344a6b	1403	lpf[0] = 0;
oprospero	0:4dd021344a6b	1404	break;
oprospero	0:4dd021344a6b	1405	}
oprospero	0:4dd021344a6b	1406	return 0;
oprospero	0:4dd021344a6b	1407	}
oprospero	0:4dd021344a6b	1408
oprospero	0:4dd021344a6b	1409	/**
oprospero	0:4dd021344a6b	1410	* @brief Set digital low pass filter.
oprospero	0:4dd021344a6b	1411	* The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
oprospero	0:4dd021344a6b	1412	* @param[in] lpf Desired LPF setting.
oprospero	0:4dd021344a6b	1413	* @return 0 if successful.
oprospero	0:4dd021344a6b	1414	*/
oprospero	0:4dd021344a6b	1415	int InvMpu::mpu_set_lpf(unsigned short lpf)
oprospero	0:4dd021344a6b	1416	{
oprospero	0:4dd021344a6b	1417	unsigned char data;
oprospero	0:4dd021344a6b	1418
oprospero	0:4dd021344a6b	1419	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	1420	return -1;
oprospero	0:4dd021344a6b	1421
oprospero	0:4dd021344a6b	1422	if (lpf >= 188)
oprospero	0:4dd021344a6b	1423	data = INV_FILTER_188HZ;
oprospero	0:4dd021344a6b	1424	else if (lpf >= 98)
oprospero	0:4dd021344a6b	1425	data = INV_FILTER_98HZ;
oprospero	0:4dd021344a6b	1426	else if (lpf >= 42)
oprospero	0:4dd021344a6b	1427	data = INV_FILTER_42HZ;
oprospero	0:4dd021344a6b	1428	else if (lpf >= 20)
oprospero	0:4dd021344a6b	1429	data = INV_FILTER_20HZ;
oprospero	0:4dd021344a6b	1430	else if (lpf >= 10)
oprospero	0:4dd021344a6b	1431	data = INV_FILTER_10HZ;
oprospero	0:4dd021344a6b	1432	else
oprospero	0:4dd021344a6b	1433	data = INV_FILTER_5HZ;
oprospero	0:4dd021344a6b	1434
oprospero	0:4dd021344a6b	1435	if (st.chip_cfg.lpf == data)
oprospero	0:4dd021344a6b	1436	return 0;
oprospero	0:4dd021344a6b	1437	if (i2Cdev.writeBytes(st.hw->addr, st.reg->lpf, 1, &data))
oprospero	0:4dd021344a6b	1438	return -1;
oprospero	0:4dd021344a6b	1439	st.chip_cfg.lpf = data;
oprospero	0:4dd021344a6b	1440	return 0;
oprospero	0:4dd021344a6b	1441	}
oprospero	0:4dd021344a6b	1442
oprospero	0:4dd021344a6b	1443	/**
oprospero	0:4dd021344a6b	1444	* @brief Get sampling rate.
oprospero	0:4dd021344a6b	1445	* @param[out] rate Current sampling rate (Hz).
oprospero	0:4dd021344a6b	1446	* @return 0 if successful.
oprospero	0:4dd021344a6b	1447	*/
oprospero	0:4dd021344a6b	1448	int InvMpu::mpu_get_sample_rate(unsigned short *rate)
oprospero	0:4dd021344a6b	1449	{
oprospero	0:4dd021344a6b	1450	if (st.chip_cfg.dmp_on)
oprospero	0:4dd021344a6b	1451	return -1;
oprospero	0:4dd021344a6b	1452	else
oprospero	0:4dd021344a6b	1453	rate[0] = st.chip_cfg.sample_rate;
oprospero	0:4dd021344a6b	1454	return 0;
oprospero	0:4dd021344a6b	1455	}
oprospero	0:4dd021344a6b	1456
oprospero	0:4dd021344a6b	1457	/**
oprospero	0:4dd021344a6b	1458	* @brief Set sampling rate.
oprospero	0:4dd021344a6b	1459	* Sampling rate must be between 4Hz and 1kHz.
oprospero	0:4dd021344a6b	1460	* @param[in] rate Desired sampling rate (Hz).
oprospero	0:4dd021344a6b	1461	* @return 0 if successful.
oprospero	0:4dd021344a6b	1462	*/
oprospero	0:4dd021344a6b	1463	int InvMpu::mpu_set_sample_rate(unsigned short rate)
oprospero	0:4dd021344a6b	1464	{
oprospero	0:4dd021344a6b	1465	unsigned char data;
oprospero	0:4dd021344a6b	1466
oprospero	0:4dd021344a6b	1467	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	1468	return -1;
oprospero	0:4dd021344a6b	1469
oprospero	0:4dd021344a6b	1470	if (st.chip_cfg.dmp_on)
oprospero	0:4dd021344a6b	1471	return -1;
oprospero	0:4dd021344a6b	1472	else {
oprospero	0:4dd021344a6b	1473	if (st.chip_cfg.lp_accel_mode) {
oprospero	0:4dd021344a6b	1474	if (rate && (rate <= 40)) {
oprospero	0:4dd021344a6b	1475	/* Just stay in low-power accel mode. */
oprospero	0:4dd021344a6b	1476	mpu_lp_accel_mode(rate);
oprospero	0:4dd021344a6b	1477	return 0;
oprospero	0:4dd021344a6b	1478	}
oprospero	0:4dd021344a6b	1479	/* Requested rate exceeds the allowed frequencies in LP accel mode,
oprospero	0:4dd021344a6b	1480	* switch back to full-power mode.
oprospero	0:4dd021344a6b	1481	*/
oprospero	0:4dd021344a6b	1482	mpu_lp_accel_mode(0);
oprospero	0:4dd021344a6b	1483	}
oprospero	0:4dd021344a6b	1484	if (rate < 4)
oprospero	0:4dd021344a6b	1485	rate = 4;
oprospero	0:4dd021344a6b	1486	else if (rate > 1000)
oprospero	0:4dd021344a6b	1487	rate = 1000;
oprospero	0:4dd021344a6b	1488
oprospero	0:4dd021344a6b	1489	data = 1000 / rate - 1;
oprospero	0:4dd021344a6b	1490	if (i2Cdev.writeBytes(st.hw->addr, st.reg->rate_div, 1, &data))
oprospero	0:4dd021344a6b	1491	return -1;
oprospero	0:4dd021344a6b	1492
oprospero	0:4dd021344a6b	1493	st.chip_cfg.sample_rate = 1000 / (1 + data);
oprospero	0:4dd021344a6b	1494
oprospero	0:4dd021344a6b	1495	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	1496	mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
oprospero	0:4dd021344a6b	1497	#endif
oprospero	0:4dd021344a6b	1498
oprospero	0:4dd021344a6b	1499	/* Automatically set LPF to 1/2 sampling rate. */
oprospero	0:4dd021344a6b	1500	mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
oprospero	0:4dd021344a6b	1501	return 0;
oprospero	0:4dd021344a6b	1502	}
oprospero	0:4dd021344a6b	1503	}
oprospero	0:4dd021344a6b	1504
oprospero	0:4dd021344a6b	1505	/**
oprospero	0:4dd021344a6b	1506	* @brief Get compass sampling rate.
oprospero	0:4dd021344a6b	1507	* @param[out] rate Current compass sampling rate (Hz).
oprospero	0:4dd021344a6b	1508	* @return 0 if successful.
oprospero	0:4dd021344a6b	1509	*/
oprospero	0:4dd021344a6b	1510	int InvMpu::mpu_get_compass_sample_rate(unsigned short *rate)
oprospero	0:4dd021344a6b	1511	{
oprospero	0:4dd021344a6b	1512	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	1513	rate[0] = st.chip_cfg.compass_sample_rate;
oprospero	0:4dd021344a6b	1514	return 0;
oprospero	0:4dd021344a6b	1515	#else
oprospero	0:4dd021344a6b	1516	rate[0] = 0;
oprospero	0:4dd021344a6b	1517	return -1;
oprospero	0:4dd021344a6b	1518	#endif
oprospero	0:4dd021344a6b	1519	}
oprospero	0:4dd021344a6b	1520
oprospero	0:4dd021344a6b	1521	/**
oprospero	0:4dd021344a6b	1522	* @brief Set compass sampling rate.
oprospero	0:4dd021344a6b	1523	* The compass on the auxiliary I2C bus is read by the MPU hardware at a
oprospero	0:4dd021344a6b	1524	* maximum of 100Hz. The actual rate can be set to a fraction of the gyro
oprospero	0:4dd021344a6b	1525	* sampling rate.
oprospero	0:4dd021344a6b	1526	*
oprospero	0:4dd021344a6b	1527	* \n WARNING: The new rate may be different than what was requested. Call
oprospero	0:4dd021344a6b	1528	* mpu_get_compass_sample_rate to check the actual setting.
oprospero	0:4dd021344a6b	1529	* @param[in] rate Desired compass sampling rate (Hz).
oprospero	0:4dd021344a6b	1530	* @return 0 if successful.
oprospero	0:4dd021344a6b	1531	*/
oprospero	0:4dd021344a6b	1532	int InvMpu::mpu_set_compass_sample_rate(unsigned short rate)
oprospero	0:4dd021344a6b	1533	{
oprospero	0:4dd021344a6b	1534	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	1535	unsigned char div;
oprospero	0:4dd021344a6b	1536	if (!rate \|\| rate > st.chip_cfg.sample_rate \|\| rate > MAX_COMPASS_SAMPLE_RATE)
oprospero	0:4dd021344a6b	1537	return -1;
oprospero	0:4dd021344a6b	1538
oprospero	0:4dd021344a6b	1539	div = st.chip_cfg.sample_rate / rate - 1;
oprospero	0:4dd021344a6b	1540	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s4_ctrl, 1, &div))
oprospero	0:4dd021344a6b	1541	return -1;
oprospero	0:4dd021344a6b	1542	st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
oprospero	0:4dd021344a6b	1543	return 0;
oprospero	0:4dd021344a6b	1544	#else
oprospero	0:4dd021344a6b	1545	return -1;
oprospero	0:4dd021344a6b	1546	#endif
oprospero	0:4dd021344a6b	1547	}
oprospero	0:4dd021344a6b	1548
oprospero	0:4dd021344a6b	1549	/**
oprospero	0:4dd021344a6b	1550	* @brief Get gyro sensitivity scale factor.
oprospero	0:4dd021344a6b	1551	* @param[out] sens Conversion from hardware units to dps.
oprospero	0:4dd021344a6b	1552	* @return 0 if successful.
oprospero	0:4dd021344a6b	1553	*/
oprospero	0:4dd021344a6b	1554	int InvMpu::mpu_get_gyro_sens(float *sens)
oprospero	0:4dd021344a6b	1555	{
oprospero	0:4dd021344a6b	1556	switch (st.chip_cfg.gyro_fsr) {
oprospero	0:4dd021344a6b	1557	case INV_FSR_250DPS:
oprospero	0:4dd021344a6b	1558	sens[0] = 131.f;
oprospero	0:4dd021344a6b	1559	break;
oprospero	0:4dd021344a6b	1560	case INV_FSR_500DPS:
oprospero	0:4dd021344a6b	1561	sens[0] = 65.5f;
oprospero	0:4dd021344a6b	1562	break;
oprospero	0:4dd021344a6b	1563	case INV_FSR_1000DPS:
oprospero	0:4dd021344a6b	1564	sens[0] = 32.8f;
oprospero	0:4dd021344a6b	1565	break;
oprospero	0:4dd021344a6b	1566	case INV_FSR_2000DPS:
oprospero	0:4dd021344a6b	1567	sens[0] = 16.4f;
oprospero	0:4dd021344a6b	1568	break;
oprospero	0:4dd021344a6b	1569	default:
oprospero	0:4dd021344a6b	1570	return -1;
oprospero	0:4dd021344a6b	1571	}
oprospero	0:4dd021344a6b	1572	return 0;
oprospero	0:4dd021344a6b	1573	}
oprospero	0:4dd021344a6b	1574
oprospero	0:4dd021344a6b	1575	/**
oprospero	0:4dd021344a6b	1576	* @brief Get accel sensitivity scale factor.
oprospero	0:4dd021344a6b	1577	* @param[out] sens Conversion from hardware units to g's.
oprospero	0:4dd021344a6b	1578	* @return 0 if successful.
oprospero	0:4dd021344a6b	1579	*/
oprospero	0:4dd021344a6b	1580	int InvMpu::mpu_get_accel_sens(unsigned short *sens)
oprospero	0:4dd021344a6b	1581	{
oprospero	0:4dd021344a6b	1582	switch (st.chip_cfg.accel_fsr) {
oprospero	0:4dd021344a6b	1583	case INV_FSR_2G:
oprospero	0:4dd021344a6b	1584	sens[0] = 16384;
oprospero	0:4dd021344a6b	1585	break;
oprospero	0:4dd021344a6b	1586	case INV_FSR_4G:
oprospero	0:4dd021344a6b	1587	sens[0] = 8092;
oprospero	0:4dd021344a6b	1588	break;
oprospero	0:4dd021344a6b	1589	case INV_FSR_8G:
oprospero	0:4dd021344a6b	1590	sens[0] = 4096;
oprospero	0:4dd021344a6b	1591	break;
oprospero	0:4dd021344a6b	1592	case INV_FSR_16G:
oprospero	0:4dd021344a6b	1593	sens[0] = 2048;
oprospero	0:4dd021344a6b	1594	break;
oprospero	0:4dd021344a6b	1595	default:
oprospero	0:4dd021344a6b	1596	return -1;
oprospero	0:4dd021344a6b	1597	}
oprospero	0:4dd021344a6b	1598	if (st.chip_cfg.accel_half)
oprospero	0:4dd021344a6b	1599	sens[0] >>= 1;
oprospero	0:4dd021344a6b	1600	return 0;
oprospero	0:4dd021344a6b	1601	}
oprospero	0:4dd021344a6b	1602
oprospero	0:4dd021344a6b	1603	/**
oprospero	0:4dd021344a6b	1604	* @brief Get current FIFO configuration.
oprospero	0:4dd021344a6b	1605	* @e sensors can contain a combination of the following flags:
oprospero	0:4dd021344a6b	1606	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
oprospero	0:4dd021344a6b	1607	* \n INV_XYZ_GYRO
oprospero	0:4dd021344a6b	1608	* \n INV_XYZ_ACCEL
oprospero	0:4dd021344a6b	1609	* @param[out] sensors Mask of sensors in FIFO.
oprospero	0:4dd021344a6b	1610	* @return 0 if successful.
oprospero	0:4dd021344a6b	1611	*/
oprospero	0:4dd021344a6b	1612	int InvMpu::mpu_get_fifo_config(unsigned char *sensors)
oprospero	0:4dd021344a6b	1613	{
oprospero	0:4dd021344a6b	1614	sensors[0] = st.chip_cfg.fifo_enable;
oprospero	0:4dd021344a6b	1615	return 0;
oprospero	0:4dd021344a6b	1616	}
oprospero	0:4dd021344a6b	1617
oprospero	0:4dd021344a6b	1618	/**
oprospero	0:4dd021344a6b	1619	* @brief Select which sensors are pushed to FIFO.
oprospero	0:4dd021344a6b	1620	* @e sensors can contain a combination of the following flags:
oprospero	0:4dd021344a6b	1621	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
oprospero	0:4dd021344a6b	1622	* \n INV_XYZ_GYRO
oprospero	0:4dd021344a6b	1623	* \n INV_XYZ_ACCEL
oprospero	0:4dd021344a6b	1624	* @param[in] sensors Mask of sensors to push to FIFO.
oprospero	0:4dd021344a6b	1625	* @return 0 if successful.
oprospero	0:4dd021344a6b	1626	*/
oprospero	0:4dd021344a6b	1627	int InvMpu::mpu_configure_fifo(unsigned char sensors)
oprospero	0:4dd021344a6b	1628	{
oprospero	0:4dd021344a6b	1629	unsigned char prev;
oprospero	0:4dd021344a6b	1630	int result = 0;
oprospero	0:4dd021344a6b	1631
oprospero	0:4dd021344a6b	1632	/* Compass data isn't going into the FIFO. Stop trying. */
oprospero	0:4dd021344a6b	1633	sensors &= ~INV_XYZ_COMPASS;
oprospero	0:4dd021344a6b	1634
oprospero	0:4dd021344a6b	1635	if (st.chip_cfg.dmp_on)
oprospero	0:4dd021344a6b	1636	return 0;
oprospero	0:4dd021344a6b	1637	else {
oprospero	0:4dd021344a6b	1638	if (!(st.chip_cfg.sensors))
oprospero	0:4dd021344a6b	1639	return -1;
oprospero	0:4dd021344a6b	1640	prev = st.chip_cfg.fifo_enable;
oprospero	0:4dd021344a6b	1641	st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
oprospero	0:4dd021344a6b	1642	if (st.chip_cfg.fifo_enable != sensors)
oprospero	0:4dd021344a6b	1643	/* You're not getting what you asked for. Some sensors are
oprospero	0:4dd021344a6b	1644	* asleep.
oprospero	0:4dd021344a6b	1645	*/
oprospero	0:4dd021344a6b	1646	result = -1;
oprospero	0:4dd021344a6b	1647	else
oprospero	0:4dd021344a6b	1648	result = 0;
oprospero	0:4dd021344a6b	1649	if (sensors \|\| st.chip_cfg.lp_accel_mode)
oprospero	0:4dd021344a6b	1650	set_int_enable(1);
oprospero	0:4dd021344a6b	1651	else
oprospero	0:4dd021344a6b	1652	set_int_enable(0);
oprospero	0:4dd021344a6b	1653	if (sensors) {
oprospero	0:4dd021344a6b	1654	if (mpu_reset_fifo()) {
oprospero	0:4dd021344a6b	1655	st.chip_cfg.fifo_enable = prev;
oprospero	0:4dd021344a6b	1656	return -1;
oprospero	0:4dd021344a6b	1657	}
oprospero	0:4dd021344a6b	1658	}
oprospero	0:4dd021344a6b	1659	}
oprospero	0:4dd021344a6b	1660
oprospero	0:4dd021344a6b	1661	return result;
oprospero	0:4dd021344a6b	1662	}
oprospero	0:4dd021344a6b	1663
oprospero	0:4dd021344a6b	1664	/**
oprospero	0:4dd021344a6b	1665	* @brief Get current power state.
oprospero	0:4dd021344a6b	1666	* @param[in] power_on 1 if turned on, 0 if suspended.
oprospero	0:4dd021344a6b	1667	* @return 0 if successful.
oprospero	0:4dd021344a6b	1668	*/
oprospero	0:4dd021344a6b	1669	int InvMpu::mpu_get_power_state(unsigned char *power_on)
oprospero	0:4dd021344a6b	1670	{
oprospero	0:4dd021344a6b	1671	if (st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	1672	power_on[0] = 1;
oprospero	0:4dd021344a6b	1673	else
oprospero	0:4dd021344a6b	1674	power_on[0] = 0;
oprospero	0:4dd021344a6b	1675	return 0;
oprospero	0:4dd021344a6b	1676	}
oprospero	0:4dd021344a6b	1677
oprospero	0:4dd021344a6b	1678	/**
oprospero	0:4dd021344a6b	1679	* @brief Turn specific sensors on/off.
oprospero	0:4dd021344a6b	1680	* @e sensors can contain a combination of the following flags:
oprospero	0:4dd021344a6b	1681	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
oprospero	0:4dd021344a6b	1682	* \n INV_XYZ_GYRO
oprospero	0:4dd021344a6b	1683	* \n INV_XYZ_ACCEL
oprospero	0:4dd021344a6b	1684	* \n INV_XYZ_COMPASS
oprospero	0:4dd021344a6b	1685	* @param[in] sensors Mask of sensors to wake.
oprospero	0:4dd021344a6b	1686	* @return 0 if successful.
oprospero	0:4dd021344a6b	1687	*/
oprospero	0:4dd021344a6b	1688	int InvMpu::mpu_set_sensors(unsigned char sensors)
oprospero	0:4dd021344a6b	1689	{
oprospero	0:4dd021344a6b	1690	// PRINTLN("\t mpu_set_sensors");
oprospero	0:4dd021344a6b	1691	unsigned char data;
oprospero	0:4dd021344a6b	1692	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	1693	unsigned char user_ctrl;
oprospero	0:4dd021344a6b	1694	#endif
oprospero	0:4dd021344a6b	1695
oprospero	0:4dd021344a6b	1696	if (sensors & INV_XYZ_GYRO)
oprospero	0:4dd021344a6b	1697	data = INV_CLK_PLL;
oprospero	0:4dd021344a6b	1698	else if (sensors)
oprospero	0:4dd021344a6b	1699	data = 0;
oprospero	0:4dd021344a6b	1700	else
oprospero	0:4dd021344a6b	1701	data = BIT_SLEEP;
oprospero	0:4dd021344a6b	1702	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
oprospero	0:4dd021344a6b	1703	st.chip_cfg.sensors = 0;
oprospero	0:4dd021344a6b	1704	return -1;
oprospero	0:4dd021344a6b	1705	}
oprospero	0:4dd021344a6b	1706	st.chip_cfg.clk_src = data & ~BIT_SLEEP;
oprospero	0:4dd021344a6b	1707
oprospero	0:4dd021344a6b	1708	data = 0;
oprospero	0:4dd021344a6b	1709	if (!(sensors & INV_X_GYRO))
oprospero	0:4dd021344a6b	1710	data \|= BIT_STBY_XG;
oprospero	0:4dd021344a6b	1711	if (!(sensors & INV_Y_GYRO))
oprospero	0:4dd021344a6b	1712	data \|= BIT_STBY_YG;
oprospero	0:4dd021344a6b	1713	if (!(sensors & INV_Z_GYRO))
oprospero	0:4dd021344a6b	1714	data \|= BIT_STBY_ZG;
oprospero	0:4dd021344a6b	1715	if (!(sensors & INV_XYZ_ACCEL))
oprospero	0:4dd021344a6b	1716	data \|= BIT_STBY_XYZA;
oprospero	0:4dd021344a6b	1717	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
oprospero	0:4dd021344a6b	1718	st.chip_cfg.sensors = 0;
oprospero	0:4dd021344a6b	1719	return -1;
oprospero	0:4dd021344a6b	1720	}
oprospero	0:4dd021344a6b	1721
oprospero	0:4dd021344a6b	1722	// PRINTLN("\t Latched interrupts");
oprospero	0:4dd021344a6b	1723	if (sensors && (sensors != INV_XYZ_ACCEL))
oprospero	0:4dd021344a6b	1724	/* Latched interrupts only used in LP accel mode. */
oprospero	0:4dd021344a6b	1725	mpu_set_int_latched(0);
oprospero	0:4dd021344a6b	1726
oprospero	0:4dd021344a6b	1727	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	1728	#ifdef AK89xx_BYPASS
oprospero	0:4dd021344a6b	1729	if (sensors & INV_XYZ_COMPASS)
oprospero	0:4dd021344a6b	1730	mpu_set_bypass(1);
oprospero	0:4dd021344a6b	1731	else
oprospero	0:4dd021344a6b	1732	mpu_set_bypass(0);
oprospero	0:4dd021344a6b	1733	#else
oprospero	0:4dd021344a6b	1734	if (i2Cdev.readBytes(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
oprospero	0:4dd021344a6b	1735	return -1;
oprospero	0:4dd021344a6b	1736	// PRINTLN("\t Handle AKM power management.");
oprospero	0:4dd021344a6b	1737	/* Handle AKM power management. */
oprospero	0:4dd021344a6b	1738	if (sensors & INV_XYZ_COMPASS) {
oprospero	0:4dd021344a6b	1739	data = AKM_SINGLE_MEASUREMENT;
oprospero	0:4dd021344a6b	1740	user_ctrl \|= BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1741	} else {
oprospero	0:4dd021344a6b	1742	data = AKM_POWER_DOWN;
oprospero	0:4dd021344a6b	1743	user_ctrl &= ~BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1744	}
oprospero	0:4dd021344a6b	1745	if (st.chip_cfg.dmp_on)
oprospero	0:4dd021344a6b	1746	user_ctrl \|= BIT_DMP_EN;
oprospero	0:4dd021344a6b	1747	else
oprospero	0:4dd021344a6b	1748	user_ctrl &= ~BIT_DMP_EN;
oprospero	0:4dd021344a6b	1749	// PRINTLN("\t BIT_DMP_EN");
oprospero	0:4dd021344a6b	1750	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s1_do, 1, &data))
oprospero	0:4dd021344a6b	1751	return -1;
oprospero	0:4dd021344a6b	1752	/* Enable/disable I2C master mode. */
oprospero	0:4dd021344a6b	1753	// PRINTLN("\t nable/disable I2C master mode");
oprospero	0:4dd021344a6b	1754	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
oprospero	0:4dd021344a6b	1755	return -1;
oprospero	0:4dd021344a6b	1756	#endif
oprospero	0:4dd021344a6b	1757	#endif
oprospero	0:4dd021344a6b	1758
oprospero	0:4dd021344a6b	1759	st.chip_cfg.sensors = sensors;
oprospero	0:4dd021344a6b	1760	st.chip_cfg.lp_accel_mode = 0;
oprospero	1:dbbd3dcd61c9	1761	wait_ms(20);
oprospero	0:4dd021344a6b	1762	return 0;
oprospero	0:4dd021344a6b	1763	}
oprospero	0:4dd021344a6b	1764
oprospero	0:4dd021344a6b	1765	/**
oprospero	0:4dd021344a6b	1766	* @brief Read the MPU interrupt status registers.
oprospero	0:4dd021344a6b	1767	* @param[out] status Mask of interrupt bits.
oprospero	0:4dd021344a6b	1768	* @return 0 if successful.
oprospero	0:4dd021344a6b	1769	*/
oprospero	0:4dd021344a6b	1770	int InvMpu::mpu_get_int_status(short *status)
oprospero	0:4dd021344a6b	1771	{
oprospero	0:4dd021344a6b	1772	unsigned char tmp[2];
oprospero	0:4dd021344a6b	1773	if (!st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	1774	return -1;
oprospero	0:4dd021344a6b	1775	if (i2Cdev.readBytes(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
oprospero	0:4dd021344a6b	1776	return -1;
oprospero	0:4dd021344a6b	1777	status[0] = (tmp[0] << 8) \| tmp[1];
oprospero	0:4dd021344a6b	1778	return 0;
oprospero	0:4dd021344a6b	1779	}
oprospero	0:4dd021344a6b	1780
oprospero	0:4dd021344a6b	1781	/**
oprospero	0:4dd021344a6b	1782	* @brief Get one packet from the FIFO.
oprospero	0:4dd021344a6b	1783	* If @e sensors does not contain a particular sensor, disregard the data
oprospero	0:4dd021344a6b	1784	* returned to that pointer.
oprospero	0:4dd021344a6b	1785	* \n @e sensors can contain a combination of the following flags:
oprospero	0:4dd021344a6b	1786	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
oprospero	0:4dd021344a6b	1787	* \n INV_XYZ_GYRO
oprospero	0:4dd021344a6b	1788	* \n INV_XYZ_ACCEL
oprospero	0:4dd021344a6b	1789	* \n If the FIFO has no new data, @e sensors will be zero.
oprospero	0:4dd021344a6b	1790	* \n If the FIFO is disabled, @e sensors will be zero and this function will
oprospero	0:4dd021344a6b	1791	* return a non-zero error code.
oprospero	0:4dd021344a6b	1792	* @param[out] gyro Gyro data in hardware units.
oprospero	0:4dd021344a6b	1793	* @param[out] accel Accel data in hardware units.
oprospero	0:4dd021344a6b	1794	* @param[out] timestamp Timestamp in milliseconds.
oprospero	0:4dd021344a6b	1795	* @param[out] sensors Mask of sensors read from FIFO.
oprospero	0:4dd021344a6b	1796	* @param[out] more Number of remaining packets.
oprospero	0:4dd021344a6b	1797	* @return 0 if successful.
oprospero	0:4dd021344a6b	1798	*/
oprospero	0:4dd021344a6b	1799	int InvMpu::mpu_read_fifo(short gyro, short accel, unsigned long *timestamp,
oprospero	0:4dd021344a6b	1800	unsigned char sensors, unsigned char more)
oprospero	0:4dd021344a6b	1801	{
oprospero	0:4dd021344a6b	1802	/* Assumes maximum packet size is gyro (6) + accel (6). */
oprospero	0:4dd021344a6b	1803	unsigned char data[MAX_PACKET_LENGTH];
oprospero	0:4dd021344a6b	1804	unsigned char packet_size = 0;
oprospero	0:4dd021344a6b	1805	unsigned short fifo_count, index = 0;
oprospero	0:4dd021344a6b	1806
oprospero	0:4dd021344a6b	1807	if (st.chip_cfg.dmp_on)
oprospero	0:4dd021344a6b	1808	return -1;
oprospero	0:4dd021344a6b	1809
oprospero	0:4dd021344a6b	1810	sensors[0] = 0;
oprospero	0:4dd021344a6b	1811	if (!st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	1812	return -1;
oprospero	0:4dd021344a6b	1813	if (!st.chip_cfg.fifo_enable)
oprospero	0:4dd021344a6b	1814	return -1;
oprospero	0:4dd021344a6b	1815
oprospero	0:4dd021344a6b	1816	if (st.chip_cfg.fifo_enable & INV_X_GYRO)
oprospero	0:4dd021344a6b	1817	packet_size += 2;
oprospero	0:4dd021344a6b	1818	if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
oprospero	0:4dd021344a6b	1819	packet_size += 2;
oprospero	0:4dd021344a6b	1820	if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
oprospero	0:4dd021344a6b	1821	packet_size += 2;
oprospero	0:4dd021344a6b	1822	if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
oprospero	0:4dd021344a6b	1823	packet_size += 6;
oprospero	0:4dd021344a6b	1824
oprospero	0:4dd021344a6b	1825	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_count_h, 2, data))
oprospero	0:4dd021344a6b	1826	return -1;
oprospero	0:4dd021344a6b	1827	fifo_count = (data[0] << 8) \| data[1];
oprospero	0:4dd021344a6b	1828	if (fifo_count < packet_size)
oprospero	0:4dd021344a6b	1829	return 0;
oprospero	0:4dd021344a6b	1830	// log_i("FIFO count: %hd\n", fifo_count);
oprospero	0:4dd021344a6b	1831	if (fifo_count > (st.hw->max_fifo >> 1)) {
oprospero	0:4dd021344a6b	1832	/* FIFO is 50% full, better check overflow bit. */
oprospero	0:4dd021344a6b	1833	if (i2Cdev.readBytes(st.hw->addr, st.reg->int_status, 1, data))
oprospero	0:4dd021344a6b	1834	return -1;
oprospero	0:4dd021344a6b	1835	if (data[0] & BIT_FIFO_OVERFLOW) {
oprospero	0:4dd021344a6b	1836	mpu_reset_fifo();
oprospero	0:4dd021344a6b	1837	return -2;
oprospero	0:4dd021344a6b	1838	}
oprospero	0:4dd021344a6b	1839	}
oprospero	0:4dd021344a6b	1840	get_ms((unsigned long*)timestamp);
oprospero	0:4dd021344a6b	1841
oprospero	0:4dd021344a6b	1842	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
oprospero	0:4dd021344a6b	1843	return -1;
oprospero	0:4dd021344a6b	1844	more[0] = fifo_count / packet_size - 1;
oprospero	0:4dd021344a6b	1845	sensors[0] = 0;
oprospero	0:4dd021344a6b	1846
oprospero	0:4dd021344a6b	1847	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
oprospero	0:4dd021344a6b	1848	accel[0] = (data[index+0] << 8) \| data[index+1];
oprospero	0:4dd021344a6b	1849	accel[1] = (data[index+2] << 8) \| data[index+3];
oprospero	0:4dd021344a6b	1850	accel[2] = (data[index+4] << 8) \| data[index+5];
oprospero	0:4dd021344a6b	1851	sensors[0] \|= INV_XYZ_ACCEL;
oprospero	0:4dd021344a6b	1852	index += 6;
oprospero	0:4dd021344a6b	1853	}
oprospero	0:4dd021344a6b	1854	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
oprospero	0:4dd021344a6b	1855	gyro[0] = (data[index+0] << 8) \| data[index+1];
oprospero	0:4dd021344a6b	1856	sensors[0] \|= INV_X_GYRO;
oprospero	0:4dd021344a6b	1857	index += 2;
oprospero	0:4dd021344a6b	1858	}
oprospero	0:4dd021344a6b	1859	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
oprospero	0:4dd021344a6b	1860	gyro[1] = (data[index+0] << 8) \| data[index+1];
oprospero	0:4dd021344a6b	1861	sensors[0] \|= INV_Y_GYRO;
oprospero	0:4dd021344a6b	1862	index += 2;
oprospero	0:4dd021344a6b	1863	}
oprospero	0:4dd021344a6b	1864	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
oprospero	0:4dd021344a6b	1865	gyro[2] = (data[index+0] << 8) \| data[index+1];
oprospero	0:4dd021344a6b	1866	sensors[0] \|= INV_Z_GYRO;
oprospero	0:4dd021344a6b	1867	index += 2;
oprospero	0:4dd021344a6b	1868	}
oprospero	0:4dd021344a6b	1869
oprospero	0:4dd021344a6b	1870	return 0;
oprospero	0:4dd021344a6b	1871	}
oprospero	0:4dd021344a6b	1872
oprospero	0:4dd021344a6b	1873	/**
oprospero	0:4dd021344a6b	1874	* @brief Get one unparsed packet from the FIFO.
oprospero	0:4dd021344a6b	1875	* This function should be used if the packet is to be parsed elsewhere.
oprospero	0:4dd021344a6b	1876	* @param[in] length Length of one FIFO packet.
oprospero	0:4dd021344a6b	1877	* @param[in] data FIFO packet.
oprospero	0:4dd021344a6b	1878	* @param[in] more Number of remaining packets.
oprospero	0:4dd021344a6b	1879	*/
oprospero	0:4dd021344a6b	1880	int InvMpu::mpu_read_fifo_stream(unsigned short length, unsigned char *data,
oprospero	0:4dd021344a6b	1881	unsigned char *more)
oprospero	0:4dd021344a6b	1882	{
oprospero	0:4dd021344a6b	1883	unsigned char tmp[2];
oprospero	0:4dd021344a6b	1884	unsigned short fifo_count;
oprospero	0:4dd021344a6b	1885	if (!st.chip_cfg.dmp_on)
oprospero	0:4dd021344a6b	1886	return -1;
oprospero	0:4dd021344a6b	1887	if (!st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	1888	return -1;
oprospero	0:4dd021344a6b	1889
oprospero	0:4dd021344a6b	1890	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
oprospero	0:4dd021344a6b	1891	return -1;
oprospero	0:4dd021344a6b	1892	// PRINTF("\tFIFO Count done: %d \n\r",fifo_count);
oprospero	0:4dd021344a6b	1893	fifo_count = ((unsigned short) tmp[0] << 8) \| tmp[1];
oprospero	0:4dd021344a6b	1894	if (fifo_count < length) {
oprospero	0:4dd021344a6b	1895	more[0] = 0;
oprospero	0:4dd021344a6b	1896	return -2;
oprospero	0:4dd021344a6b	1897	}
oprospero	0:4dd021344a6b	1898	// PRINTLN("\tFifo Count checked");
oprospero	0:4dd021344a6b	1899	if (fifo_count > (st.hw->max_fifo >> 1)) {
oprospero	0:4dd021344a6b	1900	/* FIFO is 50% full, better check overflow bit. */
oprospero	0:4dd021344a6b	1901	if (i2Cdev.readBytes(st.hw->addr, st.reg->int_status, 1, tmp))
oprospero	0:4dd021344a6b	1902	return -1;
oprospero	0:4dd021344a6b	1903	if (tmp[0] & BIT_FIFO_OVERFLOW) {
oprospero	0:4dd021344a6b	1904	mpu_reset_fifo();
oprospero	0:4dd021344a6b	1905	return -3;
oprospero	0:4dd021344a6b	1906	}
oprospero	0:4dd021344a6b	1907	}
oprospero	0:4dd021344a6b	1908	// PRINTLN("\tCheck overflow done");
oprospero	0:4dd021344a6b	1909
oprospero	0:4dd021344a6b	1910	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_r_w, length, data))
oprospero	0:4dd021344a6b	1911	return -1;
oprospero	0:4dd021344a6b	1912	more[0] = fifo_count / length - 1;
oprospero	0:4dd021344a6b	1913	// PRINTLN("\tFifo read Done");
oprospero	0:4dd021344a6b	1914	return 0;
oprospero	0:4dd021344a6b	1915	}
oprospero	0:4dd021344a6b	1916
oprospero	0:4dd021344a6b	1917	/**
oprospero	0:4dd021344a6b	1918	* @brief Set device to bypass mode.
oprospero	0:4dd021344a6b	1919	* @param[in] bypass_on 1 to enable bypass mode.
oprospero	0:4dd021344a6b	1920	* @return 0 if successful.
oprospero	0:4dd021344a6b	1921	*/
oprospero	0:4dd021344a6b	1922	int InvMpu::mpu_set_bypass(unsigned char bypass_on)
oprospero	0:4dd021344a6b	1923	{
oprospero	0:4dd021344a6b	1924	unsigned char tmp;
oprospero	0:4dd021344a6b	1925
oprospero	0:4dd021344a6b	1926	if (st.chip_cfg.bypass_mode == bypass_on)
oprospero	0:4dd021344a6b	1927	return 0;
oprospero	0:4dd021344a6b	1928
oprospero	0:4dd021344a6b	1929	if (bypass_on) {
oprospero	0:4dd021344a6b	1930	if (i2Cdev.readBytes(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
oprospero	0:4dd021344a6b	1931	return -1;
oprospero	0:4dd021344a6b	1932	tmp &= ~BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1933	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
oprospero	0:4dd021344a6b	1934	return -1;
oprospero	0:4dd021344a6b	1935	wait_ms(3);
oprospero	0:4dd021344a6b	1936	tmp = BIT_BYPASS_EN;
oprospero	0:4dd021344a6b	1937	if (st.chip_cfg.active_low_int)
oprospero	0:4dd021344a6b	1938	tmp \|= BIT_ACTL;
oprospero	0:4dd021344a6b	1939	if (st.chip_cfg.latched_int)
oprospero	0:4dd021344a6b	1940	tmp \|= BIT_LATCH_EN \| BIT_ANY_RD_CLR;
oprospero	0:4dd021344a6b	1941	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
oprospero	0:4dd021344a6b	1942	return -1;
oprospero	0:4dd021344a6b	1943	} else {
oprospero	0:4dd021344a6b	1944	/* Enable I2C master mode if compass is being used. */
oprospero	0:4dd021344a6b	1945	if (i2Cdev.readBytes(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
oprospero	0:4dd021344a6b	1946	return -1;
oprospero	0:4dd021344a6b	1947	if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
oprospero	0:4dd021344a6b	1948	tmp \|= BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1949	else
oprospero	0:4dd021344a6b	1950	tmp &= ~BIT_AUX_IF_EN;
oprospero	0:4dd021344a6b	1951	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
oprospero	0:4dd021344a6b	1952	return -1;
oprospero	0:4dd021344a6b	1953	wait_ms(3);
oprospero	0:4dd021344a6b	1954	if (st.chip_cfg.active_low_int)
oprospero	0:4dd021344a6b	1955	tmp = BIT_ACTL;
oprospero	0:4dd021344a6b	1956	else
oprospero	0:4dd021344a6b	1957	tmp = 0;
oprospero	0:4dd021344a6b	1958	if (st.chip_cfg.latched_int)
oprospero	0:4dd021344a6b	1959	tmp \|= BIT_LATCH_EN \| BIT_ANY_RD_CLR;
oprospero	0:4dd021344a6b	1960	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
oprospero	0:4dd021344a6b	1961	return -1;
oprospero	0:4dd021344a6b	1962	}
oprospero	0:4dd021344a6b	1963	st.chip_cfg.bypass_mode = bypass_on;
oprospero	0:4dd021344a6b	1964	return 0;
oprospero	0:4dd021344a6b	1965	}
oprospero	0:4dd021344a6b	1966
oprospero	0:4dd021344a6b	1967	/**
oprospero	0:4dd021344a6b	1968	* @brief Set interrupt level.
oprospero	0:4dd021344a6b	1969	* @param[in] active_low 1 for active low, 0 for active high.
oprospero	0:4dd021344a6b	1970	* @return 0 if successful.
oprospero	0:4dd021344a6b	1971	*/
oprospero	0:4dd021344a6b	1972	int InvMpu::mpu_set_int_level(unsigned char active_low)
oprospero	0:4dd021344a6b	1973	{
oprospero	0:4dd021344a6b	1974	st.chip_cfg.active_low_int = active_low;
oprospero	0:4dd021344a6b	1975	return 0;
oprospero	0:4dd021344a6b	1976	}
oprospero	0:4dd021344a6b	1977
oprospero	0:4dd021344a6b	1978	/**
oprospero	0:4dd021344a6b	1979	* @brief Enable latched interrupts.
oprospero	0:4dd021344a6b	1980	* Any MPU register will clear the interrupt.
oprospero	0:4dd021344a6b	1981	* @param[in] enable 1 to enable, 0 to disable.
oprospero	0:4dd021344a6b	1982	* @return 0 if successful.
oprospero	0:4dd021344a6b	1983	*/
oprospero	0:4dd021344a6b	1984	int InvMpu::mpu_set_int_latched(unsigned char enable)
oprospero	0:4dd021344a6b	1985	{
oprospero	0:4dd021344a6b	1986	unsigned char tmp;
oprospero	0:4dd021344a6b	1987	if (st.chip_cfg.latched_int == enable)
oprospero	0:4dd021344a6b	1988	return 0;
oprospero	0:4dd021344a6b	1989
oprospero	0:4dd021344a6b	1990	if (enable)
oprospero	0:4dd021344a6b	1991	tmp = BIT_LATCH_EN \| BIT_ANY_RD_CLR;
oprospero	0:4dd021344a6b	1992	else
oprospero	0:4dd021344a6b	1993	tmp = 0;
oprospero	0:4dd021344a6b	1994	if (st.chip_cfg.bypass_mode)
oprospero	0:4dd021344a6b	1995	tmp \|= BIT_BYPASS_EN;
oprospero	0:4dd021344a6b	1996	if (st.chip_cfg.active_low_int)
oprospero	0:4dd021344a6b	1997	tmp \|= BIT_ACTL;
oprospero	0:4dd021344a6b	1998	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
oprospero	0:4dd021344a6b	1999	return -1;
oprospero	0:4dd021344a6b	2000	st.chip_cfg.latched_int = enable;
oprospero	0:4dd021344a6b	2001	return 0;
oprospero	0:4dd021344a6b	2002	}
oprospero	0:4dd021344a6b	2003
oprospero	0:4dd021344a6b	2004	#ifdef MPU6050
oprospero	0:4dd021344a6b	2005	//static int get_accel_prod_shift(float *st_shift)
oprospero	0:4dd021344a6b	2006	int InvMpu::get_accel_prod_shift(float *st_shift)
oprospero	0:4dd021344a6b	2007	{
oprospero	0:4dd021344a6b	2008	unsigned char tmp[4], shift_code[3], ii;
oprospero	0:4dd021344a6b	2009
oprospero	0:4dd021344a6b	2010	if (i2Cdev.readBytes(st.hw->addr, 0x0D, 4, tmp))
oprospero	0:4dd021344a6b	2011	return 0x07;
oprospero	0:4dd021344a6b	2012
oprospero	0:4dd021344a6b	2013	shift_code[0] = ((tmp[0] & 0xE0) >> 3) \| ((tmp[3] & 0x30) >> 4);
oprospero	0:4dd021344a6b	2014	shift_code[1] = ((tmp[1] & 0xE0) >> 3) \| ((tmp[3] & 0x0C) >> 2);
oprospero	0:4dd021344a6b	2015	shift_code[2] = ((tmp[2] & 0xE0) >> 3) \| (tmp[3] & 0x03);
oprospero	0:4dd021344a6b	2016	for (ii = 0; ii < 3; ii++) {
oprospero	0:4dd021344a6b	2017	if (!shift_code[ii]) {
oprospero	0:4dd021344a6b	2018	st_shift[ii] = 0.f;
oprospero	0:4dd021344a6b	2019	continue;
oprospero	0:4dd021344a6b	2020	}
oprospero	0:4dd021344a6b	2021	/* Equivalent to..
oprospero	0:4dd021344a6b	2022	* st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
oprospero	0:4dd021344a6b	2023	*/
oprospero	0:4dd021344a6b	2024	st_shift[ii] = 0.34f;
oprospero	0:4dd021344a6b	2025	while (--shift_code[ii])
oprospero	0:4dd021344a6b	2026	st_shift[ii] *= 1.034f;
oprospero	0:4dd021344a6b	2027	}
oprospero	0:4dd021344a6b	2028	return 0;
oprospero	0:4dd021344a6b	2029	}
oprospero	0:4dd021344a6b	2030
oprospero	0:4dd021344a6b	2031	//static int accel_self_test(long bias_regular, long bias_st)
oprospero	0:4dd021344a6b	2032	int InvMpu::accel_self_test(long bias_regular, long bias_st)
oprospero	0:4dd021344a6b	2033	{
oprospero	0:4dd021344a6b	2034	int jj, result = 0;
oprospero	0:4dd021344a6b	2035	float st_shift[3], st_shift_cust, st_shift_var;
oprospero	0:4dd021344a6b	2036
oprospero	0:4dd021344a6b	2037	get_accel_prod_shift(st_shift);
oprospero	0:4dd021344a6b	2038	for(jj = 0; jj < 3; jj++) {
oprospero	0:4dd021344a6b	2039	st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
oprospero	0:4dd021344a6b	2040	if (st_shift[jj]) {
oprospero	0:4dd021344a6b	2041	st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
oprospero	0:4dd021344a6b	2042	if (fabs(st_shift_var) > test.max_accel_var)
oprospero	0:4dd021344a6b	2043	result \|= 1 << jj;
oprospero	0:4dd021344a6b	2044	} else if ((st_shift_cust < test.min_g) \|\|
oprospero	0:4dd021344a6b	2045	(st_shift_cust > test.max_g))
oprospero	0:4dd021344a6b	2046	result \|= 1 << jj;
oprospero	0:4dd021344a6b	2047	}
oprospero	0:4dd021344a6b	2048
oprospero	0:4dd021344a6b	2049	return result;
oprospero	0:4dd021344a6b	2050	}
oprospero	0:4dd021344a6b	2051
oprospero	0:4dd021344a6b	2052	//static int gyro_self_test(long bias_regular, long bias_st)
oprospero	0:4dd021344a6b	2053	int InvMpu::gyro_self_test(long bias_regular, long bias_st)
oprospero	0:4dd021344a6b	2054	{
oprospero	0:4dd021344a6b	2055	int jj, result = 0;
oprospero	0:4dd021344a6b	2056	unsigned char tmp[3];
oprospero	0:4dd021344a6b	2057	float st_shift, st_shift_cust, st_shift_var;
oprospero	0:4dd021344a6b	2058
oprospero	0:4dd021344a6b	2059	if (i2Cdev.readBytes(st.hw->addr, 0x0D, 3, tmp))
oprospero	0:4dd021344a6b	2060	return 0x07;
oprospero	0:4dd021344a6b	2061
oprospero	0:4dd021344a6b	2062	tmp[0] &= 0x1F;
oprospero	0:4dd021344a6b	2063	tmp[1] &= 0x1F;
oprospero	0:4dd021344a6b	2064	tmp[2] &= 0x1F;
oprospero	0:4dd021344a6b	2065
oprospero	0:4dd021344a6b	2066	for (jj = 0; jj < 3; jj++) {
oprospero	0:4dd021344a6b	2067	st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
oprospero	0:4dd021344a6b	2068	if (tmp[jj]) {
oprospero	0:4dd021344a6b	2069	st_shift = 3275.f / test.gyro_sens;
oprospero	0:4dd021344a6b	2070	while (--tmp[jj])
oprospero	0:4dd021344a6b	2071	st_shift *= 1.046f;
oprospero	0:4dd021344a6b	2072	st_shift_var = st_shift_cust / st_shift - 1.f;
oprospero	0:4dd021344a6b	2073	if (fabs(st_shift_var) > test.max_gyro_var)
oprospero	0:4dd021344a6b	2074	result \|= 1 << jj;
oprospero	0:4dd021344a6b	2075	} else if ((st_shift_cust < test.min_dps) \|\|
oprospero	0:4dd021344a6b	2076	(st_shift_cust > test.max_dps))
oprospero	0:4dd021344a6b	2077	result \|= 1 << jj;
oprospero	0:4dd021344a6b	2078	}
oprospero	0:4dd021344a6b	2079	return result;
oprospero	0:4dd021344a6b	2080	}
oprospero	0:4dd021344a6b	2081
oprospero	0:4dd021344a6b	2082	#endif
oprospero	0:4dd021344a6b	2083	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	2084	//static int compass_self_test(void)
oprospero	0:4dd021344a6b	2085	int InvMpu::compass_self_test(void)
oprospero	0:4dd021344a6b	2086	{
oprospero	0:4dd021344a6b	2087	unsigned char tmp[6];
oprospero	0:4dd021344a6b	2088	unsigned char tries = 10;
oprospero	0:4dd021344a6b	2089	int result = 0x07;
oprospero	0:4dd021344a6b	2090	short data;
oprospero	0:4dd021344a6b	2091
oprospero	0:4dd021344a6b	2092	mpu_set_bypass(1);
oprospero	0:4dd021344a6b	2093
oprospero	0:4dd021344a6b	2094	tmp[0] = AKM_POWER_DOWN;
oprospero	0:4dd021344a6b	2095	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
oprospero	0:4dd021344a6b	2096	return 0x07;
oprospero	0:4dd021344a6b	2097	tmp[0] = AKM_BIT_SELF_TEST;
oprospero	0:4dd021344a6b	2098	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
oprospero	0:4dd021344a6b	2099	goto AKM_restore;
oprospero	0:4dd021344a6b	2100	tmp[0] = AKM_MODE_SELF_TEST;
oprospero	0:4dd021344a6b	2101	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
oprospero	0:4dd021344a6b	2102	goto AKM_restore;
oprospero	0:4dd021344a6b	2103
oprospero	0:4dd021344a6b	2104	do {
oprospero	0:4dd021344a6b	2105	wait_ms(10);
oprospero	0:4dd021344a6b	2106	if (i2Cdev.readBytes(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
oprospero	0:4dd021344a6b	2107	goto AKM_restore;
oprospero	0:4dd021344a6b	2108	if (tmp[0] & AKM_DATA_READY)
oprospero	0:4dd021344a6b	2109	break;
oprospero	0:4dd021344a6b	2110	} while (tries--);
oprospero	0:4dd021344a6b	2111	if (!(tmp[0] & AKM_DATA_READY))
oprospero	0:4dd021344a6b	2112	goto AKM_restore;
oprospero	0:4dd021344a6b	2113
oprospero	0:4dd021344a6b	2114	if (i2Cdev.readBytes(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
oprospero	0:4dd021344a6b	2115	goto AKM_restore;
oprospero	0:4dd021344a6b	2116
oprospero	0:4dd021344a6b	2117	result = 0;
oprospero	0:4dd021344a6b	2118	#if defined MPU9150
oprospero	0:4dd021344a6b	2119	data = (short)(tmp[1] << 8) \| tmp[0];
oprospero	0:4dd021344a6b	2120	if ((data > 100) \|\| (data < -100))
oprospero	0:4dd021344a6b	2121	result \|= 0x01;
oprospero	0:4dd021344a6b	2122	data = (short)(tmp[3] << 8) \| tmp[2];
oprospero	0:4dd021344a6b	2123	if ((data > 100) \|\| (data < -100))
oprospero	0:4dd021344a6b	2124	result \|= 0x02;
oprospero	0:4dd021344a6b	2125	data = (short)(tmp[5] << 8) \| tmp[4];
oprospero	0:4dd021344a6b	2126	if ((data > -300) \|\| (data < -1000))
oprospero	0:4dd021344a6b	2127	result \|= 0x04;
oprospero	0:4dd021344a6b	2128	#elif defined MPU9250
oprospero	0:4dd021344a6b	2129	data = (short)(tmp[1] << 8) \| tmp[0];
oprospero	0:4dd021344a6b	2130	if ((data > 200) \|\| (data < -200))
oprospero	0:4dd021344a6b	2131	result \|= 0x01;
oprospero	0:4dd021344a6b	2132	data = (short)(tmp[3] << 8) \| tmp[2];
oprospero	0:4dd021344a6b	2133	if ((data > 200) \|\| (data < -200))
oprospero	0:4dd021344a6b	2134	result \|= 0x02;
oprospero	0:4dd021344a6b	2135	data = (short)(tmp[5] << 8) \| tmp[4];
oprospero	0:4dd021344a6b	2136	if ((data > -800) \|\| (data < -3200))
oprospero	0:4dd021344a6b	2137	result \|= 0x04;
oprospero	0:4dd021344a6b	2138	#endif
oprospero	0:4dd021344a6b	2139	AKM_restore:
oprospero	0:4dd021344a6b	2140	tmp[0] = 0 \| SUPPORTS_AK89xx_HIGH_SENS;
oprospero	0:4dd021344a6b	2141	i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
oprospero	0:4dd021344a6b	2142	tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
oprospero	0:4dd021344a6b	2143	i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
oprospero	0:4dd021344a6b	2144	mpu_set_bypass(0);
oprospero	0:4dd021344a6b	2145	return result;
oprospero	0:4dd021344a6b	2146	}
oprospero	0:4dd021344a6b	2147	#endif
oprospero	0:4dd021344a6b	2148
oprospero	0:4dd021344a6b	2149	//static int get_st_biases(long gyro, long accel, unsigned char hw_test)
oprospero	0:4dd021344a6b	2150	int InvMpu::get_st_biases(long gyro, long accel, unsigned char hw_test)
oprospero	0:4dd021344a6b	2151	{
oprospero	0:4dd021344a6b	2152	unsigned char data[MAX_PACKET_LENGTH];
oprospero	0:4dd021344a6b	2153	unsigned char packet_count, ii;
oprospero	0:4dd021344a6b	2154	unsigned short fifo_count;
oprospero	0:4dd021344a6b	2155
oprospero	0:4dd021344a6b	2156	data[0] = 0x01;
oprospero	0:4dd021344a6b	2157	data[1] = 0;
oprospero	0:4dd021344a6b	2158	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
oprospero	0:4dd021344a6b	2159	return -1;
oprospero	0:4dd021344a6b	2160	wait_ms(200);
oprospero	0:4dd021344a6b	2161	data[0] = 0;
oprospero	0:4dd021344a6b	2162	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, data))
oprospero	0:4dd021344a6b	2163	return -1;
oprospero	0:4dd021344a6b	2164	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, data))
oprospero	0:4dd021344a6b	2165	return -1;
oprospero	0:4dd021344a6b	2166	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
oprospero	0:4dd021344a6b	2167	return -1;
oprospero	0:4dd021344a6b	2168	if (i2Cdev.writeBytes(st.hw->addr, st.reg->i2c_mst, 1, data))
oprospero	0:4dd021344a6b	2169	return -1;
oprospero	0:4dd021344a6b	2170	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, data))
oprospero	0:4dd021344a6b	2171	return -1;
oprospero	0:4dd021344a6b	2172	data[0] = BIT_FIFO_RST \| BIT_DMP_RST;
oprospero	0:4dd021344a6b	2173	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, data))
oprospero	0:4dd021344a6b	2174	return -1;
oprospero	0:4dd021344a6b	2175	wait_ms(15);
oprospero	0:4dd021344a6b	2176	data[0] = st.test->reg_lpf;
oprospero	0:4dd021344a6b	2177	if (i2Cdev.writeBytes(st.hw->addr, st.reg->lpf, 1, data))
oprospero	0:4dd021344a6b	2178	return -1;
oprospero	0:4dd021344a6b	2179	data[0] = st.test->reg_rate_div;
oprospero	0:4dd021344a6b	2180	if (i2Cdev.writeBytes(st.hw->addr, st.reg->rate_div, 1, data))
oprospero	0:4dd021344a6b	2181	return -1;
oprospero	0:4dd021344a6b	2182	if (hw_test)
oprospero	0:4dd021344a6b	2183	data[0] = st.test->reg_gyro_fsr \| 0xE0;
oprospero	0:4dd021344a6b	2184	else
oprospero	0:4dd021344a6b	2185	data[0] = st.test->reg_gyro_fsr;
oprospero	0:4dd021344a6b	2186	if (i2Cdev.writeBytes(st.hw->addr, st.reg->gyro_cfg, 1, data))
oprospero	0:4dd021344a6b	2187	return -1;
oprospero	0:4dd021344a6b	2188
oprospero	0:4dd021344a6b	2189	if (hw_test)
oprospero	0:4dd021344a6b	2190	data[0] = st.test->reg_accel_fsr \| 0xE0;
oprospero	0:4dd021344a6b	2191	else
oprospero	0:4dd021344a6b	2192	data[0] = test.reg_accel_fsr;
oprospero	0:4dd021344a6b	2193	if (i2Cdev.writeBytes(st.hw->addr, st.reg->accel_cfg, 1, data))
oprospero	0:4dd021344a6b	2194	return -1;
oprospero	0:4dd021344a6b	2195	if (hw_test)
oprospero	0:4dd021344a6b	2196	wait_ms(200);
oprospero	0:4dd021344a6b	2197
oprospero	0:4dd021344a6b	2198	/* Fill FIFO for test.wait_ms milliseconds. */
oprospero	0:4dd021344a6b	2199	data[0] = BIT_FIFO_EN;
oprospero	0:4dd021344a6b	2200	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, data))
oprospero	0:4dd021344a6b	2201	return -1;
oprospero	0:4dd021344a6b	2202
oprospero	0:4dd021344a6b	2203	data[0] = INV_XYZ_GYRO \| INV_XYZ_ACCEL;
oprospero	0:4dd021344a6b	2204	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, data))
oprospero	0:4dd021344a6b	2205	return -1;
oprospero	0:4dd021344a6b	2206	wait_ms(test.wait_ms);
oprospero	0:4dd021344a6b	2207	data[0] = 0;
oprospero	0:4dd021344a6b	2208	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, data))
oprospero	0:4dd021344a6b	2209	return -1;
oprospero	0:4dd021344a6b	2210
oprospero	0:4dd021344a6b	2211	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_count_h, 2, data))
oprospero	0:4dd021344a6b	2212	return -1;
oprospero	0:4dd021344a6b	2213
oprospero	0:4dd021344a6b	2214	fifo_count = (data[0] << 8) \| data[1];
oprospero	0:4dd021344a6b	2215	packet_count = fifo_count / MAX_PACKET_LENGTH;
oprospero	0:4dd021344a6b	2216	gyro[0] = gyro[1] = gyro[2] = 0;
oprospero	0:4dd021344a6b	2217	accel[0] = accel[1] = accel[2] = 0;
oprospero	0:4dd021344a6b	2218
oprospero	0:4dd021344a6b	2219	for (ii = 0; ii < packet_count; ii++) {
oprospero	0:4dd021344a6b	2220	short accel_cur[3], gyro_cur[3];
oprospero	0:4dd021344a6b	2221	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
oprospero	0:4dd021344a6b	2222	return -1;
oprospero	0:4dd021344a6b	2223	accel_cur[0] = ((short)data[0] << 8) \| data[1];
oprospero	0:4dd021344a6b	2224	accel_cur[1] = ((short)data[2] << 8) \| data[3];
oprospero	0:4dd021344a6b	2225	accel_cur[2] = ((short)data[4] << 8) \| data[5];
oprospero	0:4dd021344a6b	2226	accel[0] += (long)accel_cur[0];
oprospero	0:4dd021344a6b	2227	accel[1] += (long)accel_cur[1];
oprospero	0:4dd021344a6b	2228	accel[2] += (long)accel_cur[2];
oprospero	0:4dd021344a6b	2229	gyro_cur[0] = (((short)data[6] << 8) \| data[7]);
oprospero	0:4dd021344a6b	2230	gyro_cur[1] = (((short)data[8] << 8) \| data[9]);
oprospero	0:4dd021344a6b	2231	gyro_cur[2] = (((short)data[10] << 8) \| data[11]);
oprospero	0:4dd021344a6b	2232	gyro[0] += (long)gyro_cur[0];
oprospero	0:4dd021344a6b	2233	gyro[1] += (long)gyro_cur[1];
oprospero	0:4dd021344a6b	2234	gyro[2] += (long)gyro_cur[2];
oprospero	0:4dd021344a6b	2235	}
oprospero	0:4dd021344a6b	2236	#ifdef EMPL_NO_64BIT
oprospero	0:4dd021344a6b	2237	gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
oprospero	0:4dd021344a6b	2238	gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
oprospero	0:4dd021344a6b	2239	gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
oprospero	0:4dd021344a6b	2240	if (has_accel) {
oprospero	0:4dd021344a6b	2241	accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
oprospero	0:4dd021344a6b	2242	packet_count);
oprospero	0:4dd021344a6b	2243	accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
oprospero	0:4dd021344a6b	2244	packet_count);
oprospero	0:4dd021344a6b	2245	accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
oprospero	0:4dd021344a6b	2246	packet_count);
oprospero	0:4dd021344a6b	2247	/* Don't remove gravity! */
oprospero	0:4dd021344a6b	2248	accel[2] -= 65536L;
oprospero	0:4dd021344a6b	2249	}
oprospero	0:4dd021344a6b	2250	#else
oprospero	0:4dd021344a6b	2251	gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
oprospero	0:4dd021344a6b	2252	gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
oprospero	0:4dd021344a6b	2253	gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
oprospero	0:4dd021344a6b	2254	accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
oprospero	0:4dd021344a6b	2255	packet_count);
oprospero	0:4dd021344a6b	2256	accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
oprospero	0:4dd021344a6b	2257	packet_count);
oprospero	0:4dd021344a6b	2258	accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
oprospero	0:4dd021344a6b	2259	packet_count);
oprospero	0:4dd021344a6b	2260	/* Don't remove gravity! */
oprospero	0:4dd021344a6b	2261	if (accel[2] > 0L)
oprospero	0:4dd021344a6b	2262	accel[2] -= 65536L;
oprospero	0:4dd021344a6b	2263	else
oprospero	0:4dd021344a6b	2264	accel[2] += 65536L;
oprospero	0:4dd021344a6b	2265	#endif
oprospero	0:4dd021344a6b	2266
oprospero	0:4dd021344a6b	2267	return 0;
oprospero	0:4dd021344a6b	2268	}
oprospero	0:4dd021344a6b	2269
oprospero	0:4dd021344a6b	2270	#ifdef MPU6500
oprospero	0:4dd021344a6b	2271	#define REG_6500_XG_ST_DATA 0x0
oprospero	0:4dd021344a6b	2272	#define REG_6500_XA_ST_DATA 0xD
oprospero	0:4dd021344a6b	2273	static const unsigned short mpu_6500_st_tb[256] = {
oprospero	0:4dd021344a6b	2274	2620,2646,2672,2699,2726,2753,2781,2808, //7
oprospero	0:4dd021344a6b	2275	2837,2865,2894,2923,2952,2981,3011,3041, //15
oprospero	0:4dd021344a6b	2276	3072,3102,3133,3165,3196,3228,3261,3293, //23
oprospero	0:4dd021344a6b	2277	3326,3359,3393,3427,3461,3496,3531,3566, //31
oprospero	0:4dd021344a6b	2278	3602,3638,3674,3711,3748,3786,3823,3862, //39
oprospero	0:4dd021344a6b	2279	3900,3939,3979,4019,4059,4099,4140,4182, //47
oprospero	0:4dd021344a6b	2280	4224,4266,4308,4352,4395,4439,4483,4528, //55
oprospero	0:4dd021344a6b	2281	4574,4619,4665,4712,4759,4807,4855,4903, //63
oprospero	0:4dd021344a6b	2282	4953,5002,5052,5103,5154,5205,5257,5310, //71
oprospero	0:4dd021344a6b	2283	5363,5417,5471,5525,5581,5636,5693,5750, //79
oprospero	0:4dd021344a6b	2284	5807,5865,5924,5983,6043,6104,6165,6226, //87
oprospero	0:4dd021344a6b	2285	6289,6351,6415,6479,6544,6609,6675,6742, //95
oprospero	0:4dd021344a6b	2286	6810,6878,6946,7016,7086,7157,7229,7301, //103
oprospero	0:4dd021344a6b	2287	7374,7448,7522,7597,7673,7750,7828,7906, //111
oprospero	0:4dd021344a6b	2288	7985,8065,8145,8227,8309,8392,8476,8561, //119
oprospero	0:4dd021344a6b	2289	8647,8733,8820,8909,8998,9088,9178,9270,
oprospero	0:4dd021344a6b	2290	9363,9457,9551,9647,9743,9841,9939,10038,
oprospero	0:4dd021344a6b	2291	10139,10240,10343,10446,10550,10656,10763,10870,
oprospero	0:4dd021344a6b	2292	10979,11089,11200,11312,11425,11539,11654,11771,
oprospero	0:4dd021344a6b	2293	11889,12008,12128,12249,12371,12495,12620,12746,
oprospero	0:4dd021344a6b	2294	12874,13002,13132,13264,13396,13530,13666,13802,
oprospero	0:4dd021344a6b	2295	13940,14080,14221,14363,14506,14652,14798,14946,
oprospero	0:4dd021344a6b	2296	15096,15247,15399,15553,15709,15866,16024,16184,
oprospero	0:4dd021344a6b	2297	16346,16510,16675,16842,17010,17180,17352,17526,
oprospero	0:4dd021344a6b	2298	17701,17878,18057,18237,18420,18604,18790,18978,
oprospero	0:4dd021344a6b	2299	19167,19359,19553,19748,19946,20145,20347,20550,
oprospero	0:4dd021344a6b	2300	20756,20963,21173,21385,21598,21814,22033,22253,
oprospero	0:4dd021344a6b	2301	22475,22700,22927,23156,23388,23622,23858,24097,
oprospero	0:4dd021344a6b	2302	24338,24581,24827,25075,25326,25579,25835,26093,
oprospero	0:4dd021344a6b	2303	26354,26618,26884,27153,27424,27699,27976,28255,
oprospero	0:4dd021344a6b	2304	28538,28823,29112,29403,29697,29994,30294,30597,
oprospero	0:4dd021344a6b	2305	30903,31212,31524,31839,32157,32479,32804,33132
oprospero	0:4dd021344a6b	2306	};
oprospero	0:4dd021344a6b	2307	static int accel_6500_self_test(long bias_regular, long bias_st, int debug)
oprospero	0:4dd021344a6b	2308	{
oprospero	0:4dd021344a6b	2309	int i, result = 0, otp_value_zero = 0;
oprospero	0:4dd021344a6b	2310	float accel_st_al_min, accel_st_al_max;
oprospero	0:4dd021344a6b	2311	float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max;
oprospero	0:4dd021344a6b	2312	unsigned char regs[3];
oprospero	0:4dd021344a6b	2313	if (i2Cdev.readBytes(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) {
oprospero	0:4dd021344a6b	2314	if(debug)
oprospero	0:4dd021344a6b	2315	log_i("Reading OTP Register Error.\n");
oprospero	0:4dd021344a6b	2316	return 0x07;
oprospero	0:4dd021344a6b	2317	}
oprospero	0:4dd021344a6b	2318	if(debug)
oprospero	0:4dd021344a6b	2319	log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]);
oprospero	0:4dd021344a6b	2320	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2321	if (regs[i] != 0) {
oprospero	0:4dd021344a6b	2322	ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
oprospero	0:4dd021344a6b	2323	ct_shift_prod[i] *= 65536.f;
oprospero	0:4dd021344a6b	2324	ct_shift_prod[i] /= test.accel_sens;
oprospero	0:4dd021344a6b	2325	}
oprospero	0:4dd021344a6b	2326	else {
oprospero	0:4dd021344a6b	2327	ct_shift_prod[i] = 0;
oprospero	0:4dd021344a6b	2328	otp_value_zero = 1;
oprospero	0:4dd021344a6b	2329	}
oprospero	0:4dd021344a6b	2330	}
oprospero	0:4dd021344a6b	2331	if(otp_value_zero == 0) {
oprospero	0:4dd021344a6b	2332	if(debug)
oprospero	0:4dd021344a6b	2333	log_i("ACCEL:CRITERIA A\n");
oprospero	0:4dd021344a6b	2334	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2335	st_shift_cust[i] = bias_st[i] - bias_regular[i];
oprospero	0:4dd021344a6b	2336	if(debug) {
oprospero	0:4dd021344a6b	2337	log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
oprospero	0:4dd021344a6b	2338	st_shift_cust[i]/1.f, bias_regular[i]/1.f,
oprospero	0:4dd021344a6b	2339	bias_st[i]/1.f);
oprospero	0:4dd021344a6b	2340	log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
oprospero	0:4dd021344a6b	2341	}
oprospero	0:4dd021344a6b	2342
oprospero	0:4dd021344a6b	2343	st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
oprospero	0:4dd021344a6b	2344
oprospero	0:4dd021344a6b	2345	if(debug)
oprospero	0:4dd021344a6b	2346	log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
oprospero	0:4dd021344a6b	2347	test.max_accel_var/1.f);
oprospero	0:4dd021344a6b	2348
oprospero	0:4dd021344a6b	2349	if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
oprospero	0:4dd021344a6b	2350	if(debug)
oprospero	0:4dd021344a6b	2351	log_i("ACCEL Fail Axis = %d\n", i);
oprospero	0:4dd021344a6b	2352	result \|= 1 << i; //Error condition
oprospero	0:4dd021344a6b	2353	}
oprospero	0:4dd021344a6b	2354	}
oprospero	0:4dd021344a6b	2355	}
oprospero	0:4dd021344a6b	2356	else {
oprospero	0:4dd021344a6b	2357	/* Self Test Pass/Fail Criteria B */
oprospero	0:4dd021344a6b	2358	accel_st_al_min = test.min_g * 65536.f;
oprospero	0:4dd021344a6b	2359	accel_st_al_max = test.max_g * 65536.f;
oprospero	0:4dd021344a6b	2360
oprospero	0:4dd021344a6b	2361	if(debug) {
oprospero	0:4dd021344a6b	2362	log_i("ACCEL:CRITERIA B\r\n");
oprospero	0:4dd021344a6b	2363	log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f);
oprospero	0:4dd021344a6b	2364	log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f);
oprospero	0:4dd021344a6b	2365	}
oprospero	0:4dd021344a6b	2366
oprospero	0:4dd021344a6b	2367	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2368	st_shift_cust[i] = bias_st[i] - bias_regular[i];
oprospero	0:4dd021344a6b	2369
oprospero	0:4dd021344a6b	2370	if(debug)
oprospero	0:4dd021344a6b	2371	log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f);
oprospero	0:4dd021344a6b	2372	if(st_shift_cust[i] < accel_st_al_min \|\| st_shift_cust[i] > accel_st_al_max) {
oprospero	0:4dd021344a6b	2373	if(debug)
oprospero	0:4dd021344a6b	2374	log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i);
oprospero	0:4dd021344a6b	2375	result \|= 1 << i; //Error condition
oprospero	0:4dd021344a6b	2376	}
oprospero	0:4dd021344a6b	2377	}
oprospero	0:4dd021344a6b	2378	}
oprospero	0:4dd021344a6b	2379
oprospero	0:4dd021344a6b	2380	if(result == 0) {
oprospero	0:4dd021344a6b	2381	/* Self Test Pass/Fail Criteria C */
oprospero	0:4dd021344a6b	2382	accel_offset_max = test.max_g_offset * 65536.f;
oprospero	0:4dd021344a6b	2383	if(debug)
oprospero	0:4dd021344a6b	2384	log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f);
oprospero	0:4dd021344a6b	2385	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2386	if(fabs(bias_regular[i]) > accel_offset_max) {
oprospero	0:4dd021344a6b	2387	if(debug)
oprospero	0:4dd021344a6b	2388	log_i("FAILED: Accel axis:%d = %d > 500mg\n", i, bias_regular[i]);
oprospero	0:4dd021344a6b	2389	result \|= 1 << i; //Error condition
oprospero	0:4dd021344a6b	2390	}
oprospero	0:4dd021344a6b	2391	}
oprospero	0:4dd021344a6b	2392	}
oprospero	0:4dd021344a6b	2393
oprospero	0:4dd021344a6b	2394	return result;
oprospero	0:4dd021344a6b	2395	}
oprospero	0:4dd021344a6b	2396
oprospero	0:4dd021344a6b	2397	static int gyro_6500_self_test(long bias_regular, long bias_st, int debug)
oprospero	0:4dd021344a6b	2398	{
oprospero	0:4dd021344a6b	2399	int i, result = 0, otp_value_zero = 0;
oprospero	0:4dd021344a6b	2400	float gyro_st_al_max;
oprospero	0:4dd021344a6b	2401	float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max;
oprospero	0:4dd021344a6b	2402	unsigned char regs[3];
oprospero	0:4dd021344a6b	2403
oprospero	0:4dd021344a6b	2404	if (i2Cdev.readBytes(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) {
oprospero	0:4dd021344a6b	2405	if(debug)
oprospero	0:4dd021344a6b	2406	log_i("Reading OTP Register Error.\n");
oprospero	0:4dd021344a6b	2407	return 0x07;
oprospero	0:4dd021344a6b	2408	}
oprospero	0:4dd021344a6b	2409
oprospero	0:4dd021344a6b	2410	if(debug)
oprospero	0:4dd021344a6b	2411	log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]);
oprospero	0:4dd021344a6b	2412
oprospero	0:4dd021344a6b	2413	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2414	if (regs[i] != 0) {
oprospero	0:4dd021344a6b	2415	ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
oprospero	0:4dd021344a6b	2416	ct_shift_prod[i] *= 65536.f;
oprospero	0:4dd021344a6b	2417	ct_shift_prod[i] /= test.gyro_sens;
oprospero	0:4dd021344a6b	2418	}
oprospero	0:4dd021344a6b	2419	else {
oprospero	0:4dd021344a6b	2420	ct_shift_prod[i] = 0;
oprospero	0:4dd021344a6b	2421	otp_value_zero = 1;
oprospero	0:4dd021344a6b	2422	}
oprospero	0:4dd021344a6b	2423	}
oprospero	0:4dd021344a6b	2424
oprospero	0:4dd021344a6b	2425	if(otp_value_zero == 0) {
oprospero	0:4dd021344a6b	2426	if(debug)
oprospero	0:4dd021344a6b	2427	log_i("GYRO:CRITERIA A\n");
oprospero	0:4dd021344a6b	2428	/* Self Test Pass/Fail Criteria A */
oprospero	0:4dd021344a6b	2429	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2430	st_shift_cust[i] = bias_st[i] - bias_regular[i];
oprospero	0:4dd021344a6b	2431
oprospero	0:4dd021344a6b	2432	if(debug) {
oprospero	0:4dd021344a6b	2433	log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
oprospero	0:4dd021344a6b	2434	st_shift_cust[i]/1.f, bias_regular[i]/1.f,
oprospero	0:4dd021344a6b	2435	bias_st[i]/1.f);
oprospero	0:4dd021344a6b	2436	log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
oprospero	0:4dd021344a6b	2437	}
oprospero	0:4dd021344a6b	2438
oprospero	0:4dd021344a6b	2439	st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
oprospero	0:4dd021344a6b	2440
oprospero	0:4dd021344a6b	2441	if(debug)
oprospero	0:4dd021344a6b	2442	log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
oprospero	0:4dd021344a6b	2443	test.max_gyro_var/1.f);
oprospero	0:4dd021344a6b	2444
oprospero	0:4dd021344a6b	2445	if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
oprospero	0:4dd021344a6b	2446	if(debug)
oprospero	0:4dd021344a6b	2447	log_i("Gyro Fail Axis = %d\n", i);
oprospero	0:4dd021344a6b	2448	result \|= 1 << i; //Error condition
oprospero	0:4dd021344a6b	2449	}
oprospero	0:4dd021344a6b	2450	}
oprospero	0:4dd021344a6b	2451	}
oprospero	0:4dd021344a6b	2452	else {
oprospero	0:4dd021344a6b	2453	/* Self Test Pass/Fail Criteria B */
oprospero	0:4dd021344a6b	2454	gyro_st_al_max = test.max_dps * 65536.f;
oprospero	0:4dd021344a6b	2455
oprospero	0:4dd021344a6b	2456	if(debug) {
oprospero	0:4dd021344a6b	2457	log_i("GYRO:CRITERIA B\r\n");
oprospero	0:4dd021344a6b	2458	log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f);
oprospero	0:4dd021344a6b	2459	}
oprospero	0:4dd021344a6b	2460
oprospero	0:4dd021344a6b	2461	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2462	st_shift_cust[i] = bias_st[i] - bias_regular[i];
oprospero	0:4dd021344a6b	2463
oprospero	0:4dd021344a6b	2464	if(debug)
oprospero	0:4dd021344a6b	2465	log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f);
oprospero	0:4dd021344a6b	2466	if(st_shift_cust[i] < gyro_st_al_max) {
oprospero	0:4dd021344a6b	2467	if(debug)
oprospero	0:4dd021344a6b	2468	log_i("GYRO FAIL axis:%d greater than 60dps\n", i);
oprospero	0:4dd021344a6b	2469	result \|= 1 << i; //Error condition
oprospero	0:4dd021344a6b	2470	}
oprospero	0:4dd021344a6b	2471	}
oprospero	0:4dd021344a6b	2472	}
oprospero	0:4dd021344a6b	2473
oprospero	0:4dd021344a6b	2474	if(result == 0) {
oprospero	0:4dd021344a6b	2475	/* Self Test Pass/Fail Criteria C */
oprospero	0:4dd021344a6b	2476	gyro_offset_max = test.min_dps * 65536.f;
oprospero	0:4dd021344a6b	2477	if(debug)
oprospero	0:4dd021344a6b	2478	log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f);
oprospero	0:4dd021344a6b	2479	for (i = 0; i < 3; i++) {
oprospero	0:4dd021344a6b	2480	if(fabs(bias_regular[i]) > gyro_offset_max) {
oprospero	0:4dd021344a6b	2481	if(debug)
oprospero	0:4dd021344a6b	2482	log_i("FAILED: Gyro axis:%d = %d > 20dps\n", i, bias_regular[i]);
oprospero	0:4dd021344a6b	2483	result \|= 1 << i; //Error condition
oprospero	0:4dd021344a6b	2484	}
oprospero	0:4dd021344a6b	2485	}
oprospero	0:4dd021344a6b	2486	}
oprospero	0:4dd021344a6b	2487	return result;
oprospero	0:4dd021344a6b	2488	}
oprospero	0:4dd021344a6b	2489
oprospero	0:4dd021344a6b	2490	static int get_st_6500_biases(long gyro, long accel, unsigned char hw_test, int debug)
oprospero	0:4dd021344a6b	2491	{
oprospero	0:4dd021344a6b	2492	unsigned char data[HWST_MAX_PACKET_LENGTH];
oprospero	0:4dd021344a6b	2493	unsigned char packet_count, ii;
oprospero	0:4dd021344a6b	2494	unsigned short fifo_count;
oprospero	0:4dd021344a6b	2495	int s = 0, read_size = 0, ind;
oprospero	0:4dd021344a6b	2496
oprospero	0:4dd021344a6b	2497	data[0] = 0x01;
oprospero	0:4dd021344a6b	2498	data[1] = 0;
oprospero	0:4dd021344a6b	2499	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
oprospero	0:4dd021344a6b	2500	return -1;
oprospero	0:4dd021344a6b	2501	wait_ms(200);
oprospero	0:4dd021344a6b	2502	data[0] = 0;
oprospero	0:4dd021344a6b	2503	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, data))
oprospero	0:4dd021344a6b	2504	return -1;
oprospero	0:4dd021344a6b	2505	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, data))
oprospero	0:4dd021344a6b	2506	return -1;
oprospero	0:4dd021344a6b	2507	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
oprospero	0:4dd021344a6b	2508	return -1;
oprospero	0:4dd021344a6b	2509	if (i2Cdev.writeBytes(st.hw->addr, st.reg->i2c_mst, 1, data))
oprospero	0:4dd021344a6b	2510	return -1;
oprospero	0:4dd021344a6b	2511	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, data))
oprospero	0:4dd021344a6b	2512	return -1;
oprospero	0:4dd021344a6b	2513	data[0] = BIT_FIFO_RST \| BIT_DMP_RST;
oprospero	0:4dd021344a6b	2514	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, data))
oprospero	0:4dd021344a6b	2515	return -1;
oprospero	0:4dd021344a6b	2516	wait_ms(15);
oprospero	0:4dd021344a6b	2517	data[0] = st.test->reg_lpf;
oprospero	0:4dd021344a6b	2518	if (i2Cdev.writeBytes(st.hw->addr, st.reg->lpf, 1, data))
oprospero	0:4dd021344a6b	2519	return -1;
oprospero	0:4dd021344a6b	2520	data[0] = st.test->reg_rate_div;
oprospero	0:4dd021344a6b	2521	if (i2Cdev.writeBytes(st.hw->addr, st.reg->rate_div, 1, data))
oprospero	0:4dd021344a6b	2522	return -1;
oprospero	0:4dd021344a6b	2523	if (hw_test)
oprospero	0:4dd021344a6b	2524	data[0] = st.test->reg_gyro_fsr \| 0xE0;
oprospero	0:4dd021344a6b	2525	else
oprospero	0:4dd021344a6b	2526	data[0] = st.test->reg_gyro_fsr;
oprospero	0:4dd021344a6b	2527	if (i2Cdev.writeBytes(st.hw->addr, st.reg->gyro_cfg, 1, data))
oprospero	0:4dd021344a6b	2528	return -1;
oprospero	0:4dd021344a6b	2529
oprospero	0:4dd021344a6b	2530	if (hw_test)
oprospero	0:4dd021344a6b	2531	data[0] = st.test->reg_accel_fsr \| 0xE0;
oprospero	0:4dd021344a6b	2532	else
oprospero	0:4dd021344a6b	2533	data[0] = test.reg_accel_fsr;
oprospero	0:4dd021344a6b	2534	if (i2Cdev.writeBytes(st.hw->addr, st.reg->accel_cfg, 1, data))
oprospero	0:4dd021344a6b	2535	return -1;
oprospero	0:4dd021344a6b	2536
oprospero	0:4dd021344a6b	2537	wait_ms(test.wait_ms); //wait 200ms for sensors to stabilize
oprospero	0:4dd021344a6b	2538
oprospero	0:4dd021344a6b	2539	/* Enable FIFO */
oprospero	0:4dd021344a6b	2540	data[0] = BIT_FIFO_EN;
oprospero	0:4dd021344a6b	2541	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 1, data))
oprospero	0:4dd021344a6b	2542	return -1;
oprospero	0:4dd021344a6b	2543	data[0] = INV_XYZ_GYRO \| INV_XYZ_ACCEL;
oprospero	0:4dd021344a6b	2544	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, data))
oprospero	0:4dd021344a6b	2545	return -1;
oprospero	0:4dd021344a6b	2546
oprospero	0:4dd021344a6b	2547	//initialize the bias return values
oprospero	0:4dd021344a6b	2548	gyro[0] = gyro[1] = gyro[2] = 0;
oprospero	0:4dd021344a6b	2549	accel[0] = accel[1] = accel[2] = 0;
oprospero	0:4dd021344a6b	2550
oprospero	0:4dd021344a6b	2551	if(debug)
oprospero	0:4dd021344a6b	2552	log_i("Starting Bias Loop Reads\n");
oprospero	0:4dd021344a6b	2553
oprospero	0:4dd021344a6b	2554	//start reading samples
oprospero	0:4dd021344a6b	2555	while (s < test.packet_thresh) {
oprospero	0:4dd021344a6b	2556	wait_ms(test.sample_wait_ms); //wait 10ms to fill FIFO
oprospero	0:4dd021344a6b	2557	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_count_h, 2, data))
oprospero	0:4dd021344a6b	2558	return -1;
oprospero	0:4dd021344a6b	2559	fifo_count = (data[0] << 8) \| data[1];
oprospero	0:4dd021344a6b	2560	packet_count = fifo_count / MAX_PACKET_LENGTH;
oprospero	0:4dd021344a6b	2561	if ((test.packet_thresh - s) < packet_count)
oprospero	0:4dd021344a6b	2562	packet_count = test.packet_thresh - s;
oprospero	0:4dd021344a6b	2563	read_size = packet_count * MAX_PACKET_LENGTH;
oprospero	0:4dd021344a6b	2564
oprospero	0:4dd021344a6b	2565	//burst read from FIFO
oprospero	0:4dd021344a6b	2566	if (i2Cdev.readBytes(st.hw->addr, st.reg->fifo_r_w, read_size, data))
oprospero	0:4dd021344a6b	2567	return -1;
oprospero	0:4dd021344a6b	2568	ind = 0;
oprospero	0:4dd021344a6b	2569	for (ii = 0; ii < packet_count; ii++) {
oprospero	0:4dd021344a6b	2570	short accel_cur[3], gyro_cur[3];
oprospero	0:4dd021344a6b	2571	accel_cur[0] = ((short)data[ind + 0] << 8) \| data[ind + 1];
oprospero	0:4dd021344a6b	2572	accel_cur[1] = ((short)data[ind + 2] << 8) \| data[ind + 3];
oprospero	0:4dd021344a6b	2573	accel_cur[2] = ((short)data[ind + 4] << 8) \| data[ind + 5];
oprospero	0:4dd021344a6b	2574	accel[0] += (long)accel_cur[0];
oprospero	0:4dd021344a6b	2575	accel[1] += (long)accel_cur[1];
oprospero	0:4dd021344a6b	2576	accel[2] += (long)accel_cur[2];
oprospero	0:4dd021344a6b	2577	gyro_cur[0] = (((short)data[ind + 6] << 8) \| data[ind + 7]);
oprospero	0:4dd021344a6b	2578	gyro_cur[1] = (((short)data[ind + 8] << 8) \| data[ind + 9]);
oprospero	0:4dd021344a6b	2579	gyro_cur[2] = (((short)data[ind + 10] << 8) \| data[ind + 11]);
oprospero	0:4dd021344a6b	2580	gyro[0] += (long)gyro_cur[0];
oprospero	0:4dd021344a6b	2581	gyro[1] += (long)gyro_cur[1];
oprospero	0:4dd021344a6b	2582	gyro[2] += (long)gyro_cur[2];
oprospero	0:4dd021344a6b	2583	ind += MAX_PACKET_LENGTH;
oprospero	0:4dd021344a6b	2584	}
oprospero	0:4dd021344a6b	2585	s += packet_count;
oprospero	0:4dd021344a6b	2586	}
oprospero	0:4dd021344a6b	2587
oprospero	0:4dd021344a6b	2588	if(debug)
oprospero	0:4dd021344a6b	2589	log_i("Samples: %d\n", s);
oprospero	0:4dd021344a6b	2590
oprospero	0:4dd021344a6b	2591	//stop FIFO
oprospero	0:4dd021344a6b	2592	data[0] = 0;
oprospero	0:4dd021344a6b	2593	if (i2Cdev.writeBytes(st.hw->addr, st.reg->fifo_en, 1, data))
oprospero	0:4dd021344a6b	2594	return -1;
oprospero	0:4dd021344a6b	2595
oprospero	0:4dd021344a6b	2596	gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s);
oprospero	0:4dd021344a6b	2597	gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s);
oprospero	0:4dd021344a6b	2598	gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s);
oprospero	0:4dd021344a6b	2599	accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s);
oprospero	0:4dd021344a6b	2600	accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s);
oprospero	0:4dd021344a6b	2601	accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s);
oprospero	0:4dd021344a6b	2602	/* remove gravity from bias calculation */
oprospero	0:4dd021344a6b	2603	if (accel[2] > 0L)
oprospero	0:4dd021344a6b	2604	accel[2] -= 65536L;
oprospero	0:4dd021344a6b	2605	else
oprospero	0:4dd021344a6b	2606	accel[2] += 65536L;
oprospero	0:4dd021344a6b	2607
oprospero	0:4dd021344a6b	2608
oprospero	0:4dd021344a6b	2609	if(debug) {
oprospero	0:4dd021344a6b	2610	log_i("Accel offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, accel[0]/65536.f, accel[1]/65536.f, accel[2]/65536.f);
oprospero	0:4dd021344a6b	2611	log_i("Gyro offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, gyro[0]/65536.f, gyro[1]/65536.f, gyro[2]/65536.f);
oprospero	0:4dd021344a6b	2612	}
oprospero	0:4dd021344a6b	2613
oprospero	0:4dd021344a6b	2614	return 0;
oprospero	0:4dd021344a6b	2615	}
oprospero	0:4dd021344a6b	2616	/**
oprospero	0:4dd021344a6b	2617	* @brief Trigger gyro/accel/compass self-test for MPU6500/MPU9250
oprospero	0:4dd021344a6b	2618	* On success/error, the self-test returns a mask representing the sensor(s)
oprospero	0:4dd021344a6b	2619	* that failed. For each bit, a one (1) represents a "pass" case; conversely,
oprospero	0:4dd021344a6b	2620	* a zero (0) indicates a failure.
oprospero	0:4dd021344a6b	2621	*
oprospero	0:4dd021344a6b	2622	* \n The mask is defined as follows:
oprospero	0:4dd021344a6b	2623	* \n Bit 0: Gyro.
oprospero	0:4dd021344a6b	2624	* \n Bit 1: Accel.
oprospero	0:4dd021344a6b	2625	* \n Bit 2: Compass.
oprospero	0:4dd021344a6b	2626	*
oprospero	0:4dd021344a6b	2627	* @param[out] gyro Gyro biases in q16 format.
oprospero	0:4dd021344a6b	2628	* @param[out] accel Accel biases (if applicable) in q16 format.
oprospero	0:4dd021344a6b	2629	* @param[in] debug Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver.
oprospero	0:4dd021344a6b	2630	* @return Result mask (see above).
oprospero	0:4dd021344a6b	2631	*/
oprospero	0:4dd021344a6b	2632	int InvMpu::mpu_run_6500_self_test(long gyro, long accel, unsigned char debug)
oprospero	0:4dd021344a6b	2633	{
oprospero	0:4dd021344a6b	2634	const unsigned char tries = 2;
oprospero	0:4dd021344a6b	2635	long gyro_st[3], accel_st[3];
oprospero	0:4dd021344a6b	2636	unsigned char accel_result, gyro_result;
oprospero	0:4dd021344a6b	2637	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	2638	unsigned char compass_result;
oprospero	0:4dd021344a6b	2639	#endif
oprospero	0:4dd021344a6b	2640	int ii;
oprospero	0:4dd021344a6b	2641
oprospero	0:4dd021344a6b	2642	int result;
oprospero	0:4dd021344a6b	2643	unsigned char accel_fsr, fifo_sensors, sensors_on;
oprospero	0:4dd021344a6b	2644	unsigned short gyro_fsr, sample_rate, lpf;
oprospero	0:4dd021344a6b	2645	unsigned char dmp_was_on;
oprospero	0:4dd021344a6b	2646
oprospero	0:4dd021344a6b	2647
oprospero	0:4dd021344a6b	2648
oprospero	0:4dd021344a6b	2649	if(debug)
oprospero	0:4dd021344a6b	2650	log_i("Starting MPU6500 HWST!\r\n");
oprospero	0:4dd021344a6b	2651
oprospero	0:4dd021344a6b	2652	if (st.chip_cfg.dmp_on) {
oprospero	0:4dd021344a6b	2653	mpu_set_dmp_state(0);
oprospero	0:4dd021344a6b	2654	dmp_was_on = 1;
oprospero	0:4dd021344a6b	2655	} else
oprospero	0:4dd021344a6b	2656	dmp_was_on = 0;
oprospero	0:4dd021344a6b	2657
oprospero	0:4dd021344a6b	2658	/* Get initial settings. */
oprospero	0:4dd021344a6b	2659	mpu_get_gyro_fsr(&gyro_fsr);
oprospero	0:4dd021344a6b	2660	mpu_get_accel_fsr(&accel_fsr);
oprospero	0:4dd021344a6b	2661	mpu_get_lpf(&lpf);
oprospero	0:4dd021344a6b	2662	mpu_get_sample_rate(&sample_rate);
oprospero	0:4dd021344a6b	2663	sensors_on = st.chip_cfg.sensors;
oprospero	0:4dd021344a6b	2664	mpu_get_fifo_config(&fifo_sensors);
oprospero	0:4dd021344a6b	2665
oprospero	0:4dd021344a6b	2666	if(debug)
oprospero	0:4dd021344a6b	2667	log_i("Retrieving Biases\r\n");
oprospero	0:4dd021344a6b	2668
oprospero	0:4dd021344a6b	2669	for (ii = 0; ii < tries; ii++)
oprospero	0:4dd021344a6b	2670	if (!get_st_6500_biases(gyro, accel, 0, debug))
oprospero	0:4dd021344a6b	2671	break;
oprospero	0:4dd021344a6b	2672	if (ii == tries) {
oprospero	0:4dd021344a6b	2673	/* If we reach this point, we most likely encountered an I2C error.
oprospero	0:4dd021344a6b	2674	* We'll just report an error for all three sensors.
oprospero	0:4dd021344a6b	2675	*/
oprospero	0:4dd021344a6b	2676	if(debug)
oprospero	0:4dd021344a6b	2677	log_i("Retrieving Biases Error - possible I2C error\n");
oprospero	0:4dd021344a6b	2678
oprospero	0:4dd021344a6b	2679	result = 0;
oprospero	0:4dd021344a6b	2680	goto restore;
oprospero	0:4dd021344a6b	2681	}
oprospero	0:4dd021344a6b	2682
oprospero	0:4dd021344a6b	2683	if(debug)
oprospero	0:4dd021344a6b	2684	log_i("Retrieving ST Biases\n");
oprospero	0:4dd021344a6b	2685
oprospero	0:4dd021344a6b	2686	for (ii = 0; ii < tries; ii++)
oprospero	0:4dd021344a6b	2687	if (!get_st_6500_biases(gyro_st, accel_st, 1, debug))
oprospero	0:4dd021344a6b	2688	break;
oprospero	0:4dd021344a6b	2689	if (ii == tries) {
oprospero	0:4dd021344a6b	2690
oprospero	0:4dd021344a6b	2691	if(debug)
oprospero	0:4dd021344a6b	2692	log_i("Retrieving ST Biases Error - possible I2C error\n");
oprospero	0:4dd021344a6b	2693
oprospero	0:4dd021344a6b	2694	/* Again, probably an I2C error. */
oprospero	0:4dd021344a6b	2695	result = 0;
oprospero	0:4dd021344a6b	2696	goto restore;
oprospero	0:4dd021344a6b	2697	}
oprospero	0:4dd021344a6b	2698
oprospero	0:4dd021344a6b	2699	accel_result = accel_6500_self_test(accel, accel_st, debug);
oprospero	0:4dd021344a6b	2700	if(debug)
oprospero	0:4dd021344a6b	2701	log_i("Accel Self Test Results: %d\n", accel_result);
oprospero	0:4dd021344a6b	2702
oprospero	0:4dd021344a6b	2703	gyro_result = gyro_6500_self_test(gyro, gyro_st, debug);
oprospero	0:4dd021344a6b	2704	if(debug)
oprospero	0:4dd021344a6b	2705	log_i("Gyro Self Test Results: %d\n", gyro_result);
oprospero	0:4dd021344a6b	2706
oprospero	0:4dd021344a6b	2707	result = 0;
oprospero	0:4dd021344a6b	2708	if (!gyro_result)
oprospero	0:4dd021344a6b	2709	result \|= 0x01;
oprospero	0:4dd021344a6b	2710	if (!accel_result)
oprospero	0:4dd021344a6b	2711	result \|= 0x02;
oprospero	0:4dd021344a6b	2712
oprospero	0:4dd021344a6b	2713	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	2714	compass_result = compass_self_test();
oprospero	0:4dd021344a6b	2715	if(debug)
oprospero	0:4dd021344a6b	2716	log_i("Compass Self Test Results: %d\n", compass_result);
oprospero	0:4dd021344a6b	2717	if (!compass_result)
oprospero	0:4dd021344a6b	2718	result \|= 0x04;
oprospero	0:4dd021344a6b	2719	#else
oprospero	0:4dd021344a6b	2720	result \|= 0x04;
oprospero	0:4dd021344a6b	2721	#endif
oprospero	0:4dd021344a6b	2722	restore:
oprospero	0:4dd021344a6b	2723	if(debug)
oprospero	0:4dd021344a6b	2724	log_i("Exiting HWST\n");
oprospero	0:4dd021344a6b	2725	/* Set to invalid values to ensure no I2C writes are skipped. */
oprospero	0:4dd021344a6b	2726	st.chip_cfg.gyro_fsr = 0xFF;
oprospero	0:4dd021344a6b	2727	st.chip_cfg.accel_fsr = 0xFF;
oprospero	0:4dd021344a6b	2728	st.chip_cfg.lpf = 0xFF;
oprospero	0:4dd021344a6b	2729	st.chip_cfg.sample_rate = 0xFFFF;
oprospero	0:4dd021344a6b	2730	st.chip_cfg.sensors = 0xFF;
oprospero	0:4dd021344a6b	2731	st.chip_cfg.fifo_enable = 0xFF;
oprospero	0:4dd021344a6b	2732	st.chip_cfg.clk_src = INV_CLK_PLL;
oprospero	0:4dd021344a6b	2733	mpu_set_gyro_fsr(gyro_fsr);
oprospero	0:4dd021344a6b	2734	mpu_set_accel_fsr(accel_fsr);
oprospero	0:4dd021344a6b	2735	mpu_set_lpf(lpf);
oprospero	0:4dd021344a6b	2736	mpu_set_sample_rate(sample_rate);
oprospero	0:4dd021344a6b	2737	mpu_set_sensors(sensors_on);
oprospero	0:4dd021344a6b	2738	mpu_configure_fifo(fifo_sensors);
oprospero	0:4dd021344a6b	2739
oprospero	0:4dd021344a6b	2740	if (dmp_was_on)
oprospero	0:4dd021344a6b	2741	mpu_set_dmp_state(1);
oprospero	0:4dd021344a6b	2742
oprospero	0:4dd021344a6b	2743	return result;
oprospero	0:4dd021344a6b	2744	}
oprospero	0:4dd021344a6b	2745	#endif
oprospero	0:4dd021344a6b	2746	/*
oprospero	0:4dd021344a6b	2747	* \n This function must be called with the device either face-up or face-down
oprospero	0:4dd021344a6b	2748	* (z-axis is parallel to gravity).
oprospero	0:4dd021344a6b	2749	* @param[out] gyro Gyro biases in q16 format.
oprospero	0:4dd021344a6b	2750	* @param[out] accel Accel biases (if applicable) in q16 format.
oprospero	0:4dd021344a6b	2751	* @return Result mask (see above).
oprospero	0:4dd021344a6b	2752	*/
oprospero	0:4dd021344a6b	2753	int InvMpu::mpu_run_self_test(long gyro, long accel)
oprospero	0:4dd021344a6b	2754	{
oprospero	0:4dd021344a6b	2755	#ifdef MPU6050
oprospero	0:4dd021344a6b	2756	const unsigned char tries = 2;
oprospero	0:4dd021344a6b	2757	long gyro_st[3], accel_st[3];
oprospero	0:4dd021344a6b	2758	unsigned char accel_result, gyro_result;
oprospero	0:4dd021344a6b	2759	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	2760	unsigned char compass_result;
oprospero	0:4dd021344a6b	2761	#endif
oprospero	0:4dd021344a6b	2762	int ii;
oprospero	0:4dd021344a6b	2763	#endif
oprospero	0:4dd021344a6b	2764	int result;
oprospero	0:4dd021344a6b	2765	unsigned char accel_fsr, fifo_sensors, sensors_on;
oprospero	0:4dd021344a6b	2766	unsigned short gyro_fsr, sample_rate, lpf;
oprospero	0:4dd021344a6b	2767	unsigned char dmp_was_on;
oprospero	0:4dd021344a6b	2768
oprospero	0:4dd021344a6b	2769	if (st.chip_cfg.dmp_on) {
oprospero	0:4dd021344a6b	2770	mpu_set_dmp_state(0);
oprospero	0:4dd021344a6b	2771	dmp_was_on = 1;
oprospero	0:4dd021344a6b	2772	} else
oprospero	0:4dd021344a6b	2773	dmp_was_on = 0;
oprospero	0:4dd021344a6b	2774
oprospero	0:4dd021344a6b	2775	/* Get initial settings. */
oprospero	0:4dd021344a6b	2776	mpu_get_gyro_fsr(&gyro_fsr);
oprospero	0:4dd021344a6b	2777	mpu_get_accel_fsr(&accel_fsr);
oprospero	0:4dd021344a6b	2778	mpu_get_lpf(&lpf);
oprospero	0:4dd021344a6b	2779	mpu_get_sample_rate(&sample_rate);
oprospero	0:4dd021344a6b	2780	sensors_on = st.chip_cfg.sensors;
oprospero	0:4dd021344a6b	2781	mpu_get_fifo_config(&fifo_sensors);
oprospero	0:4dd021344a6b	2782
oprospero	0:4dd021344a6b	2783	/* For older chips, the self-test will be different. */
oprospero	0:4dd021344a6b	2784	#if defined MPU6050
oprospero	0:4dd021344a6b	2785	for (ii = 0; ii < tries; ii++)
oprospero	0:4dd021344a6b	2786	if (!get_st_biases(gyro, accel, 0))
oprospero	0:4dd021344a6b	2787	break;
oprospero	0:4dd021344a6b	2788	if (ii == tries) {
oprospero	0:4dd021344a6b	2789	/* If we reach this point, we most likely encountered an I2C error.
oprospero	0:4dd021344a6b	2790	* We'll just report an error for all three sensors.
oprospero	0:4dd021344a6b	2791	*/
oprospero	0:4dd021344a6b	2792	result = 0;
oprospero	0:4dd021344a6b	2793	goto restore;
oprospero	0:4dd021344a6b	2794	}
oprospero	0:4dd021344a6b	2795	for (ii = 0; ii < tries; ii++)
oprospero	0:4dd021344a6b	2796	if (!get_st_biases(gyro_st, accel_st, 1))
oprospero	0:4dd021344a6b	2797	break;
oprospero	0:4dd021344a6b	2798	if (ii == tries) {
oprospero	0:4dd021344a6b	2799	/* Again, probably an I2C error. */
oprospero	0:4dd021344a6b	2800	result = 0;
oprospero	0:4dd021344a6b	2801	goto restore;
oprospero	0:4dd021344a6b	2802	}
oprospero	0:4dd021344a6b	2803	accel_result = accel_self_test(accel, accel_st);
oprospero	0:4dd021344a6b	2804	gyro_result = gyro_self_test(gyro, gyro_st);
oprospero	0:4dd021344a6b	2805
oprospero	0:4dd021344a6b	2806	result = 0;
oprospero	0:4dd021344a6b	2807	if (!gyro_result)
oprospero	0:4dd021344a6b	2808	result \|= 0x01;
oprospero	0:4dd021344a6b	2809	if (!accel_result)
oprospero	0:4dd021344a6b	2810	result \|= 0x02;
oprospero	0:4dd021344a6b	2811
oprospero	0:4dd021344a6b	2812	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	2813	compass_result = compass_self_test();
oprospero	0:4dd021344a6b	2814	if (!compass_result)
oprospero	0:4dd021344a6b	2815	result \|= 0x04;
oprospero	0:4dd021344a6b	2816	#else
oprospero	0:4dd021344a6b	2817	result \|= 0x04;
oprospero	0:4dd021344a6b	2818	#endif
oprospero	0:4dd021344a6b	2819	restore:
oprospero	0:4dd021344a6b	2820	#elif defined MPU6500
oprospero	0:4dd021344a6b	2821	/* For now, this function will return a "pass" result for all three sensors
oprospero	0:4dd021344a6b	2822	* for compatibility with current test applications.
oprospero	0:4dd021344a6b	2823	*/
oprospero	0:4dd021344a6b	2824	get_st_biases(gyro, accel, 0);
oprospero	0:4dd021344a6b	2825	result = 0x7;
oprospero	0:4dd021344a6b	2826	#endif
oprospero	0:4dd021344a6b	2827	/* Set to invalid values to ensure no I2C writes are skipped. */
oprospero	0:4dd021344a6b	2828	st.chip_cfg.gyro_fsr = 0xFF;
oprospero	0:4dd021344a6b	2829	st.chip_cfg.accel_fsr = 0xFF;
oprospero	0:4dd021344a6b	2830	st.chip_cfg.lpf = 0xFF;
oprospero	0:4dd021344a6b	2831	st.chip_cfg.sample_rate = 0xFFFF;
oprospero	0:4dd021344a6b	2832	st.chip_cfg.sensors = 0xFF;
oprospero	0:4dd021344a6b	2833	st.chip_cfg.fifo_enable = 0xFF;
oprospero	0:4dd021344a6b	2834	st.chip_cfg.clk_src = INV_CLK_PLL;
oprospero	0:4dd021344a6b	2835	mpu_set_gyro_fsr(gyro_fsr);
oprospero	0:4dd021344a6b	2836	mpu_set_accel_fsr(accel_fsr);
oprospero	0:4dd021344a6b	2837	mpu_set_lpf(lpf);
oprospero	0:4dd021344a6b	2838	mpu_set_sample_rate(sample_rate);
oprospero	0:4dd021344a6b	2839	mpu_set_sensors(sensors_on);
oprospero	0:4dd021344a6b	2840	mpu_configure_fifo(fifo_sensors);
oprospero	0:4dd021344a6b	2841
oprospero	0:4dd021344a6b	2842	if (dmp_was_on)
oprospero	0:4dd021344a6b	2843	mpu_set_dmp_state(1);
oprospero	0:4dd021344a6b	2844
oprospero	0:4dd021344a6b	2845	return result;
oprospero	0:4dd021344a6b	2846	}
oprospero	0:4dd021344a6b	2847
oprospero	0:4dd021344a6b	2848	/**
oprospero	0:4dd021344a6b	2849	* @brief Write to the DMP memory.
oprospero	0:4dd021344a6b	2850	* This function prevents I2C writes past the bank boundaries. The DMP memory
oprospero	0:4dd021344a6b	2851	* is only accessible when the chip is awake.
oprospero	0:4dd021344a6b	2852	* @param[in] mem_addr Memory location (bank << 8 \| start address)
oprospero	0:4dd021344a6b	2853	* @param[in] length Number of bytes to write.
oprospero	0:4dd021344a6b	2854	* @param[in] data Bytes to write to memory.
oprospero	0:4dd021344a6b	2855	* @return 0 if successful.
oprospero	0:4dd021344a6b	2856	*/
oprospero	0:4dd021344a6b	2857	int InvMpu::mpu_write_mem(unsigned short mem_addr, unsigned short length,
oprospero	0:4dd021344a6b	2858	unsigned char *data)
oprospero	0:4dd021344a6b	2859	{
oprospero	0:4dd021344a6b	2860	unsigned char tmp[2];
oprospero	0:4dd021344a6b	2861
oprospero	0:4dd021344a6b	2862	if (!data)
oprospero	0:4dd021344a6b	2863	return -1;
oprospero	0:4dd021344a6b	2864	if (!st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	2865	return -1;
oprospero	0:4dd021344a6b	2866
oprospero	0:4dd021344a6b	2867	tmp[0] = (unsigned char)(mem_addr >> 8);
oprospero	0:4dd021344a6b	2868	tmp[1] = (unsigned char)(mem_addr & 0xFF);
oprospero	0:4dd021344a6b	2869
oprospero	0:4dd021344a6b	2870	/* Check bank boundaries. */
oprospero	0:4dd021344a6b	2871	if (tmp[1] + length > st.hw->bank_size)
oprospero	0:4dd021344a6b	2872	return -1;
oprospero	0:4dd021344a6b	2873
oprospero	0:4dd021344a6b	2874	if (i2Cdev.writeBytes(st.hw->addr, st.reg->bank_sel, 2, tmp))
oprospero	0:4dd021344a6b	2875	return -1;
oprospero	0:4dd021344a6b	2876	if (i2Cdev.writeBytes(st.hw->addr, st.reg->mem_r_w, length, data))
oprospero	0:4dd021344a6b	2877	return -1;
oprospero	0:4dd021344a6b	2878	return 0;
oprospero	0:4dd021344a6b	2879	}
oprospero	0:4dd021344a6b	2880
oprospero	0:4dd021344a6b	2881	/**
oprospero	0:4dd021344a6b	2882	* @brief Read from the DMP memory.
oprospero	0:4dd021344a6b	2883	* This function prevents I2C reads past the bank boundaries. The DMP memory
oprospero	0:4dd021344a6b	2884	* is only accessible when the chip is awake.
oprospero	0:4dd021344a6b	2885	* @param[in] mem_addr Memory location (bank << 8 \| start address)
oprospero	0:4dd021344a6b	2886	* @param[in] length Number of bytes to read.
oprospero	0:4dd021344a6b	2887	* @param[out] data Bytes read from memory.
oprospero	0:4dd021344a6b	2888	* @return 0 if successful.
oprospero	0:4dd021344a6b	2889	*/
oprospero	0:4dd021344a6b	2890	int InvMpu::mpu_read_mem(unsigned short mem_addr, unsigned short length,
oprospero	0:4dd021344a6b	2891	unsigned char *data)
oprospero	0:4dd021344a6b	2892	{
oprospero	0:4dd021344a6b	2893	unsigned char tmp[2];
oprospero	0:4dd021344a6b	2894
oprospero	0:4dd021344a6b	2895	if (!data)
oprospero	0:4dd021344a6b	2896	return -1;
oprospero	0:4dd021344a6b	2897	if (!st.chip_cfg.sensors)
oprospero	0:4dd021344a6b	2898	return -1;
oprospero	0:4dd021344a6b	2899
oprospero	0:4dd021344a6b	2900	tmp[0] = (unsigned char)(mem_addr >> 8);
oprospero	0:4dd021344a6b	2901	tmp[1] = (unsigned char)(mem_addr & 0xFF);
oprospero	0:4dd021344a6b	2902
oprospero	0:4dd021344a6b	2903	/* Check bank boundaries. */
oprospero	0:4dd021344a6b	2904	if (tmp[1] + length > st.hw->bank_size)
oprospero	0:4dd021344a6b	2905	return -1;
oprospero	0:4dd021344a6b	2906
oprospero	0:4dd021344a6b	2907	if (i2Cdev.writeBytes(st.hw->addr, st.reg->bank_sel, 2, tmp))
oprospero	0:4dd021344a6b	2908	return -1;
oprospero	0:4dd021344a6b	2909	if (i2Cdev.readBytes(st.hw->addr, st.reg->mem_r_w, length, data))
oprospero	0:4dd021344a6b	2910	return -1;
oprospero	0:4dd021344a6b	2911	return 0;
oprospero	0:4dd021344a6b	2912	}
oprospero	0:4dd021344a6b	2913
oprospero	0:4dd021344a6b	2914	/**
oprospero	0:4dd021344a6b	2915	* @brief Load and verify DMP image.
oprospero	0:4dd021344a6b	2916	* @param[in] length Length of DMP image.
oprospero	0:4dd021344a6b	2917	* @param[in] firmware DMP code.
oprospero	0:4dd021344a6b	2918	* @param[in] start_addr Starting address of DMP code memory.
oprospero	0:4dd021344a6b	2919	* @param[in] sample_rate Fixed sampling rate used when DMP is enabled.
oprospero	0:4dd021344a6b	2920	* @return 0 if successful.
oprospero	0:4dd021344a6b	2921	*/
oprospero	0:4dd021344a6b	2922	int InvMpu::mpu_load_firmware(unsigned short length, const unsigned char *firmware,
oprospero	0:4dd021344a6b	2923	unsigned short start_addr, unsigned short sample_rate)
oprospero	0:4dd021344a6b	2924	{
oprospero	0:4dd021344a6b	2925	unsigned short ii;
oprospero	0:4dd021344a6b	2926	unsigned short this_write;
oprospero	0:4dd021344a6b	2927	/* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
oprospero	0:4dd021344a6b	2928	#define LOAD_CHUNK (16)
oprospero	0:4dd021344a6b	2929	unsigned char cur[LOAD_CHUNK], tmp[2];
oprospero	0:4dd021344a6b	2930
oprospero	0:4dd021344a6b	2931	if (st.chip_cfg.dmp_loaded)
oprospero	0:4dd021344a6b	2932	/* DMP should only be loaded once. */
oprospero	0:4dd021344a6b	2933	return -1;
oprospero	0:4dd021344a6b	2934
oprospero	0:4dd021344a6b	2935	if (!firmware)
oprospero	0:4dd021344a6b	2936	return -1;
oprospero	0:4dd021344a6b	2937	for (ii = 0; ii < length; ii += this_write) {
oprospero	0:4dd021344a6b	2938	this_write = min(LOAD_CHUNK, length - ii);
oprospero	0:4dd021344a6b	2939	if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
oprospero	0:4dd021344a6b	2940	return -1;
oprospero	0:4dd021344a6b	2941	if (mpu_read_mem(ii, this_write, cur))
oprospero	0:4dd021344a6b	2942	return -1;
oprospero	0:4dd021344a6b	2943	if (memcmp(firmware+ii, cur, this_write))
oprospero	0:4dd021344a6b	2944	return -2;
oprospero	0:4dd021344a6b	2945	}
oprospero	0:4dd021344a6b	2946
oprospero	0:4dd021344a6b	2947	/* Set program start address. */
oprospero	0:4dd021344a6b	2948	tmp[0] = start_addr >> 8;
oprospero	0:4dd021344a6b	2949	tmp[1] = start_addr & 0xFF;
oprospero	0:4dd021344a6b	2950	if (i2Cdev.writeBytes(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
oprospero	0:4dd021344a6b	2951	return -1;
oprospero	0:4dd021344a6b	2952
oprospero	0:4dd021344a6b	2953	st.chip_cfg.dmp_loaded = 1;
oprospero	0:4dd021344a6b	2954	st.chip_cfg.dmp_sample_rate = sample_rate;
oprospero	0:4dd021344a6b	2955	return 0;
oprospero	0:4dd021344a6b	2956	}
oprospero	0:4dd021344a6b	2957
oprospero	0:4dd021344a6b	2958	/**
oprospero	0:4dd021344a6b	2959	* @brief Enable/disable DMP support.
oprospero	0:4dd021344a6b	2960	* @param[in] enable 1 to turn on the DMP.
oprospero	0:4dd021344a6b	2961	* @return 0 if successful.
oprospero	0:4dd021344a6b	2962	*/
oprospero	0:4dd021344a6b	2963	int InvMpu::mpu_set_dmp_state(unsigned char enable)
oprospero	0:4dd021344a6b	2964	{
oprospero	0:4dd021344a6b	2965	unsigned char tmp;
oprospero	0:4dd021344a6b	2966	if (st.chip_cfg.dmp_on == enable)
oprospero	0:4dd021344a6b	2967	return 0;
oprospero	0:4dd021344a6b	2968
oprospero	0:4dd021344a6b	2969	if (enable) {
oprospero	0:4dd021344a6b	2970	if (!st.chip_cfg.dmp_loaded)
oprospero	0:4dd021344a6b	2971	return -1;
oprospero	0:4dd021344a6b	2972	/* Disable data ready interrupt. */
oprospero	0:4dd021344a6b	2973	set_int_enable(0);
oprospero	0:4dd021344a6b	2974	/* Disable bypass mode. */
oprospero	0:4dd021344a6b	2975	mpu_set_bypass(0);
oprospero	0:4dd021344a6b	2976	/* Keep constant sample rate, FIFO rate controlled by DMP. */
oprospero	0:4dd021344a6b	2977	mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
oprospero	0:4dd021344a6b	2978	/* Remove FIFO elements. */
oprospero	0:4dd021344a6b	2979	tmp = 0;
oprospero	0:4dd021344a6b	2980	i2Cdev.writeBytes(st.hw->addr, 0x23, 1, &tmp);
oprospero	0:4dd021344a6b	2981	st.chip_cfg.dmp_on = 1;
oprospero	0:4dd021344a6b	2982	/* Enable DMP interrupt. */
oprospero	0:4dd021344a6b	2983	set_int_enable(1);
oprospero	0:4dd021344a6b	2984	mpu_reset_fifo();
oprospero	0:4dd021344a6b	2985	} else {
oprospero	0:4dd021344a6b	2986	/* Disable DMP interrupt. */
oprospero	0:4dd021344a6b	2987	set_int_enable(0);
oprospero	0:4dd021344a6b	2988	/* Restore FIFO settings. */
oprospero	0:4dd021344a6b	2989	tmp = st.chip_cfg.fifo_enable;
oprospero	0:4dd021344a6b	2990	i2Cdev.writeBytes(st.hw->addr, 0x23, 1, &tmp);
oprospero	0:4dd021344a6b	2991	st.chip_cfg.dmp_on = 0;
oprospero	0:4dd021344a6b	2992	mpu_reset_fifo();
oprospero	0:4dd021344a6b	2993	}
oprospero	0:4dd021344a6b	2994	return 0;
oprospero	0:4dd021344a6b	2995	}
oprospero	0:4dd021344a6b	2996
oprospero	0:4dd021344a6b	2997	/**
oprospero	0:4dd021344a6b	2998	* @brief Get DMP state.
oprospero	0:4dd021344a6b	2999	* @param[out] enabled 1 if enabled.
oprospero	0:4dd021344a6b	3000	* @return 0 if successful.
oprospero	0:4dd021344a6b	3001	*/
oprospero	0:4dd021344a6b	3002	int InvMpu::mpu_get_dmp_state(unsigned char *enabled)
oprospero	0:4dd021344a6b	3003	{
oprospero	0:4dd021344a6b	3004	enabled[0] = st.chip_cfg.dmp_on;
oprospero	0:4dd021344a6b	3005	return 0;
oprospero	0:4dd021344a6b	3006	}
oprospero	0:4dd021344a6b	3007
oprospero	0:4dd021344a6b	3008	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	3009	/* This initialization is similar to the one in ak8975.c. */
oprospero	0:4dd021344a6b	3010	//static int setup_compass(void)
oprospero	0:4dd021344a6b	3011	int InvMpu::setup_compass(void)
oprospero	0:4dd021344a6b	3012	{
oprospero	0:4dd021344a6b	3013	unsigned char data[4], akm_addr;
oprospero	0:4dd021344a6b	3014
oprospero	0:4dd021344a6b	3015	mpu_set_bypass(1);
oprospero	0:4dd021344a6b	3016
oprospero	0:4dd021344a6b	3017	/* Find compass. Possible addresses range from 0x0C to 0x0F. */
oprospero	0:4dd021344a6b	3018	for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
oprospero	0:4dd021344a6b	3019	int result;
oprospero	0:4dd021344a6b	3020	result = i2Cdev.readBytes(akm_addr, AKM_REG_WHOAMI, 1, data);
oprospero	0:4dd021344a6b	3021	if (!result && (data[0] == AKM_WHOAMI))
oprospero	0:4dd021344a6b	3022	break;
oprospero	0:4dd021344a6b	3023	}
oprospero	0:4dd021344a6b	3024
oprospero	0:4dd021344a6b	3025	if (akm_addr > 0x0F) {
oprospero	0:4dd021344a6b	3026	/* TODO: Handle this case in all compass-related functions. */
oprospero	0:4dd021344a6b	3027	log_e("Compass not found.\n");
oprospero	0:4dd021344a6b	3028	return -1;
oprospero	0:4dd021344a6b	3029	}
oprospero	0:4dd021344a6b	3030
oprospero	0:4dd021344a6b	3031	st.chip_cfg.compass_addr = akm_addr;
oprospero	0:4dd021344a6b	3032
oprospero	0:4dd021344a6b	3033	data[0] = AKM_POWER_DOWN;
oprospero	0:4dd021344a6b	3034	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
oprospero	0:4dd021344a6b	3035	return -1;
oprospero	0:4dd021344a6b	3036	wait_ms(1);
oprospero	0:4dd021344a6b	3037
oprospero	0:4dd021344a6b	3038	data[0] = AKM_FUSE_ROM_ACCESS;
oprospero	0:4dd021344a6b	3039	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
oprospero	0:4dd021344a6b	3040	return -1;
oprospero	0:4dd021344a6b	3041	wait_ms(1);
oprospero	0:4dd021344a6b	3042
oprospero	0:4dd021344a6b	3043	/* Get sensitivity adjustment data from fuse ROM. */
oprospero	0:4dd021344a6b	3044	if (i2Cdev.readBytes(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
oprospero	0:4dd021344a6b	3045	return -1;
oprospero	0:4dd021344a6b	3046	st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
oprospero	0:4dd021344a6b	3047	st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
oprospero	0:4dd021344a6b	3048	st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
oprospero	0:4dd021344a6b	3049
oprospero	0:4dd021344a6b	3050	data[0] = AKM_POWER_DOWN;
oprospero	0:4dd021344a6b	3051	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
oprospero	0:4dd021344a6b	3052	return -1;
oprospero	0:4dd021344a6b	3053	wait_ms(1);
oprospero	0:4dd021344a6b	3054
oprospero	0:4dd021344a6b	3055	mpu_set_bypass(0);
oprospero	0:4dd021344a6b	3056
oprospero	0:4dd021344a6b	3057	/* Set up master mode, master clock, and ES bit. */
oprospero	0:4dd021344a6b	3058	data[0] = 0x40;
oprospero	0:4dd021344a6b	3059	if (i2Cdev.writeBytes(st.hw->addr, st.reg->i2c_mst, 1, data))
oprospero	0:4dd021344a6b	3060	return -1;
oprospero	0:4dd021344a6b	3061
oprospero	0:4dd021344a6b	3062	/* Slave 0 reads from AKM data registers. */
oprospero	0:4dd021344a6b	3063	data[0] = BIT_I2C_READ \| st.chip_cfg.compass_addr;
oprospero	0:4dd021344a6b	3064	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s0_addr, 1, data))
oprospero	0:4dd021344a6b	3065	return -1;
oprospero	0:4dd021344a6b	3066
oprospero	0:4dd021344a6b	3067	/* Compass reads start at this register. */
oprospero	0:4dd021344a6b	3068	data[0] = AKM_REG_ST1;
oprospero	0:4dd021344a6b	3069	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s0_reg, 1, data))
oprospero	0:4dd021344a6b	3070	return -1;
oprospero	0:4dd021344a6b	3071
oprospero	0:4dd021344a6b	3072	/* Enable slave 0, 8-byte reads. */
oprospero	0:4dd021344a6b	3073	data[0] = BIT_SLAVE_EN \| 8;
oprospero	0:4dd021344a6b	3074	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s0_ctrl, 1, data))
oprospero	0:4dd021344a6b	3075	return -1;
oprospero	0:4dd021344a6b	3076
oprospero	0:4dd021344a6b	3077	/* Slave 1 changes AKM measurement mode. */
oprospero	0:4dd021344a6b	3078	data[0] = st.chip_cfg.compass_addr;
oprospero	0:4dd021344a6b	3079	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s1_addr, 1, data))
oprospero	0:4dd021344a6b	3080	return -1;
oprospero	0:4dd021344a6b	3081
oprospero	0:4dd021344a6b	3082	/* AKM measurement mode register. */
oprospero	0:4dd021344a6b	3083	data[0] = AKM_REG_CNTL;
oprospero	0:4dd021344a6b	3084	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s1_reg, 1, data))
oprospero	0:4dd021344a6b	3085	return -1;
oprospero	0:4dd021344a6b	3086
oprospero	0:4dd021344a6b	3087	/* Enable slave 1, 1-byte writes. */
oprospero	0:4dd021344a6b	3088	data[0] = BIT_SLAVE_EN \| 1;
oprospero	0:4dd021344a6b	3089	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s1_ctrl, 1, data))
oprospero	0:4dd021344a6b	3090	return -1;
oprospero	0:4dd021344a6b	3091
oprospero	0:4dd021344a6b	3092	/* Set slave 1 data. */
oprospero	0:4dd021344a6b	3093	data[0] = AKM_SINGLE_MEASUREMENT;
oprospero	0:4dd021344a6b	3094	if (i2Cdev.writeBytes(st.hw->addr, st.reg->s1_do, 1, data))
oprospero	0:4dd021344a6b	3095	return -1;
oprospero	0:4dd021344a6b	3096
oprospero	0:4dd021344a6b	3097	/* Trigger slave 0 and slave 1 actions at each sample. */
oprospero	0:4dd021344a6b	3098	data[0] = 0x03;
oprospero	0:4dd021344a6b	3099	if (i2Cdev.writeBytes(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
oprospero	0:4dd021344a6b	3100	return -1;
oprospero	0:4dd021344a6b	3101
oprospero	0:4dd021344a6b	3102	#ifdef MPU9150
oprospero	0:4dd021344a6b	3103	/* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
oprospero	0:4dd021344a6b	3104	data[0] = BIT_I2C_MST_VDDIO;
oprospero	0:4dd021344a6b	3105	if (i2Cdev.writeBytes(st.hw->addr, st.reg->yg_offs_tc, 1, data))
oprospero	0:4dd021344a6b	3106	return -1;
oprospero	0:4dd021344a6b	3107	#endif
oprospero	0:4dd021344a6b	3108
oprospero	0:4dd021344a6b	3109	return 0;
oprospero	0:4dd021344a6b	3110	}
oprospero	0:4dd021344a6b	3111	#endif
oprospero	0:4dd021344a6b	3112
oprospero	0:4dd021344a6b	3113	/**
oprospero	0:4dd021344a6b	3114	* @brief Read raw compass data.
oprospero	0:4dd021344a6b	3115	* @param[out] data Raw data in hardware units.
oprospero	0:4dd021344a6b	3116	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
oprospero	0:4dd021344a6b	3117	* @return 0 if successful.
oprospero	0:4dd021344a6b	3118	*/
oprospero	0:4dd021344a6b	3119	int InvMpu::mpu_get_compass_reg(short data, unsigned long timestamp)
oprospero	0:4dd021344a6b	3120	{
oprospero	0:4dd021344a6b	3121	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	3122	unsigned char tmp[9];
oprospero	0:4dd021344a6b	3123
oprospero	0:4dd021344a6b	3124	if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
oprospero	0:4dd021344a6b	3125	return -1;
oprospero	0:4dd021344a6b	3126
oprospero	0:4dd021344a6b	3127	#ifdef AK89xx_BYPASS
oprospero	0:4dd021344a6b	3128	if (i2Cdev.readBytes(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
oprospero	0:4dd021344a6b	3129	return -1;
oprospero	0:4dd021344a6b	3130	tmp[8] = AKM_SINGLE_MEASUREMENT;
oprospero	0:4dd021344a6b	3131	if (i2Cdev.writeBytes(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
oprospero	0:4dd021344a6b	3132	return -1;
oprospero	0:4dd021344a6b	3133	#else
oprospero	0:4dd021344a6b	3134	if (i2Cdev.readBytes(st.hw->addr, st.reg->raw_compass, 8, tmp))
oprospero	0:4dd021344a6b	3135	return -1;
oprospero	0:4dd021344a6b	3136	#endif
oprospero	0:4dd021344a6b	3137
oprospero	0:4dd021344a6b	3138	#if defined AK8975_SECONDARY
oprospero	0:4dd021344a6b	3139	/* AK8975 doesn't have the overrun error bit. */
oprospero	0:4dd021344a6b	3140	if (!(tmp[0] & AKM_DATA_READY))
oprospero	0:4dd021344a6b	3141	return -2;
oprospero	0:4dd021344a6b	3142	if ((tmp[7] & AKM_OVERFLOW) \|\| (tmp[7] & AKM_DATA_ERROR))
oprospero	0:4dd021344a6b	3143	return -3;
oprospero	0:4dd021344a6b	3144	#elif defined AK8963_SECONDARY
oprospero	0:4dd021344a6b	3145	/* AK8963 doesn't have the data read error bit. */
oprospero	0:4dd021344a6b	3146	if (!(tmp[0] & AKM_DATA_READY) \|\| (tmp[0] & AKM_DATA_OVERRUN))
oprospero	0:4dd021344a6b	3147	return -2;
oprospero	0:4dd021344a6b	3148	if (tmp[7] & AKM_OVERFLOW)
oprospero	0:4dd021344a6b	3149	return -3;
oprospero	0:4dd021344a6b	3150	#endif
oprospero	0:4dd021344a6b	3151	data[0] = (tmp[2] << 8) \| tmp[1];
oprospero	0:4dd021344a6b	3152	data[1] = (tmp[4] << 8) \| tmp[3];
oprospero	0:4dd021344a6b	3153	data[2] = (tmp[6] << 8) \| tmp[5];
oprospero	0:4dd021344a6b	3154
oprospero	0:4dd021344a6b	3155	data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
oprospero	0:4dd021344a6b	3156	data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
oprospero	0:4dd021344a6b	3157	data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
oprospero	0:4dd021344a6b	3158
oprospero	0:4dd021344a6b	3159	if (timestamp)
oprospero	0:4dd021344a6b	3160	get_ms(timestamp);
oprospero	0:4dd021344a6b	3161	return 0;
oprospero	0:4dd021344a6b	3162	#else
oprospero	0:4dd021344a6b	3163	return -1;
oprospero	0:4dd021344a6b	3164	#endif
oprospero	0:4dd021344a6b	3165	}
oprospero	0:4dd021344a6b	3166
oprospero	0:4dd021344a6b	3167	/**
oprospero	0:4dd021344a6b	3168	* @brief Get the compass full-scale range.
oprospero	0:4dd021344a6b	3169	* @param[out] fsr Current full-scale range.
oprospero	0:4dd021344a6b	3170	* @return 0 if successful.
oprospero	0:4dd021344a6b	3171	*/
oprospero	0:4dd021344a6b	3172	int InvMpu::mpu_get_compass_fsr(unsigned short *fsr)
oprospero	0:4dd021344a6b	3173	{
oprospero	0:4dd021344a6b	3174	#ifdef AK89xx_SECONDARY
oprospero	0:4dd021344a6b	3175	fsr[0] = st.hw->compass_fsr;
oprospero	0:4dd021344a6b	3176	return 0;
oprospero	0:4dd021344a6b	3177	#else
oprospero	0:4dd021344a6b	3178	return -1;
oprospero	0:4dd021344a6b	3179	#endif
oprospero	0:4dd021344a6b	3180	}
oprospero	0:4dd021344a6b	3181
oprospero	0:4dd021344a6b	3182	/**
oprospero	0:4dd021344a6b	3183	* @brief Enters LP accel motion interrupt mode.
oprospero	0:4dd021344a6b	3184	* The behaviour of this feature is very different between the MPU6050 and the
oprospero	0:4dd021344a6b	3185	* MPU6500. Each chip's version of this feature is explained below.
oprospero	0:4dd021344a6b	3186	*
oprospero	0:4dd021344a6b	3187	* \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
oprospero	0:4dd021344a6b	3188	* increments.
oprospero	0:4dd021344a6b	3189	*
oprospero	0:4dd021344a6b	3190	* \n Low-power accel mode supports the following frequencies:
oprospero	0:4dd021344a6b	3191	* \n 1.25Hz, 5Hz, 20Hz, 40Hz
oprospero	0:4dd021344a6b	3192	*
oprospero	0:4dd021344a6b	3193	* \n MPU6500:
oprospero	0:4dd021344a6b	3194	* \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
oprospero	0:4dd021344a6b	3195	* sample. The hardware monitors the accel data and detects any large change
oprospero	0:4dd021344a6b	3196	* over a short period of time.
oprospero	0:4dd021344a6b	3197	*
oprospero	0:4dd021344a6b	3198	* \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
oprospero	0:4dd021344a6b	3199	* increments.
oprospero	0:4dd021344a6b	3200	*
oprospero	0:4dd021344a6b	3201	* \n MPU6500 Low-power accel mode supports the following frequencies:
oprospero	0:4dd021344a6b	3202	* \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
oprospero	0:4dd021344a6b	3203	*
oprospero	0:4dd021344a6b	3204	* \n\n NOTES:
oprospero	0:4dd021344a6b	3205	* \n The driver will round down @e thresh to the nearest supported value if
oprospero	0:4dd021344a6b	3206	* an unsupported threshold is selected.
oprospero	0:4dd021344a6b	3207	* \n To select a fractional wake-up frequency, round down the value passed to
oprospero	0:4dd021344a6b	3208	* @e lpa_freq.
oprospero	0:4dd021344a6b	3209	* \n The MPU6500 does not support a delay parameter. If this function is used
oprospero	0:4dd021344a6b	3210	* for the MPU6500, the value passed to @e time will be ignored.
oprospero	0:4dd021344a6b	3211	* \n To disable this mode, set @e lpa_freq to zero. The driver will restore
oprospero	0:4dd021344a6b	3212	* the previous configuration.
oprospero	0:4dd021344a6b	3213	*
oprospero	0:4dd021344a6b	3214	* @param[in] thresh Motion threshold in mg.
oprospero	0:4dd021344a6b	3215	* @param[in] time Duration in milliseconds that the accel data must
oprospero	0:4dd021344a6b	3216	* exceed @e thresh before motion is reported.
oprospero	0:4dd021344a6b	3217	* @param[in] lpa_freq Minimum sampling rate, or zero to disable.
oprospero	0:4dd021344a6b	3218	* @return 0 if successful.
oprospero	0:4dd021344a6b	3219	*/
oprospero	0:4dd021344a6b	3220	int InvMpu::mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
oprospero	0:4dd021344a6b	3221	unsigned char lpa_freq)
oprospero	0:4dd021344a6b	3222	{
oprospero	0:4dd021344a6b	3223
oprospero	0:4dd021344a6b	3224	#if defined MPU6500
oprospero	0:4dd021344a6b	3225	unsigned char data[3];
oprospero	0:4dd021344a6b	3226	#endif
oprospero	0:4dd021344a6b	3227	if (lpa_freq) {
oprospero	0:4dd021344a6b	3228	#if defined MPU6500
oprospero	0:4dd021344a6b	3229	unsigned char thresh_hw;
oprospero	0:4dd021344a6b	3230
oprospero	0:4dd021344a6b	3231	/* 1LSb = 4mg. */
oprospero	0:4dd021344a6b	3232	if (thresh > 1020)
oprospero	0:4dd021344a6b	3233	thresh_hw = 255;
oprospero	0:4dd021344a6b	3234	else if (thresh < 4)
oprospero	0:4dd021344a6b	3235	thresh_hw = 1;
oprospero	0:4dd021344a6b	3236	else
oprospero	0:4dd021344a6b	3237	thresh_hw = thresh >> 2;
oprospero	0:4dd021344a6b	3238	#endif
oprospero	0:4dd021344a6b	3239
oprospero	0:4dd021344a6b	3240	if (!time)
oprospero	0:4dd021344a6b	3241	/* Minimum duration must be 1ms. */
oprospero	0:4dd021344a6b	3242	time = 1;
oprospero	0:4dd021344a6b	3243
oprospero	0:4dd021344a6b	3244	#if defined MPU6500
oprospero	0:4dd021344a6b	3245	if (lpa_freq > 640)
oprospero	0:4dd021344a6b	3246	/* At this point, the chip has not been re-configured, so the
oprospero	0:4dd021344a6b	3247	* function can safely exit.
oprospero	0:4dd021344a6b	3248	*/
oprospero	0:4dd021344a6b	3249	return -1;
oprospero	0:4dd021344a6b	3250	#endif
oprospero	0:4dd021344a6b	3251
oprospero	0:4dd021344a6b	3252	if (!st.chip_cfg.int_motion_only) {
oprospero	0:4dd021344a6b	3253	/* Store current settings for later. */
oprospero	0:4dd021344a6b	3254	if (st.chip_cfg.dmp_on) {
oprospero	0:4dd021344a6b	3255	mpu_set_dmp_state(0);
oprospero	0:4dd021344a6b	3256	st.chip_cfg.cache.dmp_on = 1;
oprospero	0:4dd021344a6b	3257	} else
oprospero	0:4dd021344a6b	3258	st.chip_cfg.cache.dmp_on = 0;
oprospero	0:4dd021344a6b	3259	mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
oprospero	0:4dd021344a6b	3260	mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
oprospero	0:4dd021344a6b	3261	mpu_get_lpf(&st.chip_cfg.cache.lpf);
oprospero	0:4dd021344a6b	3262	mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
oprospero	0:4dd021344a6b	3263	st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
oprospero	0:4dd021344a6b	3264	mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
oprospero	0:4dd021344a6b	3265	}
oprospero	0:4dd021344a6b	3266
oprospero	0:4dd021344a6b	3267	#if defined MPU6500
oprospero	0:4dd021344a6b	3268	/* Disable hardware interrupts. */
oprospero	0:4dd021344a6b	3269	set_int_enable(0);
oprospero	0:4dd021344a6b	3270
oprospero	0:4dd021344a6b	3271	/* Enter full-power accel-only mode, no FIFO/DMP. */
oprospero	0:4dd021344a6b	3272	data[0] = 0;
oprospero	0:4dd021344a6b	3273	data[1] = 0;
oprospero	0:4dd021344a6b	3274	data[2] = BIT_STBY_XYZG;
oprospero	0:4dd021344a6b	3275	if (i2Cdev.writeBytes(st.hw->addr, st.reg->user_ctrl, 3, data))
oprospero	0:4dd021344a6b	3276	goto lp_int_restore;
oprospero	0:4dd021344a6b	3277
oprospero	0:4dd021344a6b	3278	/* Set motion threshold. */
oprospero	0:4dd021344a6b	3279	data[0] = thresh_hw;
oprospero	0:4dd021344a6b	3280	if (i2Cdev.writeBytes(st.hw->addr, st.reg->motion_thr, 1, data))
oprospero	0:4dd021344a6b	3281	goto lp_int_restore;
oprospero	0:4dd021344a6b	3282
oprospero	0:4dd021344a6b	3283	/* Set wake frequency. */
oprospero	0:4dd021344a6b	3284	if (lpa_freq == 1)
oprospero	0:4dd021344a6b	3285	data[0] = INV_LPA_1_25HZ;
oprospero	0:4dd021344a6b	3286	else if (lpa_freq == 2)
oprospero	0:4dd021344a6b	3287	data[0] = INV_LPA_2_5HZ;
oprospero	0:4dd021344a6b	3288	else if (lpa_freq <= 5)
oprospero	0:4dd021344a6b	3289	data[0] = INV_LPA_5HZ;
oprospero	0:4dd021344a6b	3290	else if (lpa_freq <= 10)
oprospero	0:4dd021344a6b	3291	data[0] = INV_LPA_10HZ;
oprospero	0:4dd021344a6b	3292	else if (lpa_freq <= 20)
oprospero	0:4dd021344a6b	3293	data[0] = INV_LPA_20HZ;
oprospero	0:4dd021344a6b	3294	else if (lpa_freq <= 40)
oprospero	0:4dd021344a6b	3295	data[0] = INV_LPA_40HZ;
oprospero	0:4dd021344a6b	3296	else if (lpa_freq <= 80)
oprospero	0:4dd021344a6b	3297	data[0] = INV_LPA_80HZ;
oprospero	0:4dd021344a6b	3298	else if (lpa_freq <= 160)
oprospero	0:4dd021344a6b	3299	data[0] = INV_LPA_160HZ;
oprospero	0:4dd021344a6b	3300	else if (lpa_freq <= 320)
oprospero	0:4dd021344a6b	3301	data[0] = INV_LPA_320HZ;
oprospero	0:4dd021344a6b	3302	else
oprospero	0:4dd021344a6b	3303	data[0] = INV_LPA_640HZ;
oprospero	0:4dd021344a6b	3304	if (i2Cdev.writeBytes(st.hw->addr, st.reg->lp_accel_odr, 1, data))
oprospero	0:4dd021344a6b	3305	goto lp_int_restore;
oprospero	0:4dd021344a6b	3306
oprospero	0:4dd021344a6b	3307	/* Enable motion interrupt (MPU6500 version). */
oprospero	0:4dd021344a6b	3308	data[0] = BITS_WOM_EN;
oprospero	0:4dd021344a6b	3309	if (i2Cdev.writeBytes(st.hw->addr, st.reg->accel_intel, 1, data))
oprospero	0:4dd021344a6b	3310	goto lp_int_restore;
oprospero	0:4dd021344a6b	3311
oprospero	0:4dd021344a6b	3312	/* Enable cycle mode. */
oprospero	0:4dd021344a6b	3313	data[0] = BIT_LPA_CYCLE;
oprospero	0:4dd021344a6b	3314	if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
oprospero	0:4dd021344a6b	3315	goto lp_int_restore;
oprospero	0:4dd021344a6b	3316
oprospero	0:4dd021344a6b	3317	/* Enable interrupt. */
oprospero	0:4dd021344a6b	3318	data[0] = BIT_MOT_INT_EN;
oprospero	0:4dd021344a6b	3319	if (i2Cdev.writeBytes(st.hw->addr, st.reg->int_enable, 1, data))
oprospero	0:4dd021344a6b	3320	goto lp_int_restore;
oprospero	0:4dd021344a6b	3321
oprospero	0:4dd021344a6b	3322	st.chip_cfg.int_motion_only = 1;
oprospero	0:4dd021344a6b	3323	return 0;
oprospero	0:4dd021344a6b	3324	#endif
oprospero	0:4dd021344a6b	3325	} else {
oprospero	0:4dd021344a6b	3326	/* Don't "restore" the previous state if no state has been saved. */
oprospero	0:4dd021344a6b	3327	int ii;
oprospero	0:4dd021344a6b	3328	char cache_ptr = (char)&st.chip_cfg.cache;
oprospero	0:4dd021344a6b	3329	for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
oprospero	0:4dd021344a6b	3330	if (cache_ptr[ii] != 0)
oprospero	0:4dd021344a6b	3331	goto lp_int_restore;
oprospero	0:4dd021344a6b	3332	}
oprospero	0:4dd021344a6b	3333	/* If we reach this point, motion interrupt mode hasn't been used yet. */
oprospero	0:4dd021344a6b	3334	return -1;
oprospero	0:4dd021344a6b	3335	}
oprospero	0:4dd021344a6b	3336	lp_int_restore:
oprospero	0:4dd021344a6b	3337	/* Set to invalid values to ensure no I2C writes are skipped. */
oprospero	0:4dd021344a6b	3338	st.chip_cfg.gyro_fsr = 0xFF;
oprospero	0:4dd021344a6b	3339	st.chip_cfg.accel_fsr = 0xFF;
oprospero	0:4dd021344a6b	3340	st.chip_cfg.lpf = 0xFF;
oprospero	0:4dd021344a6b	3341	st.chip_cfg.sample_rate = 0xFFFF;
oprospero	0:4dd021344a6b	3342	st.chip_cfg.sensors = 0xFF;
oprospero	0:4dd021344a6b	3343	st.chip_cfg.fifo_enable = 0xFF;
oprospero	0:4dd021344a6b	3344	st.chip_cfg.clk_src = INV_CLK_PLL;
oprospero	0:4dd021344a6b	3345	mpu_set_sensors(st.chip_cfg.cache.sensors_on);
oprospero	0:4dd021344a6b	3346	mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
oprospero	0:4dd021344a6b	3347	mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
oprospero	0:4dd021344a6b	3348	mpu_set_lpf(st.chip_cfg.cache.lpf);
oprospero	0:4dd021344a6b	3349	mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
oprospero	0:4dd021344a6b	3350	mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
oprospero	0:4dd021344a6b	3351
oprospero	0:4dd021344a6b	3352	if (st.chip_cfg.cache.dmp_on)
oprospero	0:4dd021344a6b	3353	mpu_set_dmp_state(1);
oprospero	0:4dd021344a6b	3354
oprospero	0:4dd021344a6b	3355	#ifdef MPU6500
oprospero	0:4dd021344a6b	3356	/* Disable motion interrupt (MPU6500 version). */
oprospero	0:4dd021344a6b	3357	data[0] = 0;
oprospero	0:4dd021344a6b	3358	if (i2Cdev.writeBytes(st.hw->addr, st.reg->accel_intel, 1, data))
oprospero	0:4dd021344a6b	3359	goto lp_int_restore;
oprospero	0:4dd021344a6b	3360	#endif
oprospero	0:4dd021344a6b	3361
oprospero	0:4dd021344a6b	3362	st.chip_cfg.int_motion_only = 0;
oprospero	0:4dd021344a6b	3363	return 0;
oprospero	0:4dd021344a6b	3364	}
oprospero	0:4dd021344a6b	3365
oprospero	0:4dd021344a6b	3366	/**
oprospero	0:4dd021344a6b	3367	* @}
oprospero	0:4dd021344a6b	3368	*/

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	03 Jun 2014
Imports:	41
Forks:	0
Commits:	5
Dependents:	1
Dependencies:	0
Followers:	2

inv_mpu.cpp@1:dbbd3dcd61c9, 2014-06-07 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning