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@0:61a7cadab29e, 2014-09-08 (annotated)

Committer:: janekm
Date:: Mon Sep 08 23:16:52 2014 +0000
Revision:: 0:61a7cadab29e
Child:: 1:6221b0518f94

mbed porting

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

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@0:61a7cadab29e, 2014-09-08 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning