MotionDriverv512 - InvenSense Motion Driver v5.1.2 ported

Users » oprospero » Code » MotionDriverv512

InvenSense Motion Driver v5.1.2 ported

inv_mpu.cpp@3:8ac73f9099ab, 2014-10-15 (annotated)

Committer:: oprospero
Date:: Wed Oct 15 04:59:22 2014 +0000
Revision:: 3:8ac73f9099ab
Parent:: 1:dbbd3dcd61c9
Child:: 4:2cb380415dc7

added reset

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

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@3:8ac73f9099ab, 2014-10-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning