MotionDriver_6_1 - It is modified accordingly to work with sparkfun …

Users » ozzdemir » Code » MotionDriver_6_1

It is modified accordingly to work with sparkfun dmp library under mbed platform

Dependents: MPU9250-dmp-bluepill MPU9250-dmp

Fork of MotionDriver_6_1 by Prosper Van

inv_mpu.c@1:a6c3f8680fe0, 2017-08-07 (annotated)

Committer:: mbedoguz
Date:: Mon Aug 07 13:49:51 2017 +0000
Revision:: 1:a6c3f8680fe0
Parent:: 0:5fa30cf392c3

It is modified to work wirh mbed platform. Now compiles but fifo is not available (Using sparkfun dmp library)

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	07 Aug 2017
Imports:	15
Forks:	0
Commits:	7
Dependents:	2
Dependencies:	0
Followers:	2

The code in this repository is Apache licensed.

inv_mpu.c@1:a6c3f8680fe0, 2017-08-07 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning