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@6:7469a85601f1, 2017-08-14 (annotated)

Committer:: mbedoguz
Date:: Mon Aug 14 07:36:07 2017 +0000
Revision:: 6:7469a85601f1
Parent:: 1:a6c3f8680fe0

get_ms now returns the counter.

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@6:7469a85601f1, 2017-08-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning