MotionDriverv512 - InvenSense Motion Driver v5.1.2 ported

Users » oprospero » Code » MotionDriverv512

InvenSense Motion Driver v5.1.2 ported

inv_mpu.cpp@4:2cb380415dc7, 2014-11-02 (annotated)

Committer:: oprospero
Date:: Sun Nov 02 19:17:45 2014 +0000
Revision:: 4:2cb380415dc7
Parent:: 3:8ac73f9099ab

Added reset function

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

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@4:2cb380415dc7, 2014-11-02 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning