MotionDriver_6_1 - Embedded MotionDriver 6.1 Sourced from InvenSense

Users » oprospero » Code » MotionDriver_6_1

Embedded MotionDriver 6.1 Sourced from InvenSense

inv_mpu.c@0:5fa30cf392c3, 2014-08-24 (annotated)

Committer:: oprospero
Date:: Sun Aug 24 00:52:07 2014 +0000
Revision:: 0:5fa30cf392c3

Initial commit, copied from Motion Driver 6.1 no edits

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	24 Aug 2014
Imports:	113
Forks:	1
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	13

inv_mpu.c@0:5fa30cf392c3, 2014-08-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning