eMPL_MPU - motion library for mpu6050, mpu9250 and etc, supp…

Seeed » Code » eMPL_MPU

motion library for mpu6050, mpu9250 and etc, supports i2c and spi

inv_mpu.c@0:814475fdc553, 2016-07-05 (annotated)

Committer:: yihui
Date:: Tue Jul 05 07:19:59 2016 +0000
Revision:: 0:814475fdc553

initial

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	05 Jul 2016
Imports:	71
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	58

inv_mpu.c@0:814475fdc553, 2016-07-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning