MPL6_1 - Motion Processing Library adapted to mbed

Users » janekm » Code » MPL6_1

Motion Processing Library adapted to mbed

Dependents: openwear-lifelogger-example

inv_mpu.c@1:6221b0518f94, 2014-09-16 (annotated)

Committer:: janekm
Date:: Tue Sep 16 22:42:12 2014 +0000
Revision:: 1:6221b0518f94
Parent:: 0:61a7cadab29e

I forgot

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	16 Sep 2014
Imports:	1
Forks:	0
Commits:	2
Dependents:	1
Dependencies:	0
Followers:	1

This repository is Public (Unlisted).

inv_mpu.c@1:6221b0518f94, 2014-09-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning