ICM_20948 - Modified Arduino library for ICM_20948 IMU for Nu…

Users » saloutos » Code » ICM_20948

Andrew SaLoutos / ICM_20948

Modified Arduino library for ICM_20948 IMU for Nucleo boards

util/ICM_20948_C.c@0:894b603d32ee, 2022-01-31 (annotated)

Committer:: saloutos
Date:: Mon Jan 31 03:25:31 2022 +0000
Revision:: 0:894b603d32ee

modified ICM_20948 Arduino library for Nucleo boards

Who changed what in which revision?

User	Revision	Line number	New contents of line
saloutos	0:894b603d32ee	1	#include "ICM_20948_C.h"
saloutos	0:894b603d32ee	2	#include "ICM_20948_REGISTERS.h"
saloutos	0:894b603d32ee	3	#include "AK09916_REGISTERS.h"
saloutos	0:894b603d32ee	4
saloutos	0:894b603d32ee	5	/*
saloutos	0:894b603d32ee	6	* Icm20948 device require a DMP image to be loaded on init
saloutos	0:894b603d32ee	7	* Provide such images by mean of a byte array
saloutos	0:894b603d32ee	8	*/
saloutos	0:894b603d32ee	9	#if defined(ICM_20948_USE_DMP) // Only include the 14301 Bytes of DMP if ICM_20948_USE_DMP is defined
saloutos	0:894b603d32ee	10
saloutos	0:894b603d32ee	11	#if defined(ARDUINO_ARCH_MBED) // ARDUINO_ARCH_MBED (APOLLO3 v2) does not support or require pgmspace.h / PROGMEM
saloutos	0:894b603d32ee	12	const uint8_t dmp3_image[] = {
saloutos	0:894b603d32ee	13	#elif (defined(__AVR__) \|\| defined(__arm__) \|\| defined(__ARDUINO_ARC__)) && !defined(__linux__) // Store the DMP firmware in PROGMEM on older AVR (ATmega) platforms
saloutos	0:894b603d32ee	14	#define ICM_20948_USE_PROGMEM_FOR_DMP
saloutos	0:894b603d32ee	15	#include <avr/pgmspace.h>
saloutos	0:894b603d32ee	16	const uint8_t dmp3_image[] PROGMEM = {
saloutos	0:894b603d32ee	17	#else
saloutos	0:894b603d32ee	18	const uint8_t dmp3_image[] = {
saloutos	0:894b603d32ee	19	#endif
saloutos	0:894b603d32ee	20
saloutos	0:894b603d32ee	21	#include "icm20948_img.dmp3a.h"
saloutos	0:894b603d32ee	22	};
saloutos	0:894b603d32ee	23	#endif
saloutos	0:894b603d32ee	24
saloutos	0:894b603d32ee	25	// ICM-20948 data is big-endian. We need to make it little-endian when writing into icm_20948_DMP_data_t
saloutos	0:894b603d32ee	26	const int DMP_Quat9_Byte_Ordering[icm_20948_DMP_Quat9_Bytes] =
saloutos	0:894b603d32ee	27	{
saloutos	0:894b603d32ee	28	3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8, 13, 12 // Also used for Geomag
saloutos	0:894b603d32ee	29	};
saloutos	0:894b603d32ee	30	const int DMP_Quat6_Byte_Ordering[icm_20948_DMP_Quat6_Bytes] =
saloutos	0:894b603d32ee	31	{
saloutos	0:894b603d32ee	32	3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8 // Also used for Gyro_Calibr, Compass_Calibr
saloutos	0:894b603d32ee	33	};
saloutos	0:894b603d32ee	34	const int DMP_PQuat6_Byte_Ordering[icm_20948_DMP_PQuat6_Bytes] =
saloutos	0:894b603d32ee	35	{
saloutos	0:894b603d32ee	36	1, 0, 3, 2, 5, 4 // Also used for Raw_Accel, Compass
saloutos	0:894b603d32ee	37	};
saloutos	0:894b603d32ee	38	const int DMP_Raw_Gyro_Byte_Ordering[icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes] =
saloutos	0:894b603d32ee	39	{
saloutos	0:894b603d32ee	40	1, 0, 3, 2, 5, 4, 7, 6, 9, 8, 11, 10};
saloutos	0:894b603d32ee	41	const int DMP_Activity_Recognition_Byte_Ordering[icm_20948_DMP_Activity_Recognition_Bytes] =
saloutos	0:894b603d32ee	42	{
saloutos	0:894b603d32ee	43	0, 1, 5, 4, 3, 2};
saloutos	0:894b603d32ee	44	const int DMP_Secondary_On_Off_Byte_Ordering[icm_20948_DMP_Secondary_On_Off_Bytes] =
saloutos	0:894b603d32ee	45	{
saloutos	0:894b603d32ee	46	1, 0};
saloutos	0:894b603d32ee	47
saloutos	0:894b603d32ee	48	const uint16_t inv_androidSensor_to_control_bits[ANDROID_SENSOR_NUM_MAX] =
saloutos	0:894b603d32ee	49	{
saloutos	0:894b603d32ee	50	// Data output control 1 register bit definition
saloutos	0:894b603d32ee	51	// 16-bit accel 0x8000
saloutos	0:894b603d32ee	52	// 16-bit gyro 0x4000
saloutos	0:894b603d32ee	53	// 16-bit compass 0x2000
saloutos	0:894b603d32ee	54	// 16-bit ALS 0x1000
saloutos	0:894b603d32ee	55	// 32-bit 6-axis quaternion 0x0800
saloutos	0:894b603d32ee	56	// 32-bit 9-axis quaternion + heading accuracy 0x0400
saloutos	0:894b603d32ee	57	// 16-bit pedometer quaternion 0x0200
saloutos	0:894b603d32ee	58	// 32-bit Geomag rv + heading accuracy 0x0100
saloutos	0:894b603d32ee	59	// 16-bit Pressure 0x0080
saloutos	0:894b603d32ee	60	// 32-bit calibrated gyro 0x0040
saloutos	0:894b603d32ee	61	// 32-bit calibrated compass 0x0020
saloutos	0:894b603d32ee	62	// Pedometer Step Detector 0x0010
saloutos	0:894b603d32ee	63	// Header 2 0x0008
saloutos	0:894b603d32ee	64	// Pedometer Step Indicator Bit 2 0x0004
saloutos	0:894b603d32ee	65	// Pedometer Step Indicator Bit 1 0x0002
saloutos	0:894b603d32ee	66	// Pedometer Step Indicator Bit 0 0x0001
saloutos	0:894b603d32ee	67	// Unsupported Sensors are 0xFFFF
saloutos	0:894b603d32ee	68
saloutos	0:894b603d32ee	69	0xFFFF, // 0 Meta Data
saloutos	0:894b603d32ee	70	0x8008, // 1 Accelerometer
saloutos	0:894b603d32ee	71	0x0028, // 2 Magnetic Field
saloutos	0:894b603d32ee	72	0x0408, // 3 Orientation
saloutos	0:894b603d32ee	73	0x4048, // 4 Gyroscope
saloutos	0:894b603d32ee	74	0x1008, // 5 Light
saloutos	0:894b603d32ee	75	0x0088, // 6 Pressure
saloutos	0:894b603d32ee	76	0xFFFF, // 7 Temperature
saloutos	0:894b603d32ee	77	0xFFFF, // 8 Proximity <----------- fixme
saloutos	0:894b603d32ee	78	0x0808, // 9 Gravity
saloutos	0:894b603d32ee	79	0x8808, // 10 Linear Acceleration
saloutos	0:894b603d32ee	80	0x0408, // 11 Rotation Vector
saloutos	0:894b603d32ee	81	0xFFFF, // 12 Humidity
saloutos	0:894b603d32ee	82	0xFFFF, // 13 Ambient Temperature
saloutos	0:894b603d32ee	83	0x2008, // 14 Magnetic Field Uncalibrated
saloutos	0:894b603d32ee	84	0x0808, // 15 Game Rotation Vector
saloutos	0:894b603d32ee	85	0x4008, // 16 Gyroscope Uncalibrated
saloutos	0:894b603d32ee	86	0x0000, // 17 Significant Motion
saloutos	0:894b603d32ee	87	0x0018, // 18 Step Detector
saloutos	0:894b603d32ee	88	0x0010, // 19 Step Counter <----------- fixme
saloutos	0:894b603d32ee	89	0x0108, // 20 Geomagnetic Rotation Vector
saloutos	0:894b603d32ee	90	0xFFFF, // 21 ANDROID_SENSOR_HEART_RATE,
saloutos	0:894b603d32ee	91	0xFFFF, // 22 ANDROID_SENSOR_PROXIMITY,
saloutos	0:894b603d32ee	92
saloutos	0:894b603d32ee	93	0x8008, // 23 ANDROID_SENSOR_WAKEUP_ACCELEROMETER,
saloutos	0:894b603d32ee	94	0x0028, // 24 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD,
saloutos	0:894b603d32ee	95	0x0408, // 25 ANDROID_SENSOR_WAKEUP_ORIENTATION,
saloutos	0:894b603d32ee	96	0x4048, // 26 ANDROID_SENSOR_WAKEUP_GYROSCOPE,
saloutos	0:894b603d32ee	97	0x1008, // 27 ANDROID_SENSOR_WAKEUP_LIGHT,
saloutos	0:894b603d32ee	98	0x0088, // 28 ANDROID_SENSOR_WAKEUP_PRESSURE,
saloutos	0:894b603d32ee	99	0x0808, // 29 ANDROID_SENSOR_WAKEUP_GRAVITY,
saloutos	0:894b603d32ee	100	0x8808, // 30 ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION,
saloutos	0:894b603d32ee	101	0x0408, // 31 ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR,
saloutos	0:894b603d32ee	102	0xFFFF, // 32 ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY,
saloutos	0:894b603d32ee	103	0xFFFF, // 33 ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE,
saloutos	0:894b603d32ee	104	0x2008, // 34 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED,
saloutos	0:894b603d32ee	105	0x0808, // 35 ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR,
saloutos	0:894b603d32ee	106	0x4008, // 36 ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED,
saloutos	0:894b603d32ee	107	0x0018, // 37 ANDROID_SENSOR_WAKEUP_STEP_DETECTOR,
saloutos	0:894b603d32ee	108	0x0010, // 38 ANDROID_SENSOR_WAKEUP_STEP_COUNTER,
saloutos	0:894b603d32ee	109	0x0108, // 39 ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR
saloutos	0:894b603d32ee	110	0xFFFF, // 40 ANDROID_SENSOR_WAKEUP_HEART_RATE,
saloutos	0:894b603d32ee	111	0x0000, // 41 ANDROID_SENSOR_WAKEUP_TILT_DETECTOR,
saloutos	0:894b603d32ee	112	0x8008, // 42 Raw Acc
saloutos	0:894b603d32ee	113	0x4048, // 43 Raw Gyr
saloutos	0:894b603d32ee	114	};
saloutos	0:894b603d32ee	115
saloutos	0:894b603d32ee	116	const ICM_20948_Serif_t NullSerif = {
saloutos	0:894b603d32ee	117	NULL, // write
saloutos	0:894b603d32ee	118	NULL, // read
saloutos	0:894b603d32ee	119	NULL, // user
saloutos	0:894b603d32ee	120	};
saloutos	0:894b603d32ee	121
saloutos	0:894b603d32ee	122	// Private function prototypes
saloutos	0:894b603d32ee	123
saloutos	0:894b603d32ee	124	// Function definitions
saloutos	0:894b603d32ee	125	ICM_20948_Status_e ICM_20948_init_struct(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	126	{
saloutos	0:894b603d32ee	127	// Initialize all elements by 0 except for _last_bank
saloutos	0:894b603d32ee	128	// Initialize _last_bank to 4 (invalid bank number)
saloutos	0:894b603d32ee	129	// so ICM_20948_set_bank function does not skip issuing bank change operation
saloutos	0:894b603d32ee	130	static const ICM_20948_Device_t init_device = { ._last_bank = 4 };
saloutos	0:894b603d32ee	131	*pdev = init_device;
saloutos	0:894b603d32ee	132	return ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	133	}
saloutos	0:894b603d32ee	134
saloutos	0:894b603d32ee	135	ICM_20948_Status_e ICM_20948_link_serif(ICM_20948_Device_t pdev, const ICM_20948_Serif_t s)
saloutos	0:894b603d32ee	136	{
saloutos	0:894b603d32ee	137	if (s == NULL)
saloutos	0:894b603d32ee	138	{
saloutos	0:894b603d32ee	139	return ICM_20948_Stat_ParamErr;
saloutos	0:894b603d32ee	140	}
saloutos	0:894b603d32ee	141	if (pdev == NULL)
saloutos	0:894b603d32ee	142	{
saloutos	0:894b603d32ee	143	return ICM_20948_Stat_ParamErr;
saloutos	0:894b603d32ee	144	}
saloutos	0:894b603d32ee	145	pdev->_serif = s;
saloutos	0:894b603d32ee	146	return ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	147	}
saloutos	0:894b603d32ee	148
saloutos	0:894b603d32ee	149	ICM_20948_Status_e ICM_20948_execute_w(ICM_20948_Device_t pdev, uint8_t regaddr, uint8_t pdata, uint32_t len)
saloutos	0:894b603d32ee	150	{
saloutos	0:894b603d32ee	151	if (pdev->_serif->write == NULL)
saloutos	0:894b603d32ee	152	{
saloutos	0:894b603d32ee	153	return ICM_20948_Stat_NotImpl;
saloutos	0:894b603d32ee	154	}
saloutos	0:894b603d32ee	155	return (*pdev->_serif->write)(regaddr, pdata, len, pdev->_serif->user);
saloutos	0:894b603d32ee	156	}
saloutos	0:894b603d32ee	157
saloutos	0:894b603d32ee	158	ICM_20948_Status_e ICM_20948_execute_r(ICM_20948_Device_t pdev, uint8_t regaddr, uint8_t pdata, uint32_t len)
saloutos	0:894b603d32ee	159	{
saloutos	0:894b603d32ee	160	if (pdev->_serif->read == NULL)
saloutos	0:894b603d32ee	161	{
saloutos	0:894b603d32ee	162	return ICM_20948_Stat_NotImpl;
saloutos	0:894b603d32ee	163	}
saloutos	0:894b603d32ee	164	return (*pdev->_serif->read)(regaddr, pdata, len, pdev->_serif->user);
saloutos	0:894b603d32ee	165	}
saloutos	0:894b603d32ee	166
saloutos	0:894b603d32ee	167	//Transact directly with an I2C device, one byte at a time
saloutos	0:894b603d32ee	168	//Used to configure a device before it is setup into a normal 0-3 peripheral slot
saloutos	0:894b603d32ee	169	ICM_20948_Status_e ICM_20948_i2c_controller_periph4_txn(ICM_20948_Device_t pdev, uint8_t addr, uint8_t reg, uint8_t data, uint8_t len, bool Rw, bool send_reg_addr)
saloutos	0:894b603d32ee	170	{
saloutos	0:894b603d32ee	171	// Thanks MikeFair! // https://github.com/kriswiner/MPU9250/issues/86
saloutos	0:894b603d32ee	172	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	173
saloutos	0:894b603d32ee	174	addr = (((Rw) ? 0x80 : 0x00) \| addr);
saloutos	0:894b603d32ee	175
saloutos	0:894b603d32ee	176	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	177	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_ADDR, (uint8_t *)&addr, 1);
saloutos	0:894b603d32ee	178	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	179	{
saloutos	0:894b603d32ee	180	return retval;
saloutos	0:894b603d32ee	181	}
saloutos	0:894b603d32ee	182
saloutos	0:894b603d32ee	183	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	184	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_REG, (uint8_t *)&reg, 1);
saloutos	0:894b603d32ee	185	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	186	{
saloutos	0:894b603d32ee	187	return retval;
saloutos	0:894b603d32ee	188	}
saloutos	0:894b603d32ee	189
saloutos	0:894b603d32ee	190	ICM_20948_I2C_PERIPH4_CTRL_t ctrl;
saloutos	0:894b603d32ee	191	ctrl.EN = 1;
saloutos	0:894b603d32ee	192	ctrl.INT_EN = false;
saloutos	0:894b603d32ee	193	ctrl.DLY = 0;
saloutos	0:894b603d32ee	194	ctrl.REG_DIS = !send_reg_addr;
saloutos	0:894b603d32ee	195
saloutos	0:894b603d32ee	196	ICM_20948_I2C_MST_STATUS_t i2c_mst_status;
saloutos	0:894b603d32ee	197	bool txn_failed = false;
saloutos	0:894b603d32ee	198	uint16_t nByte = 0;
saloutos	0:894b603d32ee	199
saloutos	0:894b603d32ee	200	while (nByte < len)
saloutos	0:894b603d32ee	201	{
saloutos	0:894b603d32ee	202	if (!Rw)
saloutos	0:894b603d32ee	203	{
saloutos	0:894b603d32ee	204	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	205	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_DO, (uint8_t *)&(data[nByte]), 1);
saloutos	0:894b603d32ee	206	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	207	{
saloutos	0:894b603d32ee	208	return retval;
saloutos	0:894b603d32ee	209	}
saloutos	0:894b603d32ee	210	}
saloutos	0:894b603d32ee	211
saloutos	0:894b603d32ee	212	// Kick off txn
saloutos	0:894b603d32ee	213	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	214	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_PERIPH4_CTRL_t));
saloutos	0:894b603d32ee	215	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	216	{
saloutos	0:894b603d32ee	217	return retval;
saloutos	0:894b603d32ee	218	}
saloutos	0:894b603d32ee	219
saloutos	0:894b603d32ee	220	// long tsTimeout = millis() + 3000; // Emergency timeout for txn (hard coded to 3 secs)
saloutos	0:894b603d32ee	221	uint32_t max_cycles = 1000;
saloutos	0:894b603d32ee	222	uint32_t count = 0;
saloutos	0:894b603d32ee	223	bool peripheral4Done = false;
saloutos	0:894b603d32ee	224	while (!peripheral4Done)
saloutos	0:894b603d32ee	225	{
saloutos	0:894b603d32ee	226	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	227	retval = ICM_20948_execute_r(pdev, AGB0_REG_I2C_MST_STATUS, (uint8_t *)&i2c_mst_status, 1);
saloutos	0:894b603d32ee	228
saloutos	0:894b603d32ee	229	peripheral4Done = (i2c_mst_status.I2C_PERIPH4_DONE /\| (millis() > tsTimeout) /); //Avoid forever-loops
saloutos	0:894b603d32ee	230	peripheral4Done \|= (count >= max_cycles);
saloutos	0:894b603d32ee	231	count++;
saloutos	0:894b603d32ee	232	}
saloutos	0:894b603d32ee	233	txn_failed = (i2c_mst_status.I2C_PERIPH4_NACK /\| (millis() > tsTimeout) /);
saloutos	0:894b603d32ee	234	txn_failed \|= (count >= max_cycles);
saloutos	0:894b603d32ee	235	if (txn_failed)
saloutos	0:894b603d32ee	236	break;
saloutos	0:894b603d32ee	237
saloutos	0:894b603d32ee	238	if (Rw)
saloutos	0:894b603d32ee	239	{
saloutos	0:894b603d32ee	240	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	241	retval = ICM_20948_execute_r(pdev, AGB3_REG_I2C_PERIPH4_DI, &data[nByte], 1);
saloutos	0:894b603d32ee	242	}
saloutos	0:894b603d32ee	243
saloutos	0:894b603d32ee	244	nByte++;
saloutos	0:894b603d32ee	245	}
saloutos	0:894b603d32ee	246
saloutos	0:894b603d32ee	247	if (txn_failed)
saloutos	0:894b603d32ee	248	{
saloutos	0:894b603d32ee	249	//We often fail here if mag is stuck
saloutos	0:894b603d32ee	250	return ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	251	}
saloutos	0:894b603d32ee	252
saloutos	0:894b603d32ee	253	return retval;
saloutos	0:894b603d32ee	254	}
saloutos	0:894b603d32ee	255
saloutos	0:894b603d32ee	256	ICM_20948_Status_e ICM_20948_i2c_master_single_w(ICM_20948_Device_t pdev, uint8_t addr, uint8_t reg, uint8_t data)
saloutos	0:894b603d32ee	257	{
saloutos	0:894b603d32ee	258	return ICM_20948_i2c_controller_periph4_txn(pdev, addr, reg, data, 1, false, true);
saloutos	0:894b603d32ee	259	}
saloutos	0:894b603d32ee	260
saloutos	0:894b603d32ee	261	ICM_20948_Status_e ICM_20948_i2c_master_single_r(ICM_20948_Device_t pdev, uint8_t addr, uint8_t reg, uint8_t data)
saloutos	0:894b603d32ee	262	{
saloutos	0:894b603d32ee	263	return ICM_20948_i2c_controller_periph4_txn(pdev, addr, reg, data, 1, true, true);
saloutos	0:894b603d32ee	264	}
saloutos	0:894b603d32ee	265
saloutos	0:894b603d32ee	266	ICM_20948_Status_e ICM_20948_set_bank(ICM_20948_Device_t *pdev, uint8_t bank)
saloutos	0:894b603d32ee	267	{
saloutos	0:894b603d32ee	268	if (bank > 3)
saloutos	0:894b603d32ee	269	{
saloutos	0:894b603d32ee	270	return ICM_20948_Stat_ParamErr;
saloutos	0:894b603d32ee	271	} // Only 4 possible banks
saloutos	0:894b603d32ee	272
saloutos	0:894b603d32ee	273	if (bank == pdev->_last_bank) // Do we need to change bank?
saloutos	0:894b603d32ee	274	return ICM_20948_Stat_Ok; // Bail if we don't need to change bank to avoid unnecessary bus traffic
saloutos	0:894b603d32ee	275
saloutos	0:894b603d32ee	276	pdev->_last_bank = bank; // Store the requested bank (before we bit-shift)
saloutos	0:894b603d32ee	277	bank = (bank << 4) & 0x30; // bits 5:4 of REG_BANK_SEL
saloutos	0:894b603d32ee	278	return ICM_20948_execute_w(pdev, REG_BANK_SEL, &bank, 1);
saloutos	0:894b603d32ee	279	}
saloutos	0:894b603d32ee	280
saloutos	0:894b603d32ee	281	ICM_20948_Status_e ICM_20948_sw_reset(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	282	{
saloutos	0:894b603d32ee	283	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	284	ICM_20948_PWR_MGMT_1_t reg;
saloutos	0:894b603d32ee	285
saloutos	0:894b603d32ee	286	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	287
saloutos	0:894b603d32ee	288	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	289	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	290	{
saloutos	0:894b603d32ee	291	return retval;
saloutos	0:894b603d32ee	292	}
saloutos	0:894b603d32ee	293
saloutos	0:894b603d32ee	294	reg.DEVICE_RESET = 1;
saloutos	0:894b603d32ee	295
saloutos	0:894b603d32ee	296	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	297	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	298	{
saloutos	0:894b603d32ee	299	return retval;
saloutos	0:894b603d32ee	300	}
saloutos	0:894b603d32ee	301	return retval;
saloutos	0:894b603d32ee	302	}
saloutos	0:894b603d32ee	303
saloutos	0:894b603d32ee	304	ICM_20948_Status_e ICM_20948_sleep(ICM_20948_Device_t *pdev, bool on)
saloutos	0:894b603d32ee	305	{
saloutos	0:894b603d32ee	306	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	307	ICM_20948_PWR_MGMT_1_t reg;
saloutos	0:894b603d32ee	308
saloutos	0:894b603d32ee	309	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	310
saloutos	0:894b603d32ee	311	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	312	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	313	{
saloutos	0:894b603d32ee	314	return retval;
saloutos	0:894b603d32ee	315	}
saloutos	0:894b603d32ee	316
saloutos	0:894b603d32ee	317	if (on)
saloutos	0:894b603d32ee	318	{
saloutos	0:894b603d32ee	319	reg.SLEEP = 1;
saloutos	0:894b603d32ee	320	}
saloutos	0:894b603d32ee	321	else
saloutos	0:894b603d32ee	322	{
saloutos	0:894b603d32ee	323	reg.SLEEP = 0;
saloutos	0:894b603d32ee	324	}
saloutos	0:894b603d32ee	325
saloutos	0:894b603d32ee	326	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	327	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	328	{
saloutos	0:894b603d32ee	329	return retval;
saloutos	0:894b603d32ee	330	}
saloutos	0:894b603d32ee	331	return retval;
saloutos	0:894b603d32ee	332	}
saloutos	0:894b603d32ee	333
saloutos	0:894b603d32ee	334	ICM_20948_Status_e ICM_20948_low_power(ICM_20948_Device_t *pdev, bool on)
saloutos	0:894b603d32ee	335	{
saloutos	0:894b603d32ee	336	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	337	ICM_20948_PWR_MGMT_1_t reg;
saloutos	0:894b603d32ee	338
saloutos	0:894b603d32ee	339	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	340
saloutos	0:894b603d32ee	341	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	342	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	343	{
saloutos	0:894b603d32ee	344	return retval;
saloutos	0:894b603d32ee	345	}
saloutos	0:894b603d32ee	346
saloutos	0:894b603d32ee	347	if (on)
saloutos	0:894b603d32ee	348	{
saloutos	0:894b603d32ee	349	reg.LP_EN = 1;
saloutos	0:894b603d32ee	350	}
saloutos	0:894b603d32ee	351	else
saloutos	0:894b603d32ee	352	{
saloutos	0:894b603d32ee	353	reg.LP_EN = 0;
saloutos	0:894b603d32ee	354	}
saloutos	0:894b603d32ee	355
saloutos	0:894b603d32ee	356	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	357	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	358	{
saloutos	0:894b603d32ee	359	return retval;
saloutos	0:894b603d32ee	360	}
saloutos	0:894b603d32ee	361	return retval;
saloutos	0:894b603d32ee	362	}
saloutos	0:894b603d32ee	363
saloutos	0:894b603d32ee	364	ICM_20948_Status_e ICM_20948_set_clock_source(ICM_20948_Device_t *pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source)
saloutos	0:894b603d32ee	365	{
saloutos	0:894b603d32ee	366	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	367	ICM_20948_PWR_MGMT_1_t reg;
saloutos	0:894b603d32ee	368
saloutos	0:894b603d32ee	369	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	370
saloutos	0:894b603d32ee	371	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	372	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	373	{
saloutos	0:894b603d32ee	374	return retval;
saloutos	0:894b603d32ee	375	}
saloutos	0:894b603d32ee	376
saloutos	0:894b603d32ee	377	reg.CLKSEL = source;
saloutos	0:894b603d32ee	378
saloutos	0:894b603d32ee	379	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
saloutos	0:894b603d32ee	380	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	381	{
saloutos	0:894b603d32ee	382	return retval;
saloutos	0:894b603d32ee	383	}
saloutos	0:894b603d32ee	384	return retval;
saloutos	0:894b603d32ee	385	}
saloutos	0:894b603d32ee	386
saloutos	0:894b603d32ee	387	ICM_20948_Status_e ICM_20948_get_who_am_i(ICM_20948_Device_t pdev, uint8_t whoami)
saloutos	0:894b603d32ee	388	{
saloutos	0:894b603d32ee	389	if (whoami == NULL)
saloutos	0:894b603d32ee	390	{
saloutos	0:894b603d32ee	391	return ICM_20948_Stat_ParamErr;
saloutos	0:894b603d32ee	392	}
saloutos	0:894b603d32ee	393	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	394	return ICM_20948_execute_r(pdev, AGB0_REG_WHO_AM_I, whoami, 1);
saloutos	0:894b603d32ee	395	}
saloutos	0:894b603d32ee	396
saloutos	0:894b603d32ee	397	ICM_20948_Status_e ICM_20948_check_id(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	398	{
saloutos	0:894b603d32ee	399	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	400	uint8_t whoami = 0x00;
saloutos	0:894b603d32ee	401	retval = ICM_20948_get_who_am_i(pdev, &whoami);
saloutos	0:894b603d32ee	402	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	403	{
saloutos	0:894b603d32ee	404	return retval;
saloutos	0:894b603d32ee	405	}
saloutos	0:894b603d32ee	406	if (whoami != ICM_20948_WHOAMI)
saloutos	0:894b603d32ee	407	{
saloutos	0:894b603d32ee	408	return ICM_20948_Stat_WrongID;
saloutos	0:894b603d32ee	409	}
saloutos	0:894b603d32ee	410	return retval;
saloutos	0:894b603d32ee	411	}
saloutos	0:894b603d32ee	412
saloutos	0:894b603d32ee	413	ICM_20948_Status_e ICM_20948_data_ready(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	414	{
saloutos	0:894b603d32ee	415	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	416	ICM_20948_INT_STATUS_1_t reg;
saloutos	0:894b603d32ee	417	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	418	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	419	{
saloutos	0:894b603d32ee	420	return retval;
saloutos	0:894b603d32ee	421	}
saloutos	0:894b603d32ee	422	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_STATUS_1, (uint8_t *)&reg, sizeof(ICM_20948_INT_STATUS_1_t));
saloutos	0:894b603d32ee	423	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	424	{
saloutos	0:894b603d32ee	425	return retval;
saloutos	0:894b603d32ee	426	}
saloutos	0:894b603d32ee	427	if (!reg.RAW_DATA_0_RDY_INT)
saloutos	0:894b603d32ee	428	{
saloutos	0:894b603d32ee	429	retval = ICM_20948_Stat_NoData;
saloutos	0:894b603d32ee	430	}
saloutos	0:894b603d32ee	431	return retval;
saloutos	0:894b603d32ee	432	}
saloutos	0:894b603d32ee	433
saloutos	0:894b603d32ee	434	// Interrupt Configuration
saloutos	0:894b603d32ee	435	ICM_20948_Status_e ICM_20948_int_pin_cfg(ICM_20948_Device_t pdev, ICM_20948_INT_PIN_CFG_t write, ICM_20948_INT_PIN_CFG_t *read)
saloutos	0:894b603d32ee	436	{
saloutos	0:894b603d32ee	437	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	438	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	439	if (write != NULL)
saloutos	0:894b603d32ee	440	{ // write first, if available
saloutos	0:894b603d32ee	441	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)write, sizeof(ICM_20948_INT_PIN_CFG_t));
saloutos	0:894b603d32ee	442	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	443	{
saloutos	0:894b603d32ee	444	return retval;
saloutos	0:894b603d32ee	445	}
saloutos	0:894b603d32ee	446	}
saloutos	0:894b603d32ee	447	if (read != NULL)
saloutos	0:894b603d32ee	448	{ // then read, to allow for verification
saloutos	0:894b603d32ee	449	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)read, sizeof(ICM_20948_INT_PIN_CFG_t));
saloutos	0:894b603d32ee	450	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	451	{
saloutos	0:894b603d32ee	452	return retval;
saloutos	0:894b603d32ee	453	}
saloutos	0:894b603d32ee	454	}
saloutos	0:894b603d32ee	455	return retval;
saloutos	0:894b603d32ee	456	}
saloutos	0:894b603d32ee	457
saloutos	0:894b603d32ee	458	ICM_20948_Status_e ICM_20948_int_enable(ICM_20948_Device_t pdev, ICM_20948_INT_enable_t write, ICM_20948_INT_enable_t *read)
saloutos	0:894b603d32ee	459	{
saloutos	0:894b603d32ee	460	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	461
saloutos	0:894b603d32ee	462	ICM_20948_INT_ENABLE_t en_0;
saloutos	0:894b603d32ee	463	ICM_20948_INT_ENABLE_1_t en_1;
saloutos	0:894b603d32ee	464	ICM_20948_INT_ENABLE_2_t en_2;
saloutos	0:894b603d32ee	465	ICM_20948_INT_ENABLE_3_t en_3;
saloutos	0:894b603d32ee	466
saloutos	0:894b603d32ee	467	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	468
saloutos	0:894b603d32ee	469	if (write != NULL)
saloutos	0:894b603d32ee	470	{ // If the write pointer is not NULL then write to the registers BEFORE reading
saloutos	0:894b603d32ee	471	en_0.I2C_MST_INT_EN = write->I2C_MST_INT_EN;
saloutos	0:894b603d32ee	472	en_0.DMP_INT1_EN = write->DMP_INT1_EN;
saloutos	0:894b603d32ee	473	en_0.PLL_READY_EN = write->PLL_RDY_EN;
saloutos	0:894b603d32ee	474	en_0.WOM_INT_EN = write->WOM_INT_EN;
saloutos	0:894b603d32ee	475	en_0.reserved_0 = 0; // Clear RAM garbage
saloutos	0:894b603d32ee	476	en_0.REG_WOF_EN = write->REG_WOF_EN;
saloutos	0:894b603d32ee	477	en_1.RAW_DATA_0_RDY_EN = write->RAW_DATA_0_RDY_EN;
saloutos	0:894b603d32ee	478	en_1.reserved_0 = 0; // Clear RAM garbage
saloutos	0:894b603d32ee	479	en_2.individual.FIFO_OVERFLOW_EN_4 = write->FIFO_OVERFLOW_EN_4;
saloutos	0:894b603d32ee	480	en_2.individual.FIFO_OVERFLOW_EN_3 = write->FIFO_OVERFLOW_EN_3;
saloutos	0:894b603d32ee	481	en_2.individual.FIFO_OVERFLOW_EN_2 = write->FIFO_OVERFLOW_EN_2;
saloutos	0:894b603d32ee	482	en_2.individual.FIFO_OVERFLOW_EN_1 = write->FIFO_OVERFLOW_EN_1;
saloutos	0:894b603d32ee	483	en_2.individual.FIFO_OVERFLOW_EN_0 = write->FIFO_OVERFLOW_EN_0;
saloutos	0:894b603d32ee	484	en_2.individual.reserved_0 = 0; // Clear RAM garbage
saloutos	0:894b603d32ee	485	en_3.individual.FIFO_WM_EN_4 = write->FIFO_WM_EN_4;
saloutos	0:894b603d32ee	486	en_3.individual.FIFO_WM_EN_3 = write->FIFO_WM_EN_3;
saloutos	0:894b603d32ee	487	en_3.individual.FIFO_WM_EN_2 = write->FIFO_WM_EN_2;
saloutos	0:894b603d32ee	488	en_3.individual.FIFO_WM_EN_1 = write->FIFO_WM_EN_1;
saloutos	0:894b603d32ee	489	en_3.individual.FIFO_WM_EN_0 = write->FIFO_WM_EN_0;
saloutos	0:894b603d32ee	490	en_3.individual.reserved_0 = 0; // Clear RAM garbage
saloutos	0:894b603d32ee	491
saloutos	0:894b603d32ee	492	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE, (uint8_t *)&en_0, sizeof(ICM_20948_INT_ENABLE_t));
saloutos	0:894b603d32ee	493	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	494	{
saloutos	0:894b603d32ee	495	return retval;
saloutos	0:894b603d32ee	496	}
saloutos	0:894b603d32ee	497	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_1, (uint8_t *)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t));
saloutos	0:894b603d32ee	498	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	499	{
saloutos	0:894b603d32ee	500	return retval;
saloutos	0:894b603d32ee	501	}
saloutos	0:894b603d32ee	502	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_2, (uint8_t *)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t));
saloutos	0:894b603d32ee	503	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	504	{
saloutos	0:894b603d32ee	505	return retval;
saloutos	0:894b603d32ee	506	}
saloutos	0:894b603d32ee	507	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_3, (uint8_t *)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t));
saloutos	0:894b603d32ee	508	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	509	{
saloutos	0:894b603d32ee	510	return retval;
saloutos	0:894b603d32ee	511	}
saloutos	0:894b603d32ee	512	}
saloutos	0:894b603d32ee	513
saloutos	0:894b603d32ee	514	if (read != NULL)
saloutos	0:894b603d32ee	515	{ // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
saloutos	0:894b603d32ee	516	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE, (uint8_t *)&en_0, sizeof(ICM_20948_INT_ENABLE_t));
saloutos	0:894b603d32ee	517	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	518	{
saloutos	0:894b603d32ee	519	return retval;
saloutos	0:894b603d32ee	520	}
saloutos	0:894b603d32ee	521	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_1, (uint8_t *)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t));
saloutos	0:894b603d32ee	522	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	523	{
saloutos	0:894b603d32ee	524	return retval;
saloutos	0:894b603d32ee	525	}
saloutos	0:894b603d32ee	526	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_2, (uint8_t *)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t));
saloutos	0:894b603d32ee	527	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	528	{
saloutos	0:894b603d32ee	529	return retval;
saloutos	0:894b603d32ee	530	}
saloutos	0:894b603d32ee	531	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_3, (uint8_t *)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t));
saloutos	0:894b603d32ee	532	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	533	{
saloutos	0:894b603d32ee	534	return retval;
saloutos	0:894b603d32ee	535	}
saloutos	0:894b603d32ee	536
saloutos	0:894b603d32ee	537	read->I2C_MST_INT_EN = en_0.I2C_MST_INT_EN;
saloutos	0:894b603d32ee	538	read->DMP_INT1_EN = en_0.DMP_INT1_EN;
saloutos	0:894b603d32ee	539	read->PLL_RDY_EN = en_0.PLL_READY_EN;
saloutos	0:894b603d32ee	540	read->WOM_INT_EN = en_0.WOM_INT_EN;
saloutos	0:894b603d32ee	541	read->REG_WOF_EN = en_0.REG_WOF_EN;
saloutos	0:894b603d32ee	542	read->RAW_DATA_0_RDY_EN = en_1.RAW_DATA_0_RDY_EN;
saloutos	0:894b603d32ee	543	read->FIFO_OVERFLOW_EN_4 = en_2.individual.FIFO_OVERFLOW_EN_4;
saloutos	0:894b603d32ee	544	read->FIFO_OVERFLOW_EN_3 = en_2.individual.FIFO_OVERFLOW_EN_3;
saloutos	0:894b603d32ee	545	read->FIFO_OVERFLOW_EN_2 = en_2.individual.FIFO_OVERFLOW_EN_2;
saloutos	0:894b603d32ee	546	read->FIFO_OVERFLOW_EN_1 = en_2.individual.FIFO_OVERFLOW_EN_1;
saloutos	0:894b603d32ee	547	read->FIFO_OVERFLOW_EN_0 = en_2.individual.FIFO_OVERFLOW_EN_0;
saloutos	0:894b603d32ee	548	read->FIFO_WM_EN_4 = en_3.individual.FIFO_WM_EN_4;
saloutos	0:894b603d32ee	549	read->FIFO_WM_EN_3 = en_3.individual.FIFO_WM_EN_3;
saloutos	0:894b603d32ee	550	read->FIFO_WM_EN_2 = en_3.individual.FIFO_WM_EN_2;
saloutos	0:894b603d32ee	551	read->FIFO_WM_EN_1 = en_3.individual.FIFO_WM_EN_1;
saloutos	0:894b603d32ee	552	read->FIFO_WM_EN_0 = en_3.individual.FIFO_WM_EN_0;
saloutos	0:894b603d32ee	553	}
saloutos	0:894b603d32ee	554
saloutos	0:894b603d32ee	555	return retval;
saloutos	0:894b603d32ee	556	}
saloutos	0:894b603d32ee	557
saloutos	0:894b603d32ee	558	ICM_20948_Status_e ICM_20948_wom_threshold(ICM_20948_Device_t pdev, ICM_20948_ACCEL_WOM_THR_t write, ICM_20948_ACCEL_WOM_THR_t *read)
saloutos	0:894b603d32ee	559	{
saloutos	0:894b603d32ee	560	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	561
saloutos	0:894b603d32ee	562	ICM_20948_ACCEL_WOM_THR_t thr;
saloutos	0:894b603d32ee	563
saloutos	0:894b603d32ee	564	retval = ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	565
saloutos	0:894b603d32ee	566	if (write != NULL)
saloutos	0:894b603d32ee	567	{ // If the write pointer is not NULL then write to the registers BEFORE reading
saloutos	0:894b603d32ee	568	thr.WOM_THRESHOLD = write->WOM_THRESHOLD;
saloutos	0:894b603d32ee	569
saloutos	0:894b603d32ee	570	retval = ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_WOM_THR, (uint8_t *)&thr, sizeof(ICM_20948_ACCEL_WOM_THR_t));
saloutos	0:894b603d32ee	571	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	572	{
saloutos	0:894b603d32ee	573	return retval;
saloutos	0:894b603d32ee	574	}
saloutos	0:894b603d32ee	575	}
saloutos	0:894b603d32ee	576
saloutos	0:894b603d32ee	577	if (read != NULL)
saloutos	0:894b603d32ee	578	{ // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
saloutos	0:894b603d32ee	579	retval = ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_WOM_THR, (uint8_t *)&thr, sizeof(ICM_20948_ACCEL_WOM_THR_t));
saloutos	0:894b603d32ee	580	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	581	{
saloutos	0:894b603d32ee	582	return retval;
saloutos	0:894b603d32ee	583	}
saloutos	0:894b603d32ee	584
saloutos	0:894b603d32ee	585	read->WOM_THRESHOLD = thr.WOM_THRESHOLD;
saloutos	0:894b603d32ee	586	}
saloutos	0:894b603d32ee	587
saloutos	0:894b603d32ee	588	return retval;
saloutos	0:894b603d32ee	589	}
saloutos	0:894b603d32ee	590
saloutos	0:894b603d32ee	591	ICM_20948_Status_e ICM_20948_set_sample_mode(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode)
saloutos	0:894b603d32ee	592	{
saloutos	0:894b603d32ee	593	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	594	ICM_20948_LP_CONFIG_t reg;
saloutos	0:894b603d32ee	595
saloutos	0:894b603d32ee	596	if (!(sensors & (ICM_20948_Internal_Acc \| ICM_20948_Internal_Gyr \| ICM_20948_Internal_Mst)))
saloutos	0:894b603d32ee	597	{
saloutos	0:894b603d32ee	598	return ICM_20948_Stat_SensorNotSupported;
saloutos	0:894b603d32ee	599	}
saloutos	0:894b603d32ee	600
saloutos	0:894b603d32ee	601	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
saloutos	0:894b603d32ee	602	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	603	{
saloutos	0:894b603d32ee	604	return retval;
saloutos	0:894b603d32ee	605	}
saloutos	0:894b603d32ee	606	retval = ICM_20948_execute_r(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
saloutos	0:894b603d32ee	607	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	608	{
saloutos	0:894b603d32ee	609	return retval;
saloutos	0:894b603d32ee	610	}
saloutos	0:894b603d32ee	611
saloutos	0:894b603d32ee	612	if (sensors & ICM_20948_Internal_Acc)
saloutos	0:894b603d32ee	613	{
saloutos	0:894b603d32ee	614	reg.ACCEL_CYCLE = mode;
saloutos	0:894b603d32ee	615	} // Set all desired sensors to this setting
saloutos	0:894b603d32ee	616	if (sensors & ICM_20948_Internal_Gyr)
saloutos	0:894b603d32ee	617	{
saloutos	0:894b603d32ee	618	reg.GYRO_CYCLE = mode;
saloutos	0:894b603d32ee	619	}
saloutos	0:894b603d32ee	620	if (sensors & ICM_20948_Internal_Mst)
saloutos	0:894b603d32ee	621	{
saloutos	0:894b603d32ee	622	reg.I2C_MST_CYCLE = mode;
saloutos	0:894b603d32ee	623	}
saloutos	0:894b603d32ee	624
saloutos	0:894b603d32ee	625	retval = ICM_20948_execute_w(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
saloutos	0:894b603d32ee	626	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	627	{
saloutos	0:894b603d32ee	628	return retval;
saloutos	0:894b603d32ee	629	}
saloutos	0:894b603d32ee	630
saloutos	0:894b603d32ee	631	// Check the data was written correctly
saloutos	0:894b603d32ee	632	retval = ICM_20948_execute_r(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
saloutos	0:894b603d32ee	633	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	634	{
saloutos	0:894b603d32ee	635	return retval;
saloutos	0:894b603d32ee	636	}
saloutos	0:894b603d32ee	637	if (sensors & ICM_20948_Internal_Acc)
saloutos	0:894b603d32ee	638	{
saloutos	0:894b603d32ee	639	if (reg.ACCEL_CYCLE != mode) retval = ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	640	}
saloutos	0:894b603d32ee	641	if (sensors & ICM_20948_Internal_Gyr)
saloutos	0:894b603d32ee	642	{
saloutos	0:894b603d32ee	643	if (reg.GYRO_CYCLE != mode) retval = ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	644	}
saloutos	0:894b603d32ee	645	if (sensors & ICM_20948_Internal_Mst)
saloutos	0:894b603d32ee	646	{
saloutos	0:894b603d32ee	647	if (reg.I2C_MST_CYCLE != mode) retval = ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	648	}
saloutos	0:894b603d32ee	649
saloutos	0:894b603d32ee	650	return retval;
saloutos	0:894b603d32ee	651	}
saloutos	0:894b603d32ee	652
saloutos	0:894b603d32ee	653	ICM_20948_Status_e ICM_20948_set_full_scale(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss)
saloutos	0:894b603d32ee	654	{
saloutos	0:894b603d32ee	655	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	656
saloutos	0:894b603d32ee	657	if (!(sensors & (ICM_20948_Internal_Acc \| ICM_20948_Internal_Gyr)))
saloutos	0:894b603d32ee	658	{
saloutos	0:894b603d32ee	659	return ICM_20948_Stat_SensorNotSupported;
saloutos	0:894b603d32ee	660	}
saloutos	0:894b603d32ee	661
saloutos	0:894b603d32ee	662	if (sensors & ICM_20948_Internal_Acc)
saloutos	0:894b603d32ee	663	{
saloutos	0:894b603d32ee	664	ICM_20948_ACCEL_CONFIG_t reg;
saloutos	0:894b603d32ee	665	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	666	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	667	reg.ACCEL_FS_SEL = fss.a;
saloutos	0:894b603d32ee	668	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	669	// Check the data was written correctly
saloutos	0:894b603d32ee	670	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	671	if (reg.ACCEL_FS_SEL != fss.a) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	672	}
saloutos	0:894b603d32ee	673	if (sensors & ICM_20948_Internal_Gyr)
saloutos	0:894b603d32ee	674	{
saloutos	0:894b603d32ee	675	ICM_20948_GYRO_CONFIG_1_t reg;
saloutos	0:894b603d32ee	676	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	677	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	678	reg.GYRO_FS_SEL = fss.g;
saloutos	0:894b603d32ee	679	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	680	// Check the data was written correctly
saloutos	0:894b603d32ee	681	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	682	if (reg.GYRO_FS_SEL != fss.g) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	683	}
saloutos	0:894b603d32ee	684	return retval;
saloutos	0:894b603d32ee	685	}
saloutos	0:894b603d32ee	686
saloutos	0:894b603d32ee	687	ICM_20948_Status_e ICM_20948_set_dlpf_cfg(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg)
saloutos	0:894b603d32ee	688	{
saloutos	0:894b603d32ee	689	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	690
saloutos	0:894b603d32ee	691	if (!(sensors & (ICM_20948_Internal_Acc \| ICM_20948_Internal_Gyr)))
saloutos	0:894b603d32ee	692	{
saloutos	0:894b603d32ee	693	return ICM_20948_Stat_SensorNotSupported;
saloutos	0:894b603d32ee	694	}
saloutos	0:894b603d32ee	695
saloutos	0:894b603d32ee	696	if (sensors & ICM_20948_Internal_Acc)
saloutos	0:894b603d32ee	697	{
saloutos	0:894b603d32ee	698	ICM_20948_ACCEL_CONFIG_t reg;
saloutos	0:894b603d32ee	699	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	700	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	701	reg.ACCEL_DLPFCFG = cfg.a;
saloutos	0:894b603d32ee	702	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	703	// Check the data was written correctly
saloutos	0:894b603d32ee	704	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	705	if (reg.ACCEL_DLPFCFG != cfg.a) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	706	}
saloutos	0:894b603d32ee	707	if (sensors & ICM_20948_Internal_Gyr)
saloutos	0:894b603d32ee	708	{
saloutos	0:894b603d32ee	709	ICM_20948_GYRO_CONFIG_1_t reg;
saloutos	0:894b603d32ee	710	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	711	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	712	reg.GYRO_DLPFCFG = cfg.g;
saloutos	0:894b603d32ee	713	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	714	// Check the data was written correctly
saloutos	0:894b603d32ee	715	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	716	if (reg.GYRO_DLPFCFG != cfg.g) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	717	}
saloutos	0:894b603d32ee	718	return retval;
saloutos	0:894b603d32ee	719	}
saloutos	0:894b603d32ee	720
saloutos	0:894b603d32ee	721	ICM_20948_Status_e ICM_20948_enable_dlpf(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, bool enable)
saloutos	0:894b603d32ee	722	{
saloutos	0:894b603d32ee	723	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	724
saloutos	0:894b603d32ee	725	if (!(sensors & (ICM_20948_Internal_Acc \| ICM_20948_Internal_Gyr)))
saloutos	0:894b603d32ee	726	{
saloutos	0:894b603d32ee	727	return ICM_20948_Stat_SensorNotSupported;
saloutos	0:894b603d32ee	728	}
saloutos	0:894b603d32ee	729
saloutos	0:894b603d32ee	730	if (sensors & ICM_20948_Internal_Acc)
saloutos	0:894b603d32ee	731	{
saloutos	0:894b603d32ee	732	ICM_20948_ACCEL_CONFIG_t reg;
saloutos	0:894b603d32ee	733	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	734	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	735	if (enable)
saloutos	0:894b603d32ee	736	{
saloutos	0:894b603d32ee	737	reg.ACCEL_FCHOICE = 1;
saloutos	0:894b603d32ee	738	}
saloutos	0:894b603d32ee	739	else
saloutos	0:894b603d32ee	740	{
saloutos	0:894b603d32ee	741	reg.ACCEL_FCHOICE = 0;
saloutos	0:894b603d32ee	742	}
saloutos	0:894b603d32ee	743	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	744	// Check the data was written correctly
saloutos	0:894b603d32ee	745	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
saloutos	0:894b603d32ee	746	if (enable)
saloutos	0:894b603d32ee	747	{
saloutos	0:894b603d32ee	748	if (reg.ACCEL_FCHOICE != 1) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	749	}
saloutos	0:894b603d32ee	750	else
saloutos	0:894b603d32ee	751	{
saloutos	0:894b603d32ee	752	if (reg.ACCEL_FCHOICE != 0) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	753	}
saloutos	0:894b603d32ee	754	}
saloutos	0:894b603d32ee	755	if (sensors & ICM_20948_Internal_Gyr)
saloutos	0:894b603d32ee	756	{
saloutos	0:894b603d32ee	757	ICM_20948_GYRO_CONFIG_1_t reg;
saloutos	0:894b603d32ee	758	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	759	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	760	if (enable)
saloutos	0:894b603d32ee	761	{
saloutos	0:894b603d32ee	762	reg.GYRO_FCHOICE = 1;
saloutos	0:894b603d32ee	763	}
saloutos	0:894b603d32ee	764	else
saloutos	0:894b603d32ee	765	{
saloutos	0:894b603d32ee	766	reg.GYRO_FCHOICE = 0;
saloutos	0:894b603d32ee	767	}
saloutos	0:894b603d32ee	768	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	769	// Check the data was written correctly
saloutos	0:894b603d32ee	770	retval \|= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
saloutos	0:894b603d32ee	771	if (enable)
saloutos	0:894b603d32ee	772	{
saloutos	0:894b603d32ee	773	if (reg.GYRO_FCHOICE != 1) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	774	}
saloutos	0:894b603d32ee	775	else
saloutos	0:894b603d32ee	776	{
saloutos	0:894b603d32ee	777	if (reg.GYRO_FCHOICE != 0) retval \|= ICM_20948_Stat_Err;
saloutos	0:894b603d32ee	778	}
saloutos	0:894b603d32ee	779	}
saloutos	0:894b603d32ee	780	return retval;
saloutos	0:894b603d32ee	781	}
saloutos	0:894b603d32ee	782
saloutos	0:894b603d32ee	783	ICM_20948_Status_e ICM_20948_set_sample_rate(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt)
saloutos	0:894b603d32ee	784	{
saloutos	0:894b603d32ee	785	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	786
saloutos	0:894b603d32ee	787	if (!(sensors & (ICM_20948_Internal_Acc \| ICM_20948_Internal_Gyr)))
saloutos	0:894b603d32ee	788	{
saloutos	0:894b603d32ee	789	return ICM_20948_Stat_SensorNotSupported;
saloutos	0:894b603d32ee	790	}
saloutos	0:894b603d32ee	791
saloutos	0:894b603d32ee	792	if (sensors & ICM_20948_Internal_Acc)
saloutos	0:894b603d32ee	793	{
saloutos	0:894b603d32ee	794	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	795	uint8_t div1 = (smplrt.a << 8);
saloutos	0:894b603d32ee	796	uint8_t div2 = (smplrt.a & 0xFF);
saloutos	0:894b603d32ee	797	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_SMPLRT_DIV_1, &div1, 1);
saloutos	0:894b603d32ee	798	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_SMPLRT_DIV_2, &div2, 1);
saloutos	0:894b603d32ee	799	}
saloutos	0:894b603d32ee	800	if (sensors & ICM_20948_Internal_Gyr)
saloutos	0:894b603d32ee	801	{
saloutos	0:894b603d32ee	802	retval \|= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
saloutos	0:894b603d32ee	803	uint8_t div = (smplrt.g);
saloutos	0:894b603d32ee	804	retval \|= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_SMPLRT_DIV, &div, 1);
saloutos	0:894b603d32ee	805	}
saloutos	0:894b603d32ee	806	return retval;
saloutos	0:894b603d32ee	807	}
saloutos	0:894b603d32ee	808
saloutos	0:894b603d32ee	809	// Interface Things
saloutos	0:894b603d32ee	810	ICM_20948_Status_e ICM_20948_i2c_master_passthrough(ICM_20948_Device_t *pdev, bool passthrough)
saloutos	0:894b603d32ee	811	{
saloutos	0:894b603d32ee	812	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	813
saloutos	0:894b603d32ee	814	ICM_20948_INT_PIN_CFG_t reg;
saloutos	0:894b603d32ee	815	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	816	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	817	{
saloutos	0:894b603d32ee	818	return retval;
saloutos	0:894b603d32ee	819	}
saloutos	0:894b603d32ee	820	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_INT_PIN_CFG_t));
saloutos	0:894b603d32ee	821	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	822	{
saloutos	0:894b603d32ee	823	return retval;
saloutos	0:894b603d32ee	824	}
saloutos	0:894b603d32ee	825	reg.BYPASS_EN = passthrough;
saloutos	0:894b603d32ee	826	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_INT_PIN_CFG_t));
saloutos	0:894b603d32ee	827	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	828	{
saloutos	0:894b603d32ee	829	return retval;
saloutos	0:894b603d32ee	830	}
saloutos	0:894b603d32ee	831
saloutos	0:894b603d32ee	832	return retval;
saloutos	0:894b603d32ee	833	}
saloutos	0:894b603d32ee	834
saloutos	0:894b603d32ee	835	ICM_20948_Status_e ICM_20948_i2c_master_enable(ICM_20948_Device_t *pdev, bool enable)
saloutos	0:894b603d32ee	836	{
saloutos	0:894b603d32ee	837	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	838
saloutos	0:894b603d32ee	839	// Disable BYPASS_EN
saloutos	0:894b603d32ee	840	retval = ICM_20948_i2c_master_passthrough(pdev, false);
saloutos	0:894b603d32ee	841	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	842	{
saloutos	0:894b603d32ee	843	return retval;
saloutos	0:894b603d32ee	844	}
saloutos	0:894b603d32ee	845
saloutos	0:894b603d32ee	846	ICM_20948_I2C_MST_CTRL_t ctrl;
saloutos	0:894b603d32ee	847	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	848	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	849	{
saloutos	0:894b603d32ee	850	return retval;
saloutos	0:894b603d32ee	851	}
saloutos	0:894b603d32ee	852	retval = ICM_20948_execute_r(pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t));
saloutos	0:894b603d32ee	853	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	854	{
saloutos	0:894b603d32ee	855	return retval;
saloutos	0:894b603d32ee	856	}
saloutos	0:894b603d32ee	857	ctrl.I2C_MST_CLK = 0x07; // corresponds to 345.6 kHz, good for up to 400 kHz
saloutos	0:894b603d32ee	858	ctrl.I2C_MST_P_NSR = 1;
saloutos	0:894b603d32ee	859	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t));
saloutos	0:894b603d32ee	860	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	861	{
saloutos	0:894b603d32ee	862	return retval;
saloutos	0:894b603d32ee	863	}
saloutos	0:894b603d32ee	864
saloutos	0:894b603d32ee	865	ICM_20948_USER_CTRL_t reg;
saloutos	0:894b603d32ee	866	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	867	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	868	{
saloutos	0:894b603d32ee	869	return retval;
saloutos	0:894b603d32ee	870	}
saloutos	0:894b603d32ee	871	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&reg, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	872	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	873	{
saloutos	0:894b603d32ee	874	return retval;
saloutos	0:894b603d32ee	875	}
saloutos	0:894b603d32ee	876	if (enable)
saloutos	0:894b603d32ee	877	{
saloutos	0:894b603d32ee	878	reg.I2C_MST_EN = 1;
saloutos	0:894b603d32ee	879	}
saloutos	0:894b603d32ee	880	else
saloutos	0:894b603d32ee	881	{
saloutos	0:894b603d32ee	882	reg.I2C_MST_EN = 0;
saloutos	0:894b603d32ee	883	}
saloutos	0:894b603d32ee	884	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&reg, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	885	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	886	{
saloutos	0:894b603d32ee	887	return retval;
saloutos	0:894b603d32ee	888	}
saloutos	0:894b603d32ee	889
saloutos	0:894b603d32ee	890	return retval;
saloutos	0:894b603d32ee	891	}
saloutos	0:894b603d32ee	892
saloutos	0:894b603d32ee	893	ICM_20948_Status_e ICM_20948_i2c_master_reset(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	894	{
saloutos	0:894b603d32ee	895	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	896
saloutos	0:894b603d32ee	897	ICM_20948_USER_CTRL_t ctrl;
saloutos	0:894b603d32ee	898	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	899	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	900	{
saloutos	0:894b603d32ee	901	return retval;
saloutos	0:894b603d32ee	902	}
saloutos	0:894b603d32ee	903
saloutos	0:894b603d32ee	904	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	905	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	906	{
saloutos	0:894b603d32ee	907	return retval;
saloutos	0:894b603d32ee	908	}
saloutos	0:894b603d32ee	909
saloutos	0:894b603d32ee	910	ctrl.I2C_MST_RST = 1; //Reset!
saloutos	0:894b603d32ee	911
saloutos	0:894b603d32ee	912	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	913	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	914	{
saloutos	0:894b603d32ee	915	return retval;
saloutos	0:894b603d32ee	916	}
saloutos	0:894b603d32ee	917	return retval;
saloutos	0:894b603d32ee	918	}
saloutos	0:894b603d32ee	919
saloutos	0:894b603d32ee	920	ICM_20948_Status_e ICM_20948_i2c_controller_configure_peripheral(ICM_20948_Device_t *pdev, uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap, uint8_t dataOut)
saloutos	0:894b603d32ee	921	{
saloutos	0:894b603d32ee	922	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	923
saloutos	0:894b603d32ee	924	uint8_t periph_addr_reg;
saloutos	0:894b603d32ee	925	uint8_t periph_reg_reg;
saloutos	0:894b603d32ee	926	uint8_t periph_ctrl_reg;
saloutos	0:894b603d32ee	927	uint8_t periph_do_reg;
saloutos	0:894b603d32ee	928
saloutos	0:894b603d32ee	929	switch (peripheral)
saloutos	0:894b603d32ee	930	{
saloutos	0:894b603d32ee	931	case 0:
saloutos	0:894b603d32ee	932	periph_addr_reg = AGB3_REG_I2C_PERIPH0_ADDR;
saloutos	0:894b603d32ee	933	periph_reg_reg = AGB3_REG_I2C_PERIPH0_REG;
saloutos	0:894b603d32ee	934	periph_ctrl_reg = AGB3_REG_I2C_PERIPH0_CTRL;
saloutos	0:894b603d32ee	935	periph_do_reg = AGB3_REG_I2C_PERIPH0_DO;
saloutos	0:894b603d32ee	936	break;
saloutos	0:894b603d32ee	937	case 1:
saloutos	0:894b603d32ee	938	periph_addr_reg = AGB3_REG_I2C_PERIPH1_ADDR;
saloutos	0:894b603d32ee	939	periph_reg_reg = AGB3_REG_I2C_PERIPH1_REG;
saloutos	0:894b603d32ee	940	periph_ctrl_reg = AGB3_REG_I2C_PERIPH1_CTRL;
saloutos	0:894b603d32ee	941	periph_do_reg = AGB3_REG_I2C_PERIPH1_DO;
saloutos	0:894b603d32ee	942	break;
saloutos	0:894b603d32ee	943	case 2:
saloutos	0:894b603d32ee	944	periph_addr_reg = AGB3_REG_I2C_PERIPH2_ADDR;
saloutos	0:894b603d32ee	945	periph_reg_reg = AGB3_REG_I2C_PERIPH2_REG;
saloutos	0:894b603d32ee	946	periph_ctrl_reg = AGB3_REG_I2C_PERIPH2_CTRL;
saloutos	0:894b603d32ee	947	periph_do_reg = AGB3_REG_I2C_PERIPH2_DO;
saloutos	0:894b603d32ee	948	break;
saloutos	0:894b603d32ee	949	case 3:
saloutos	0:894b603d32ee	950	periph_addr_reg = AGB3_REG_I2C_PERIPH3_ADDR;
saloutos	0:894b603d32ee	951	periph_reg_reg = AGB3_REG_I2C_PERIPH3_REG;
saloutos	0:894b603d32ee	952	periph_ctrl_reg = AGB3_REG_I2C_PERIPH3_CTRL;
saloutos	0:894b603d32ee	953	periph_do_reg = AGB3_REG_I2C_PERIPH3_DO;
saloutos	0:894b603d32ee	954	break;
saloutos	0:894b603d32ee	955	default:
saloutos	0:894b603d32ee	956	return ICM_20948_Stat_ParamErr;
saloutos	0:894b603d32ee	957	}
saloutos	0:894b603d32ee	958
saloutos	0:894b603d32ee	959	retval = ICM_20948_set_bank(pdev, 3);
saloutos	0:894b603d32ee	960	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	961	{
saloutos	0:894b603d32ee	962	return retval;
saloutos	0:894b603d32ee	963	}
saloutos	0:894b603d32ee	964
saloutos	0:894b603d32ee	965	// Set the peripheral address and the Rw flag
saloutos	0:894b603d32ee	966	ICM_20948_I2C_PERIPHX_ADDR_t address;
saloutos	0:894b603d32ee	967	address.ID = addr;
saloutos	0:894b603d32ee	968	if (Rw)
saloutos	0:894b603d32ee	969	{
saloutos	0:894b603d32ee	970	address.RNW = 1;
saloutos	0:894b603d32ee	971	}
saloutos	0:894b603d32ee	972	else
saloutos	0:894b603d32ee	973	{
saloutos	0:894b603d32ee	974	address.RNW = 0; // Make sure bit is clear (just in case there is any garbage in that RAM location)
saloutos	0:894b603d32ee	975	}
saloutos	0:894b603d32ee	976	retval = ICM_20948_execute_w(pdev, periph_addr_reg, (uint8_t *)&address, sizeof(ICM_20948_I2C_PERIPHX_ADDR_t));
saloutos	0:894b603d32ee	977	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	978	{
saloutos	0:894b603d32ee	979	return retval;
saloutos	0:894b603d32ee	980	}
saloutos	0:894b603d32ee	981
saloutos	0:894b603d32ee	982	// If we are setting up a write, configure the Data Out register too
saloutos	0:894b603d32ee	983	if (!Rw)
saloutos	0:894b603d32ee	984	{
saloutos	0:894b603d32ee	985	ICM_20948_I2C_PERIPHX_DO_t dataOutByte;
saloutos	0:894b603d32ee	986	dataOutByte.DO = dataOut;
saloutos	0:894b603d32ee	987	retval = ICM_20948_execute_w(pdev, periph_do_reg, (uint8_t *)&dataOutByte, sizeof(ICM_20948_I2C_PERIPHX_DO_t));
saloutos	0:894b603d32ee	988	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	989	{
saloutos	0:894b603d32ee	990	return retval;
saloutos	0:894b603d32ee	991	}
saloutos	0:894b603d32ee	992	}
saloutos	0:894b603d32ee	993
saloutos	0:894b603d32ee	994	// Set the peripheral sub-address (register address)
saloutos	0:894b603d32ee	995	ICM_20948_I2C_PERIPHX_REG_t subaddress;
saloutos	0:894b603d32ee	996	subaddress.REG = reg;
saloutos	0:894b603d32ee	997	retval = ICM_20948_execute_w(pdev, periph_reg_reg, (uint8_t *)&subaddress, sizeof(ICM_20948_I2C_PERIPHX_REG_t));
saloutos	0:894b603d32ee	998	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	999	{
saloutos	0:894b603d32ee	1000	return retval;
saloutos	0:894b603d32ee	1001	}
saloutos	0:894b603d32ee	1002
saloutos	0:894b603d32ee	1003	// Set up the control info
saloutos	0:894b603d32ee	1004	ICM_20948_I2C_PERIPHX_CTRL_t ctrl;
saloutos	0:894b603d32ee	1005	ctrl.LENG = len;
saloutos	0:894b603d32ee	1006	ctrl.EN = enable;
saloutos	0:894b603d32ee	1007	ctrl.REG_DIS = data_only;
saloutos	0:894b603d32ee	1008	ctrl.GRP = grp;
saloutos	0:894b603d32ee	1009	ctrl.BYTE_SW = swap;
saloutos	0:894b603d32ee	1010	retval = ICM_20948_execute_w(pdev, periph_ctrl_reg, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_PERIPHX_CTRL_t));
saloutos	0:894b603d32ee	1011	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1012	{
saloutos	0:894b603d32ee	1013	return retval;
saloutos	0:894b603d32ee	1014	}
saloutos	0:894b603d32ee	1015
saloutos	0:894b603d32ee	1016	return retval;
saloutos	0:894b603d32ee	1017	}
saloutos	0:894b603d32ee	1018
saloutos	0:894b603d32ee	1019	// Higher Level
saloutos	0:894b603d32ee	1020	ICM_20948_Status_e ICM_20948_get_agmt(ICM_20948_Device_t pdev, ICM_20948_AGMT_t pagmt)
saloutos	0:894b603d32ee	1021	{
saloutos	0:894b603d32ee	1022	if (pagmt == NULL)
saloutos	0:894b603d32ee	1023	{
saloutos	0:894b603d32ee	1024	return ICM_20948_Stat_ParamErr;
saloutos	0:894b603d32ee	1025	}
saloutos	0:894b603d32ee	1026
saloutos	0:894b603d32ee	1027	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1028	const uint8_t numbytes = 14 + 9; //Read Accel, gyro, temp, and 9 bytes of mag
saloutos	0:894b603d32ee	1029	uint8_t buff[numbytes];
saloutos	0:894b603d32ee	1030
saloutos	0:894b603d32ee	1031	// Get readings
saloutos	0:894b603d32ee	1032	retval \|= ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1033	retval \|= ICM_20948_execute_r(pdev, (uint8_t)AGB0_REG_ACCEL_XOUT_H, buff, numbytes);
saloutos	0:894b603d32ee	1034
saloutos	0:894b603d32ee	1035	pagmt->acc.axes.x = ((buff[0] << 8) \| (buff[1] & 0xFF));
saloutos	0:894b603d32ee	1036	pagmt->acc.axes.y = ((buff[2] << 8) \| (buff[3] & 0xFF));
saloutos	0:894b603d32ee	1037	pagmt->acc.axes.z = ((buff[4] << 8) \| (buff[5] & 0xFF));
saloutos	0:894b603d32ee	1038
saloutos	0:894b603d32ee	1039	pagmt->gyr.axes.x = ((buff[6] << 8) \| (buff[7] & 0xFF));
saloutos	0:894b603d32ee	1040	pagmt->gyr.axes.y = ((buff[8] << 8) \| (buff[9] & 0xFF));
saloutos	0:894b603d32ee	1041	pagmt->gyr.axes.z = ((buff[10] << 8) \| (buff[11] & 0xFF));
saloutos	0:894b603d32ee	1042
saloutos	0:894b603d32ee	1043	pagmt->tmp.val = ((buff[12] << 8) \| (buff[13] & 0xFF));
saloutos	0:894b603d32ee	1044
saloutos	0:894b603d32ee	1045	pagmt->magStat1 = buff[14];
saloutos	0:894b603d32ee	1046	pagmt->mag.axes.x = ((buff[16] << 8) \| (buff[15] & 0xFF)); //Mag data is read little endian
saloutos	0:894b603d32ee	1047	pagmt->mag.axes.y = ((buff[18] << 8) \| (buff[17] & 0xFF));
saloutos	0:894b603d32ee	1048	pagmt->mag.axes.z = ((buff[20] << 8) \| (buff[19] & 0xFF));
saloutos	0:894b603d32ee	1049	pagmt->magStat2 = buff[22];
saloutos	0:894b603d32ee	1050
saloutos	0:894b603d32ee	1051	// Get settings to be able to compute scaled values
saloutos	0:894b603d32ee	1052	retval \|= ICM_20948_set_bank(pdev, 2);
saloutos	0:894b603d32ee	1053	ICM_20948_ACCEL_CONFIG_t acfg;
saloutos	0:894b603d32ee	1054	retval \|= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG, (uint8_t )&acfg, 1 sizeof(acfg));
saloutos	0:894b603d32ee	1055	pagmt->fss.a = acfg.ACCEL_FS_SEL; // Worth noting that without explicitly setting the FS range of the accelerometer it was showing the register value for +/- 2g but the reported values were actually scaled to the +/- 16g range
saloutos	0:894b603d32ee	1056	// Wait a minute... now it seems like this problem actually comes from the digital low-pass filter. When enabled the value is 1/8 what it should be...
saloutos	0:894b603d32ee	1057	retval \|= ICM_20948_set_bank(pdev, 2);
saloutos	0:894b603d32ee	1058	ICM_20948_GYRO_CONFIG_1_t gcfg1;
saloutos	0:894b603d32ee	1059	retval \|= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_GYRO_CONFIG_1, (uint8_t )&gcfg1, 1 sizeof(gcfg1));
saloutos	0:894b603d32ee	1060	pagmt->fss.g = gcfg1.GYRO_FS_SEL;
saloutos	0:894b603d32ee	1061	ICM_20948_ACCEL_CONFIG_2_t acfg2;
saloutos	0:894b603d32ee	1062	retval \|= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG_2, (uint8_t )&acfg2, 1 sizeof(acfg2));
saloutos	0:894b603d32ee	1063
saloutos	0:894b603d32ee	1064	return retval;
saloutos	0:894b603d32ee	1065	}
saloutos	0:894b603d32ee	1066
saloutos	0:894b603d32ee	1067	// FIFO
saloutos	0:894b603d32ee	1068
saloutos	0:894b603d32ee	1069	ICM_20948_Status_e ICM_20948_enable_FIFO(ICM_20948_Device_t *pdev, bool enable)
saloutos	0:894b603d32ee	1070	{
saloutos	0:894b603d32ee	1071	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1072
saloutos	0:894b603d32ee	1073	ICM_20948_USER_CTRL_t ctrl;
saloutos	0:894b603d32ee	1074	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1075	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1076	{
saloutos	0:894b603d32ee	1077	return retval;
saloutos	0:894b603d32ee	1078	}
saloutos	0:894b603d32ee	1079
saloutos	0:894b603d32ee	1080	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	1081	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1082	{
saloutos	0:894b603d32ee	1083	return retval;
saloutos	0:894b603d32ee	1084	}
saloutos	0:894b603d32ee	1085
saloutos	0:894b603d32ee	1086	if (enable)
saloutos	0:894b603d32ee	1087	ctrl.FIFO_EN = 1;
saloutos	0:894b603d32ee	1088	else
saloutos	0:894b603d32ee	1089	ctrl.FIFO_EN = 0;
saloutos	0:894b603d32ee	1090
saloutos	0:894b603d32ee	1091	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	1092	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1093	{
saloutos	0:894b603d32ee	1094	return retval;
saloutos	0:894b603d32ee	1095	}
saloutos	0:894b603d32ee	1096	return retval;
saloutos	0:894b603d32ee	1097	}
saloutos	0:894b603d32ee	1098
saloutos	0:894b603d32ee	1099	ICM_20948_Status_e ICM_20948_reset_FIFO(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	1100	{
saloutos	0:894b603d32ee	1101	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1102
saloutos	0:894b603d32ee	1103	ICM_20948_FIFO_RST_t ctrl;
saloutos	0:894b603d32ee	1104	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1105	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1106	{
saloutos	0:894b603d32ee	1107	return retval;
saloutos	0:894b603d32ee	1108	}
saloutos	0:894b603d32ee	1109
saloutos	0:894b603d32ee	1110	retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_RST, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_RST_t));
saloutos	0:894b603d32ee	1111	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1112	{
saloutos	0:894b603d32ee	1113	return retval;
saloutos	0:894b603d32ee	1114	}
saloutos	0:894b603d32ee	1115
saloutos	0:894b603d32ee	1116	ctrl.FIFO_RESET = 0x1F; // Datasheet says "FIFO_RESET[4:0]"
saloutos	0:894b603d32ee	1117
saloutos	0:894b603d32ee	1118	retval = ICM_20948_execute_w(pdev, AGB0_REG_FIFO_RST, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_RST_t));
saloutos	0:894b603d32ee	1119	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1120	{
saloutos	0:894b603d32ee	1121	return retval;
saloutos	0:894b603d32ee	1122	}
saloutos	0:894b603d32ee	1123
saloutos	0:894b603d32ee	1124	//delay ???
saloutos	0:894b603d32ee	1125
saloutos	0:894b603d32ee	1126	ctrl.FIFO_RESET = 0x1E; // The InvenSense Nucleo examples write 0x1F followed by 0x1E
saloutos	0:894b603d32ee	1127
saloutos	0:894b603d32ee	1128	retval = ICM_20948_execute_w(pdev, AGB0_REG_FIFO_RST, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_RST_t));
saloutos	0:894b603d32ee	1129	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1130	{
saloutos	0:894b603d32ee	1131	return retval;
saloutos	0:894b603d32ee	1132	}
saloutos	0:894b603d32ee	1133
saloutos	0:894b603d32ee	1134	return retval;
saloutos	0:894b603d32ee	1135	}
saloutos	0:894b603d32ee	1136
saloutos	0:894b603d32ee	1137	ICM_20948_Status_e ICM_20948_set_FIFO_mode(ICM_20948_Device_t *pdev, bool snapshot)
saloutos	0:894b603d32ee	1138	{
saloutos	0:894b603d32ee	1139	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1140
saloutos	0:894b603d32ee	1141	ICM_20948_FIFO_MODE_t ctrl;
saloutos	0:894b603d32ee	1142	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1143	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1144	{
saloutos	0:894b603d32ee	1145	return retval;
saloutos	0:894b603d32ee	1146	}
saloutos	0:894b603d32ee	1147
saloutos	0:894b603d32ee	1148	retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_MODE, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_MODE_t));
saloutos	0:894b603d32ee	1149	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1150	{
saloutos	0:894b603d32ee	1151	return retval;
saloutos	0:894b603d32ee	1152	}
saloutos	0:894b603d32ee	1153
saloutos	0:894b603d32ee	1154	if (snapshot)
saloutos	0:894b603d32ee	1155	ctrl.FIFO_MODE = 0x1F; // Datasheet says "FIFO_MODE[4:0]"
saloutos	0:894b603d32ee	1156	else
saloutos	0:894b603d32ee	1157	ctrl.FIFO_MODE = 0;
saloutos	0:894b603d32ee	1158
saloutos	0:894b603d32ee	1159	retval = ICM_20948_execute_w(pdev, AGB0_REG_FIFO_MODE, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_MODE_t));
saloutos	0:894b603d32ee	1160	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1161	{
saloutos	0:894b603d32ee	1162	return retval;
saloutos	0:894b603d32ee	1163	}
saloutos	0:894b603d32ee	1164	return retval;
saloutos	0:894b603d32ee	1165	}
saloutos	0:894b603d32ee	1166
saloutos	0:894b603d32ee	1167	ICM_20948_Status_e ICM_20948_get_FIFO_count(ICM_20948_Device_t pdev, uint16_t count)
saloutos	0:894b603d32ee	1168	{
saloutos	0:894b603d32ee	1169	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1170
saloutos	0:894b603d32ee	1171	ICM_20948_FIFO_COUNTH_t ctrlh;
saloutos	0:894b603d32ee	1172	ICM_20948_FIFO_COUNTL_t ctrll;
saloutos	0:894b603d32ee	1173	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1174	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1175	{
saloutos	0:894b603d32ee	1176	return retval;
saloutos	0:894b603d32ee	1177	}
saloutos	0:894b603d32ee	1178
saloutos	0:894b603d32ee	1179	retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_COUNT_H, (uint8_t *)&ctrlh, sizeof(ICM_20948_FIFO_COUNTH_t));
saloutos	0:894b603d32ee	1180	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1181	{
saloutos	0:894b603d32ee	1182	return retval;
saloutos	0:894b603d32ee	1183	}
saloutos	0:894b603d32ee	1184
saloutos	0:894b603d32ee	1185	ctrlh.FIFO_COUNTH &= 0x1F; // Datasheet says "FIFO_CNT[12:8]"
saloutos	0:894b603d32ee	1186
saloutos	0:894b603d32ee	1187	retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_COUNT_L, (uint8_t *)&ctrll, sizeof(ICM_20948_FIFO_COUNTL_t));
saloutos	0:894b603d32ee	1188	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1189	{
saloutos	0:894b603d32ee	1190	return retval;
saloutos	0:894b603d32ee	1191	}
saloutos	0:894b603d32ee	1192
saloutos	0:894b603d32ee	1193	*count = (((uint16_t)ctrlh.FIFO_COUNTH) << 8) \| (uint16_t)ctrll.FIFO_COUNTL;
saloutos	0:894b603d32ee	1194
saloutos	0:894b603d32ee	1195	return retval;
saloutos	0:894b603d32ee	1196	}
saloutos	0:894b603d32ee	1197
saloutos	0:894b603d32ee	1198	ICM_20948_Status_e ICM_20948_read_FIFO(ICM_20948_Device_t pdev, uint8_t data, uint8_t len)
saloutos	0:894b603d32ee	1199	{
saloutos	0:894b603d32ee	1200	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1201
saloutos	0:894b603d32ee	1202	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1203	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1204	{
saloutos	0:894b603d32ee	1205	return retval;
saloutos	0:894b603d32ee	1206	}
saloutos	0:894b603d32ee	1207
saloutos	0:894b603d32ee	1208	retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_R_W, data, len);
saloutos	0:894b603d32ee	1209	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1210	{
saloutos	0:894b603d32ee	1211	return retval;
saloutos	0:894b603d32ee	1212	}
saloutos	0:894b603d32ee	1213
saloutos	0:894b603d32ee	1214	return retval;
saloutos	0:894b603d32ee	1215	}
saloutos	0:894b603d32ee	1216
saloutos	0:894b603d32ee	1217	// DMP
saloutos	0:894b603d32ee	1218
saloutos	0:894b603d32ee	1219	ICM_20948_Status_e ICM_20948_enable_DMP(ICM_20948_Device_t *pdev, bool enable)
saloutos	0:894b603d32ee	1220	{
saloutos	0:894b603d32ee	1221	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1222
saloutos	0:894b603d32ee	1223	ICM_20948_USER_CTRL_t ctrl;
saloutos	0:894b603d32ee	1224	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1225	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1226	{
saloutos	0:894b603d32ee	1227	return retval;
saloutos	0:894b603d32ee	1228	}
saloutos	0:894b603d32ee	1229
saloutos	0:894b603d32ee	1230	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	1231	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1232	{
saloutos	0:894b603d32ee	1233	return retval;
saloutos	0:894b603d32ee	1234	}
saloutos	0:894b603d32ee	1235
saloutos	0:894b603d32ee	1236	if (enable)
saloutos	0:894b603d32ee	1237	ctrl.DMP_EN = 1;
saloutos	0:894b603d32ee	1238	else
saloutos	0:894b603d32ee	1239	ctrl.DMP_EN = 0;
saloutos	0:894b603d32ee	1240
saloutos	0:894b603d32ee	1241	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	1242	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1243	{
saloutos	0:894b603d32ee	1244	return retval;
saloutos	0:894b603d32ee	1245	}
saloutos	0:894b603d32ee	1246	return retval;
saloutos	0:894b603d32ee	1247	}
saloutos	0:894b603d32ee	1248
saloutos	0:894b603d32ee	1249	ICM_20948_Status_e ICM_20948_reset_DMP(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	1250	{
saloutos	0:894b603d32ee	1251	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1252
saloutos	0:894b603d32ee	1253	ICM_20948_USER_CTRL_t ctrl;
saloutos	0:894b603d32ee	1254	retval = ICM_20948_set_bank(pdev, 0);
saloutos	0:894b603d32ee	1255	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1256	{
saloutos	0:894b603d32ee	1257	return retval;
saloutos	0:894b603d32ee	1258	}
saloutos	0:894b603d32ee	1259
saloutos	0:894b603d32ee	1260	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	1261	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1262	{
saloutos	0:894b603d32ee	1263	return retval;
saloutos	0:894b603d32ee	1264	}
saloutos	0:894b603d32ee	1265
saloutos	0:894b603d32ee	1266	ctrl.DMP_RST = 1;
saloutos	0:894b603d32ee	1267
saloutos	0:894b603d32ee	1268	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
saloutos	0:894b603d32ee	1269	if (retval != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1270	{
saloutos	0:894b603d32ee	1271	return retval;
saloutos	0:894b603d32ee	1272	}
saloutos	0:894b603d32ee	1273	return retval;
saloutos	0:894b603d32ee	1274	}
saloutos	0:894b603d32ee	1275
saloutos	0:894b603d32ee	1276	ICM_20948_Status_e ICM_20948_firmware_load(ICM_20948_Device_t *pdev)
saloutos	0:894b603d32ee	1277	{
saloutos	0:894b603d32ee	1278	#if defined(ICM_20948_USE_DMP)
saloutos	0:894b603d32ee	1279	return (inv_icm20948_firmware_load(pdev, dmp3_image, sizeof(dmp3_image), DMP_LOAD_START));
saloutos	0:894b603d32ee	1280	#else
saloutos	0:894b603d32ee	1281	return ICM_20948_Stat_DMPNotSupported;
saloutos	0:894b603d32ee	1282	#endif
saloutos	0:894b603d32ee	1283	}
saloutos	0:894b603d32ee	1284
saloutos	0:894b603d32ee	1285	/** @brief Loads the DMP firmware from SRAM
saloutos	0:894b603d32ee	1286	* @param[in] data pointer where the image
saloutos	0:894b603d32ee	1287	* @param[in] size size if the image
saloutos	0:894b603d32ee	1288	* @param[in] load_addr address to loading the image
saloutos	0:894b603d32ee	1289	* @return 0 in case of success, -1 for any error
saloutos	0:894b603d32ee	1290	*/
saloutos	0:894b603d32ee	1291	ICM_20948_Status_e inv_icm20948_firmware_load(ICM_20948_Device_t pdev, const unsigned char data_start, unsigned short size_start, unsigned short load_addr)
saloutos	0:894b603d32ee	1292	{
saloutos	0:894b603d32ee	1293	int write_size;
saloutos	0:894b603d32ee	1294	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1295	unsigned short memaddr;
saloutos	0:894b603d32ee	1296	const unsigned char *data;
saloutos	0:894b603d32ee	1297	unsigned short size;
saloutos	0:894b603d32ee	1298	unsigned char data_cmp[INV_MAX_SERIAL_READ];
saloutos	0:894b603d32ee	1299	int flag = 0;
saloutos	0:894b603d32ee	1300
saloutos	0:894b603d32ee	1301	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	1302	return ICM_20948_Stat_DMPNotSupported;
saloutos	0:894b603d32ee	1303
saloutos	0:894b603d32ee	1304	if (pdev->_firmware_loaded)
saloutos	0:894b603d32ee	1305	return ICM_20948_Stat_Ok; // Bail with no error if firmware is already loaded
saloutos	0:894b603d32ee	1306
saloutos	0:894b603d32ee	1307	result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
saloutos	0:894b603d32ee	1308	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1309	{
saloutos	0:894b603d32ee	1310	return result;
saloutos	0:894b603d32ee	1311	}
saloutos	0:894b603d32ee	1312
saloutos	0:894b603d32ee	1313	result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
saloutos	0:894b603d32ee	1314	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1315	{
saloutos	0:894b603d32ee	1316	return result;
saloutos	0:894b603d32ee	1317	}
saloutos	0:894b603d32ee	1318
saloutos	0:894b603d32ee	1319	// Write DMP memory
saloutos	0:894b603d32ee	1320
saloutos	0:894b603d32ee	1321	data = data_start;
saloutos	0:894b603d32ee	1322	size = size_start;
saloutos	0:894b603d32ee	1323	memaddr = load_addr;
saloutos	0:894b603d32ee	1324	#ifdef ICM_20948_USE_PROGMEM_FOR_DMP
saloutos	0:894b603d32ee	1325	unsigned char data_not_pg[INV_MAX_SERIAL_READ]; // Suggested by @HyperKokichi in Issue #63
saloutos	0:894b603d32ee	1326	#endif
saloutos	0:894b603d32ee	1327	while (size > 0)
saloutos	0:894b603d32ee	1328	{
saloutos	0:894b603d32ee	1329	//write_size = min(size, INV_MAX_SERIAL_WRITE); // Write in chunks of INV_MAX_SERIAL_WRITE
saloutos	0:894b603d32ee	1330	if (size <= INV_MAX_SERIAL_WRITE) // Write in chunks of INV_MAX_SERIAL_WRITE
saloutos	0:894b603d32ee	1331	write_size = size;
saloutos	0:894b603d32ee	1332	else
saloutos	0:894b603d32ee	1333	write_size = INV_MAX_SERIAL_WRITE;
saloutos	0:894b603d32ee	1334	if ((memaddr & 0xff) + write_size > 0x100)
saloutos	0:894b603d32ee	1335	{
saloutos	0:894b603d32ee	1336	// Moved across a bank
saloutos	0:894b603d32ee	1337	write_size = (memaddr & 0xff) + write_size - 0x100;
saloutos	0:894b603d32ee	1338	}
saloutos	0:894b603d32ee	1339	#ifdef ICM_20948_USE_PROGMEM_FOR_DMP
saloutos	0:894b603d32ee	1340	memcpy_P(data_not_pg, data, write_size); // Suggested by @HyperKokichi in Issue #63
saloutos	0:894b603d32ee	1341	result = inv_icm20948_write_mems(pdev, memaddr, write_size, (unsigned char *)data_not_pg);
saloutos	0:894b603d32ee	1342	#else
saloutos	0:894b603d32ee	1343	result = inv_icm20948_write_mems(pdev, memaddr, write_size, (unsigned char *)data);
saloutos	0:894b603d32ee	1344	#endif
saloutos	0:894b603d32ee	1345	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1346	return result;
saloutos	0:894b603d32ee	1347	data += write_size;
saloutos	0:894b603d32ee	1348	size -= write_size;
saloutos	0:894b603d32ee	1349	memaddr += write_size;
saloutos	0:894b603d32ee	1350	}
saloutos	0:894b603d32ee	1351
saloutos	0:894b603d32ee	1352	// Verify DMP memory
saloutos	0:894b603d32ee	1353
saloutos	0:894b603d32ee	1354	data = data_start;
saloutos	0:894b603d32ee	1355	size = size_start;
saloutos	0:894b603d32ee	1356	memaddr = load_addr;
saloutos	0:894b603d32ee	1357	while (size > 0)
saloutos	0:894b603d32ee	1358	{
saloutos	0:894b603d32ee	1359	//write_size = min(size, INV_MAX_SERIAL_READ); // Read in chunks of INV_MAX_SERIAL_READ
saloutos	0:894b603d32ee	1360	if (size <= INV_MAX_SERIAL_READ) // Read in chunks of INV_MAX_SERIAL_READ
saloutos	0:894b603d32ee	1361	write_size = size;
saloutos	0:894b603d32ee	1362	else
saloutos	0:894b603d32ee	1363	write_size = INV_MAX_SERIAL_READ;
saloutos	0:894b603d32ee	1364	if ((memaddr & 0xff) + write_size > 0x100)
saloutos	0:894b603d32ee	1365	{
saloutos	0:894b603d32ee	1366	// Moved across a bank
saloutos	0:894b603d32ee	1367	write_size = (memaddr & 0xff) + write_size - 0x100;
saloutos	0:894b603d32ee	1368	}
saloutos	0:894b603d32ee	1369	result = inv_icm20948_read_mems(pdev, memaddr, write_size, data_cmp);
saloutos	0:894b603d32ee	1370	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1371	flag++; // Error, DMP not written correctly
saloutos	0:894b603d32ee	1372	#ifdef ICM_20948_USE_PROGMEM_FOR_DMP
saloutos	0:894b603d32ee	1373	memcpy_P(data_not_pg, data, write_size); // Suggested by @HyperKokichi in Issue #63
saloutos	0:894b603d32ee	1374	if (memcmp(data_cmp, data_not_pg, write_size))
saloutos	0:894b603d32ee	1375	#else
saloutos	0:894b603d32ee	1376	if (memcmp(data_cmp, data, write_size)) // Compare the data
saloutos	0:894b603d32ee	1377	#endif
saloutos	0:894b603d32ee	1378	return ICM_20948_Stat_DMPVerifyFail;
saloutos	0:894b603d32ee	1379	data += write_size;
saloutos	0:894b603d32ee	1380	size -= write_size;
saloutos	0:894b603d32ee	1381	memaddr += write_size;
saloutos	0:894b603d32ee	1382	}
saloutos	0:894b603d32ee	1383
saloutos	0:894b603d32ee	1384	//Enable LP_EN since we disabled it at begining of this function.
saloutos	0:894b603d32ee	1385	result = ICM_20948_low_power(pdev, true); // Put chip into low power state
saloutos	0:894b603d32ee	1386	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1387	return result;
saloutos	0:894b603d32ee	1388
saloutos	0:894b603d32ee	1389	if (!flag)
saloutos	0:894b603d32ee	1390	{
saloutos	0:894b603d32ee	1391	//Serial.println("DMP Firmware was updated successfully..");
saloutos	0:894b603d32ee	1392	pdev->_firmware_loaded = true;
saloutos	0:894b603d32ee	1393	}
saloutos	0:894b603d32ee	1394
saloutos	0:894b603d32ee	1395	return result;
saloutos	0:894b603d32ee	1396	}
saloutos	0:894b603d32ee	1397
saloutos	0:894b603d32ee	1398	ICM_20948_Status_e ICM_20948_set_dmp_start_address(ICM_20948_Device_t *pdev, unsigned short address)
saloutos	0:894b603d32ee	1399	{
saloutos	0:894b603d32ee	1400	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1401
saloutos	0:894b603d32ee	1402	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	1403	return ICM_20948_Stat_DMPNotSupported;
saloutos	0:894b603d32ee	1404
saloutos	0:894b603d32ee	1405	unsigned char start_address[2];
saloutos	0:894b603d32ee	1406
saloutos	0:894b603d32ee	1407	start_address[0] = (unsigned char)(address >> 8);
saloutos	0:894b603d32ee	1408	start_address[1] = (unsigned char)(address & 0xff);
saloutos	0:894b603d32ee	1409
saloutos	0:894b603d32ee	1410	// result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
saloutos	0:894b603d32ee	1411	// if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1412	// {
saloutos	0:894b603d32ee	1413	// return result;
saloutos	0:894b603d32ee	1414	// }
saloutos	0:894b603d32ee	1415	//
saloutos	0:894b603d32ee	1416	// result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
saloutos	0:894b603d32ee	1417	// if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1418	// {
saloutos	0:894b603d32ee	1419	// return result;
saloutos	0:894b603d32ee	1420	// }
saloutos	0:894b603d32ee	1421
saloutos	0:894b603d32ee	1422	result = ICM_20948_set_bank(pdev, 2); // Set bank 2
saloutos	0:894b603d32ee	1423	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1424	{
saloutos	0:894b603d32ee	1425	return result;
saloutos	0:894b603d32ee	1426	}
saloutos	0:894b603d32ee	1427
saloutos	0:894b603d32ee	1428	// Write the sensor control bits into memory address AGB2_REG_PRGM_START_ADDRH
saloutos	0:894b603d32ee	1429	result = ICM_20948_execute_w(pdev, AGB2_REG_PRGM_START_ADDRH, (uint8_t *)start_address, 2);
saloutos	0:894b603d32ee	1430
saloutos	0:894b603d32ee	1431	return result;
saloutos	0:894b603d32ee	1432	}
saloutos	0:894b603d32ee	1433
saloutos	0:894b603d32ee	1434	/**
saloutos	0:894b603d32ee	1435	* @brief Write data to a register in DMP memory
saloutos	0:894b603d32ee	1436	* @param[in] DMP memory address
saloutos	0:894b603d32ee	1437	* @param[in] number of byte to be written
saloutos	0:894b603d32ee	1438	* @param[out] output data from the register
saloutos	0:894b603d32ee	1439	* @return 0 if successful.
saloutos	0:894b603d32ee	1440	*/
saloutos	0:894b603d32ee	1441	ICM_20948_Status_e inv_icm20948_write_mems(ICM_20948_Device_t pdev, unsigned short reg, unsigned int length, const unsigned char data)
saloutos	0:894b603d32ee	1442	{
saloutos	0:894b603d32ee	1443	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1444	unsigned int bytesWritten = 0;
saloutos	0:894b603d32ee	1445	unsigned int thisLen;
saloutos	0:894b603d32ee	1446	unsigned char lBankSelected;
saloutos	0:894b603d32ee	1447	unsigned char lStartAddrSelected;
saloutos	0:894b603d32ee	1448
saloutos	0:894b603d32ee	1449	if (!data)
saloutos	0:894b603d32ee	1450	{
saloutos	0:894b603d32ee	1451	return ICM_20948_Stat_NoData;
saloutos	0:894b603d32ee	1452	}
saloutos	0:894b603d32ee	1453
saloutos	0:894b603d32ee	1454	result = ICM_20948_set_bank(pdev, 0); // Set bank 0
saloutos	0:894b603d32ee	1455	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1456	{
saloutos	0:894b603d32ee	1457	return result;
saloutos	0:894b603d32ee	1458	}
saloutos	0:894b603d32ee	1459
saloutos	0:894b603d32ee	1460	lBankSelected = (reg >> 8);
saloutos	0:894b603d32ee	1461
saloutos	0:894b603d32ee	1462	if (lBankSelected != pdev->_last_mems_bank)
saloutos	0:894b603d32ee	1463	{
saloutos	0:894b603d32ee	1464	pdev->_last_mems_bank = lBankSelected;
saloutos	0:894b603d32ee	1465	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
saloutos	0:894b603d32ee	1466	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1467	{
saloutos	0:894b603d32ee	1468	return result;
saloutos	0:894b603d32ee	1469	}
saloutos	0:894b603d32ee	1470	}
saloutos	0:894b603d32ee	1471
saloutos	0:894b603d32ee	1472	while (bytesWritten < length)
saloutos	0:894b603d32ee	1473	{
saloutos	0:894b603d32ee	1474	lStartAddrSelected = (reg & 0xff);
saloutos	0:894b603d32ee	1475
saloutos	0:894b603d32ee	1476	/* Sets the starting read or write address for the selected memory, inside of the selected page (see MEM_SEL Register).
saloutos	0:894b603d32ee	1477	Contents are changed after read or write of the selected memory.
saloutos	0:894b603d32ee	1478	This register must be written prior to each access to initialize the register to the proper starting address.
saloutos	0:894b603d32ee	1479	The address will auto increment during burst transactions. Two consecutive bursts without re-initializing the start address would skip one address. */
saloutos	0:894b603d32ee	1480
saloutos	0:894b603d32ee	1481	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_START_ADDR, &lStartAddrSelected, 1);
saloutos	0:894b603d32ee	1482	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1483	{
saloutos	0:894b603d32ee	1484	return result;
saloutos	0:894b603d32ee	1485	}
saloutos	0:894b603d32ee	1486
saloutos	0:894b603d32ee	1487	if (length - bytesWritten <= INV_MAX_SERIAL_WRITE)
saloutos	0:894b603d32ee	1488	thisLen = length - bytesWritten;
saloutos	0:894b603d32ee	1489	else
saloutos	0:894b603d32ee	1490	thisLen = INV_MAX_SERIAL_WRITE;
saloutos	0:894b603d32ee	1491
saloutos	0:894b603d32ee	1492	/* Write data */
saloutos	0:894b603d32ee	1493
saloutos	0:894b603d32ee	1494	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_R_W, (uint8_t *)&data[bytesWritten], thisLen);
saloutos	0:894b603d32ee	1495	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1496	{
saloutos	0:894b603d32ee	1497	return result;
saloutos	0:894b603d32ee	1498	}
saloutos	0:894b603d32ee	1499
saloutos	0:894b603d32ee	1500	bytesWritten += thisLen;
saloutos	0:894b603d32ee	1501	reg += thisLen;
saloutos	0:894b603d32ee	1502	}
saloutos	0:894b603d32ee	1503
saloutos	0:894b603d32ee	1504	return result;
saloutos	0:894b603d32ee	1505	}
saloutos	0:894b603d32ee	1506
saloutos	0:894b603d32ee	1507	/**
saloutos	0:894b603d32ee	1508	* @brief Read data from a register in DMP memory
saloutos	0:894b603d32ee	1509	* @param[in] DMP memory address
saloutos	0:894b603d32ee	1510	* @param[in] number of byte to be read
saloutos	0:894b603d32ee	1511	* @param[in] input data from the register
saloutos	0:894b603d32ee	1512	* @return 0 if successful.
saloutos	0:894b603d32ee	1513	*/
saloutos	0:894b603d32ee	1514	ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t pdev, unsigned short reg, unsigned int length, unsigned char data)
saloutos	0:894b603d32ee	1515	{
saloutos	0:894b603d32ee	1516	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1517	unsigned int bytesRead = 0;
saloutos	0:894b603d32ee	1518	unsigned int thisLen;
saloutos	0:894b603d32ee	1519	unsigned char lBankSelected;
saloutos	0:894b603d32ee	1520	unsigned char lStartAddrSelected;
saloutos	0:894b603d32ee	1521
saloutos	0:894b603d32ee	1522	if (!data)
saloutos	0:894b603d32ee	1523	{
saloutos	0:894b603d32ee	1524	return ICM_20948_Stat_NoData;
saloutos	0:894b603d32ee	1525	}
saloutos	0:894b603d32ee	1526
saloutos	0:894b603d32ee	1527	result = ICM_20948_set_bank(pdev, 0); // Set bank 0
saloutos	0:894b603d32ee	1528	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1529	{
saloutos	0:894b603d32ee	1530	return result;
saloutos	0:894b603d32ee	1531	}
saloutos	0:894b603d32ee	1532
saloutos	0:894b603d32ee	1533	lBankSelected = (reg >> 8);
saloutos	0:894b603d32ee	1534
saloutos	0:894b603d32ee	1535	if (lBankSelected != pdev->_last_mems_bank)
saloutos	0:894b603d32ee	1536	{
saloutos	0:894b603d32ee	1537	pdev->_last_mems_bank = lBankSelected;
saloutos	0:894b603d32ee	1538	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
saloutos	0:894b603d32ee	1539	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1540	{
saloutos	0:894b603d32ee	1541	return result;
saloutos	0:894b603d32ee	1542	}
saloutos	0:894b603d32ee	1543	}
saloutos	0:894b603d32ee	1544
saloutos	0:894b603d32ee	1545	while (bytesRead < length)
saloutos	0:894b603d32ee	1546	{
saloutos	0:894b603d32ee	1547	lStartAddrSelected = (reg & 0xff);
saloutos	0:894b603d32ee	1548
saloutos	0:894b603d32ee	1549	/* Sets the starting read or write address for the selected memory, inside of the selected page (see MEM_SEL Register).
saloutos	0:894b603d32ee	1550	Contents are changed after read or write of the selected memory.
saloutos	0:894b603d32ee	1551	This register must be written prior to each access to initialize the register to the proper starting address.
saloutos	0:894b603d32ee	1552	The address will auto increment during burst transactions. Two consecutive bursts without re-initializing the start address would skip one address. */
saloutos	0:894b603d32ee	1553
saloutos	0:894b603d32ee	1554	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_START_ADDR, &lStartAddrSelected, 1);
saloutos	0:894b603d32ee	1555	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1556	{
saloutos	0:894b603d32ee	1557	return result;
saloutos	0:894b603d32ee	1558	}
saloutos	0:894b603d32ee	1559
saloutos	0:894b603d32ee	1560	if (length - bytesRead <= INV_MAX_SERIAL_READ)
saloutos	0:894b603d32ee	1561	thisLen = length - bytesRead;
saloutos	0:894b603d32ee	1562	else
saloutos	0:894b603d32ee	1563	thisLen = INV_MAX_SERIAL_READ;
saloutos	0:894b603d32ee	1564
saloutos	0:894b603d32ee	1565	/* Read data */
saloutos	0:894b603d32ee	1566
saloutos	0:894b603d32ee	1567	result = ICM_20948_execute_r(pdev, AGB0_REG_MEM_R_W, &data[bytesRead], thisLen);
saloutos	0:894b603d32ee	1568	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1569	{
saloutos	0:894b603d32ee	1570	return result;
saloutos	0:894b603d32ee	1571	}
saloutos	0:894b603d32ee	1572
saloutos	0:894b603d32ee	1573	bytesRead += thisLen;
saloutos	0:894b603d32ee	1574	reg += thisLen;
saloutos	0:894b603d32ee	1575	}
saloutos	0:894b603d32ee	1576
saloutos	0:894b603d32ee	1577	return result;
saloutos	0:894b603d32ee	1578	}
saloutos	0:894b603d32ee	1579
saloutos	0:894b603d32ee	1580	ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev, enum DMP_ODR_Registers odr_reg, uint16_t interval)
saloutos	0:894b603d32ee	1581	{
saloutos	0:894b603d32ee	1582	// Set the ODR registers and clear the ODR counter
saloutos	0:894b603d32ee	1583
saloutos	0:894b603d32ee	1584	// In order to set an ODR for a given sensor data, write 2-byte value to DMP using key defined above for a particular sensor.
saloutos	0:894b603d32ee	1585	// Setting value can be calculated as follows:
saloutos	0:894b603d32ee	1586	// Value = (DMP running rate (225Hz) / ODR ) - 1
saloutos	0:894b603d32ee	1587	// E.g. For a 25Hz ODR rate, value= (225/25) -1 = 8.
saloutos	0:894b603d32ee	1588
saloutos	0:894b603d32ee	1589	// During run-time, if an ODR is changed, the corresponding rate counter must be reset.
saloutos	0:894b603d32ee	1590	// To reset, write 2-byte {0,0} to DMP using keys below for a particular sensor:
saloutos	0:894b603d32ee	1591
saloutos	0:894b603d32ee	1592	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1593	ICM_20948_Status_e result2 = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1594
saloutos	0:894b603d32ee	1595	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	1596	return ICM_20948_Stat_DMPNotSupported;
saloutos	0:894b603d32ee	1597
saloutos	0:894b603d32ee	1598	unsigned char odr_reg_val[2];
saloutos	0:894b603d32ee	1599	odr_reg_val[0] = (unsigned char)(interval >> 8);
saloutos	0:894b603d32ee	1600	odr_reg_val[1] = (unsigned char)(interval & 0xff);
saloutos	0:894b603d32ee	1601
saloutos	0:894b603d32ee	1602	unsigned char odr_count_zero[2] = {0x00, 0x00};
saloutos	0:894b603d32ee	1603
saloutos	0:894b603d32ee	1604	result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
saloutos	0:894b603d32ee	1605	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1606	{
saloutos	0:894b603d32ee	1607	return result;
saloutos	0:894b603d32ee	1608	}
saloutos	0:894b603d32ee	1609
saloutos	0:894b603d32ee	1610	result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
saloutos	0:894b603d32ee	1611	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1612	{
saloutos	0:894b603d32ee	1613	return result;
saloutos	0:894b603d32ee	1614	}
saloutos	0:894b603d32ee	1615
saloutos	0:894b603d32ee	1616	switch (odr_reg)
saloutos	0:894b603d32ee	1617	{
saloutos	0:894b603d32ee	1618	case DMP_ODR_Reg_Cpass_Calibr:
saloutos	0:894b603d32ee	1619	{
saloutos	0:894b603d32ee	1620	result = inv_icm20948_write_mems(pdev, ODR_CPASS_CALIBR, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1621	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_CPASS_CALIBR, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1622	}
saloutos	0:894b603d32ee	1623	break;
saloutos	0:894b603d32ee	1624	case DMP_ODR_Reg_Gyro_Calibr:
saloutos	0:894b603d32ee	1625	{
saloutos	0:894b603d32ee	1626	result = inv_icm20948_write_mems(pdev, ODR_GYRO_CALIBR, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1627	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_GYRO_CALIBR, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1628	}
saloutos	0:894b603d32ee	1629	break;
saloutos	0:894b603d32ee	1630	case DMP_ODR_Reg_Pressure:
saloutos	0:894b603d32ee	1631	{
saloutos	0:894b603d32ee	1632	result = inv_icm20948_write_mems(pdev, ODR_PRESSURE, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1633	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_PRESSURE, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1634	}
saloutos	0:894b603d32ee	1635	break;
saloutos	0:894b603d32ee	1636	case DMP_ODR_Reg_Geomag:
saloutos	0:894b603d32ee	1637	{
saloutos	0:894b603d32ee	1638	result = inv_icm20948_write_mems(pdev, ODR_GEOMAG, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1639	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_GEOMAG, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1640	}
saloutos	0:894b603d32ee	1641	break;
saloutos	0:894b603d32ee	1642	case DMP_ODR_Reg_PQuat6:
saloutos	0:894b603d32ee	1643	{
saloutos	0:894b603d32ee	1644	result = inv_icm20948_write_mems(pdev, ODR_PQUAT6, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1645	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_PQUAT6, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1646	}
saloutos	0:894b603d32ee	1647	break;
saloutos	0:894b603d32ee	1648	case DMP_ODR_Reg_Quat9:
saloutos	0:894b603d32ee	1649	{
saloutos	0:894b603d32ee	1650	result = inv_icm20948_write_mems(pdev, ODR_QUAT9, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1651	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_QUAT9, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1652	}
saloutos	0:894b603d32ee	1653	break;
saloutos	0:894b603d32ee	1654	case DMP_ODR_Reg_Quat6:
saloutos	0:894b603d32ee	1655	{
saloutos	0:894b603d32ee	1656	result = inv_icm20948_write_mems(pdev, ODR_QUAT6, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1657	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_QUAT6, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1658	}
saloutos	0:894b603d32ee	1659	break;
saloutos	0:894b603d32ee	1660	case DMP_ODR_Reg_ALS:
saloutos	0:894b603d32ee	1661	{
saloutos	0:894b603d32ee	1662	result = inv_icm20948_write_mems(pdev, ODR_ALS, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1663	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_ALS, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1664	}
saloutos	0:894b603d32ee	1665	break;
saloutos	0:894b603d32ee	1666	case DMP_ODR_Reg_Cpass:
saloutos	0:894b603d32ee	1667	{
saloutos	0:894b603d32ee	1668	result = inv_icm20948_write_mems(pdev, ODR_CPASS, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1669	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_CPASS, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1670	}
saloutos	0:894b603d32ee	1671	break;
saloutos	0:894b603d32ee	1672	case DMP_ODR_Reg_Gyro:
saloutos	0:894b603d32ee	1673	{
saloutos	0:894b603d32ee	1674	result = inv_icm20948_write_mems(pdev, ODR_GYRO, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1675	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_GYRO, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1676	}
saloutos	0:894b603d32ee	1677	break;
saloutos	0:894b603d32ee	1678	case DMP_ODR_Reg_Accel:
saloutos	0:894b603d32ee	1679	{
saloutos	0:894b603d32ee	1680	result = inv_icm20948_write_mems(pdev, ODR_ACCEL, 2, (const unsigned char *)&odr_reg_val);
saloutos	0:894b603d32ee	1681	result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_ACCEL, 2, (const unsigned char *)&odr_count_zero);
saloutos	0:894b603d32ee	1682	}
saloutos	0:894b603d32ee	1683	break;
saloutos	0:894b603d32ee	1684	default:
saloutos	0:894b603d32ee	1685	result = ICM_20948_Stat_InvalDMPRegister;
saloutos	0:894b603d32ee	1686	break;
saloutos	0:894b603d32ee	1687	}
saloutos	0:894b603d32ee	1688
saloutos	0:894b603d32ee	1689	result = ICM_20948_low_power(pdev, true); // Put chip into low power state
saloutos	0:894b603d32ee	1690	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1691	return result;
saloutos	0:894b603d32ee	1692
saloutos	0:894b603d32ee	1693	if (result2 > result)
saloutos	0:894b603d32ee	1694	result = result2; // Return the highest error
saloutos	0:894b603d32ee	1695
saloutos	0:894b603d32ee	1696	return result;
saloutos	0:894b603d32ee	1697	}
saloutos	0:894b603d32ee	1698
saloutos	0:894b603d32ee	1699	ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, int state)
saloutos	0:894b603d32ee	1700	{
saloutos	0:894b603d32ee	1701	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1702
saloutos	0:894b603d32ee	1703	uint16_t inv_event_control = 0; // Use this to store the value for MOTION_EVENT_CTL
saloutos	0:894b603d32ee	1704	uint16_t data_rdy_status = 0; // Use this to store the value for DATA_RDY_STATUS
saloutos	0:894b603d32ee	1705
saloutos	0:894b603d32ee	1706	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	1707	return ICM_20948_Stat_DMPNotSupported; // Bail if DMP is not supported
saloutos	0:894b603d32ee	1708
saloutos	0:894b603d32ee	1709	uint8_t androidSensor = sensor_type_2_android_sensor(sensor); // Convert sensor from enum inv_icm20948_sensor to Android numbering
saloutos	0:894b603d32ee	1710
saloutos	0:894b603d32ee	1711	if (androidSensor >= ANDROID_SENSOR_NUM_MAX)
saloutos	0:894b603d32ee	1712	return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported (TO DO: Support B2S etc)
saloutos	0:894b603d32ee	1713
saloutos	0:894b603d32ee	1714	// Convert the Android sensor into a bit mask for DATA_OUT_CTL1
saloutos	0:894b603d32ee	1715	uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
saloutos	0:894b603d32ee	1716	if (delta == 0xFFFF)
saloutos	0:894b603d32ee	1717	return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
saloutos	0:894b603d32ee	1718
saloutos	0:894b603d32ee	1719	// Convert the Android sensor number into a bitmask and set or clear that bit in _enabled_Android_0 / _enabled_Android_1
saloutos	0:894b603d32ee	1720	unsigned long androidSensorAsBitMask;
saloutos	0:894b603d32ee	1721	if (androidSensor < 32) // Sensors 0-31
saloutos	0:894b603d32ee	1722	{
saloutos	0:894b603d32ee	1723	androidSensorAsBitMask = 1L << androidSensor;
saloutos	0:894b603d32ee	1724	if (state == 0) // Should we disable the sensor?
saloutos	0:894b603d32ee	1725	{
saloutos	0:894b603d32ee	1726	pdev->_enabled_Android_0 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor
saloutos	0:894b603d32ee	1727	}
saloutos	0:894b603d32ee	1728	else
saloutos	0:894b603d32ee	1729	{
saloutos	0:894b603d32ee	1730	pdev->_enabled_Android_0 \|= androidSensorAsBitMask; // Set the bit to enable the sensor
saloutos	0:894b603d32ee	1731	}
saloutos	0:894b603d32ee	1732	}
saloutos	0:894b603d32ee	1733	else // Sensors 32-
saloutos	0:894b603d32ee	1734	{
saloutos	0:894b603d32ee	1735	androidSensorAsBitMask = 1L << (androidSensor - 32);
saloutos	0:894b603d32ee	1736	if (state == 0) // Should we disable the sensor?
saloutos	0:894b603d32ee	1737	{
saloutos	0:894b603d32ee	1738	pdev->_enabled_Android_1 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor
saloutos	0:894b603d32ee	1739	}
saloutos	0:894b603d32ee	1740	else
saloutos	0:894b603d32ee	1741	{
saloutos	0:894b603d32ee	1742	pdev->_enabled_Android_1 \|= androidSensorAsBitMask; // Set the bit to enable the sensor
saloutos	0:894b603d32ee	1743	}
saloutos	0:894b603d32ee	1744	}
saloutos	0:894b603d32ee	1745
saloutos	0:894b603d32ee	1746	// Now we know androidSensor is valid, reconstruct the value for DATA_OUT_CTL1 from _enabled_Android_0 and _enabled_Android_0
saloutos	0:894b603d32ee	1747	delta = 0; // Clear delta
saloutos	0:894b603d32ee	1748	for (int i = 0; i < 32; i++)
saloutos	0:894b603d32ee	1749	{
saloutos	0:894b603d32ee	1750	androidSensorAsBitMask = 1L << i;
saloutos	0:894b603d32ee	1751	if ((pdev->_enabled_Android_0 & androidSensorAsBitMask) > 0) // Check if the Android sensor (0-31) is enabled
saloutos	0:894b603d32ee	1752	{
saloutos	0:894b603d32ee	1753	delta \|= inv_androidSensor_to_control_bits[i]; // If it is, or the required bits into delta
saloutos	0:894b603d32ee	1754	}
saloutos	0:894b603d32ee	1755	if ((pdev->_enabled_Android_1 & androidSensorAsBitMask) > 0) // Check if the Android sensor (32-) is enabled
saloutos	0:894b603d32ee	1756	{
saloutos	0:894b603d32ee	1757	delta \|= inv_androidSensor_to_control_bits[i + 32]; // If it is, or the required bits into delta
saloutos	0:894b603d32ee	1758	}
saloutos	0:894b603d32ee	1759	// Also check which bits need to be set in the Data Ready Status and Motion Event Control registers
saloutos	0:894b603d32ee	1760	// Compare to INV_NEEDS_ACCEL_MASK, INV_NEEDS_GYRO_MASK and INV_NEEDS_COMPASS_MASK
saloutos	0:894b603d32ee	1761	if (((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK) > 0) \|\| ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK1) > 0))
saloutos	0:894b603d32ee	1762	{
saloutos	0:894b603d32ee	1763	data_rdy_status \|= DMP_Data_ready_Accel;
saloutos	0:894b603d32ee	1764	inv_event_control \|= DMP_Motion_Event_Control_Accel_Calibr;
saloutos	0:894b603d32ee	1765	}
saloutos	0:894b603d32ee	1766	if (((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK) > 0) \|\| ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK1) > 0))
saloutos	0:894b603d32ee	1767	{
saloutos	0:894b603d32ee	1768	data_rdy_status \|= DMP_Data_ready_Gyro;
saloutos	0:894b603d32ee	1769	inv_event_control \|= DMP_Motion_Event_Control_Gyro_Calibr;
saloutos	0:894b603d32ee	1770	}
saloutos	0:894b603d32ee	1771	if (((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK) > 0) \|\| ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK1) > 0))
saloutos	0:894b603d32ee	1772	{
saloutos	0:894b603d32ee	1773	data_rdy_status \|= DMP_Data_ready_Secondary_Compass;
saloutos	0:894b603d32ee	1774	inv_event_control \|= DMP_Motion_Event_Control_Compass_Calibr;
saloutos	0:894b603d32ee	1775	}
saloutos	0:894b603d32ee	1776	}
saloutos	0:894b603d32ee	1777
saloutos	0:894b603d32ee	1778	result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
saloutos	0:894b603d32ee	1779	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1780	{
saloutos	0:894b603d32ee	1781	return result;
saloutos	0:894b603d32ee	1782	}
saloutos	0:894b603d32ee	1783
saloutos	0:894b603d32ee	1784	result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
saloutos	0:894b603d32ee	1785	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1786	{
saloutos	0:894b603d32ee	1787	return result;
saloutos	0:894b603d32ee	1788	}
saloutos	0:894b603d32ee	1789
saloutos	0:894b603d32ee	1790	// Check if Accel, Gyro/Gyro_Calibr or Compass_Calibr/Quat9/GeoMag/Compass are to be enabled. If they are then we need to request the accuracy data via header2.
saloutos	0:894b603d32ee	1791	uint16_t delta2 = 0;
saloutos	0:894b603d32ee	1792	if ((delta & DMP_Data_Output_Control_1_Accel) > 0)
saloutos	0:894b603d32ee	1793	{
saloutos	0:894b603d32ee	1794	delta2 \|= DMP_Data_Output_Control_2_Accel_Accuracy;
saloutos	0:894b603d32ee	1795	}
saloutos	0:894b603d32ee	1796	if (((delta & DMP_Data_Output_Control_1_Gyro_Calibr) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Gyro) > 0))
saloutos	0:894b603d32ee	1797	{
saloutos	0:894b603d32ee	1798	delta2 \|= DMP_Data_Output_Control_2_Gyro_Accuracy;
saloutos	0:894b603d32ee	1799	}
saloutos	0:894b603d32ee	1800	if (((delta & DMP_Data_Output_Control_1_Compass_Calibr) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Compass) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Quat9) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Geomag) > 0))
saloutos	0:894b603d32ee	1801	{
saloutos	0:894b603d32ee	1802	delta2 \|= DMP_Data_Output_Control_2_Compass_Accuracy;
saloutos	0:894b603d32ee	1803	}
saloutos	0:894b603d32ee	1804	// TO DO: Add DMP_Data_Output_Control_2_Pickup etc. if required
saloutos	0:894b603d32ee	1805
saloutos	0:894b603d32ee	1806	// Write the sensor control bits into memory address DATA_OUT_CTL1
saloutos	0:894b603d32ee	1807	unsigned char data_output_control_reg[2];
saloutos	0:894b603d32ee	1808	data_output_control_reg[0] = (unsigned char)(delta >> 8);
saloutos	0:894b603d32ee	1809	data_output_control_reg[1] = (unsigned char)(delta & 0xff);
saloutos	0:894b603d32ee	1810	pdev->_dataOutCtl1 = delta; // Diagnostics
saloutos	0:894b603d32ee	1811	result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL1, 2, (const unsigned char *)&data_output_control_reg);
saloutos	0:894b603d32ee	1812	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1813	{
saloutos	0:894b603d32ee	1814	return result;
saloutos	0:894b603d32ee	1815	}
saloutos	0:894b603d32ee	1816
saloutos	0:894b603d32ee	1817	// Write the 'header2' sensor control bits into memory address DATA_OUT_CTL2
saloutos	0:894b603d32ee	1818	data_output_control_reg[0] = (unsigned char)(delta2 >> 8);
saloutos	0:894b603d32ee	1819	data_output_control_reg[1] = (unsigned char)(delta2 & 0xff);
saloutos	0:894b603d32ee	1820	pdev->_dataOutCtl2 = delta2; // Diagnostics
saloutos	0:894b603d32ee	1821	result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL2, 2, (const unsigned char *)&data_output_control_reg);
saloutos	0:894b603d32ee	1822	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1823	{
saloutos	0:894b603d32ee	1824	return result;
saloutos	0:894b603d32ee	1825	}
saloutos	0:894b603d32ee	1826
saloutos	0:894b603d32ee	1827	// Set the DATA_RDY_STATUS register
saloutos	0:894b603d32ee	1828	data_output_control_reg[0] = (unsigned char)(data_rdy_status >> 8);
saloutos	0:894b603d32ee	1829	data_output_control_reg[1] = (unsigned char)(data_rdy_status & 0xff);
saloutos	0:894b603d32ee	1830	pdev->_dataRdyStatus = data_rdy_status; // Diagnostics
saloutos	0:894b603d32ee	1831	result = inv_icm20948_write_mems(pdev, DATA_RDY_STATUS, 2, (const unsigned char *)&data_output_control_reg);
saloutos	0:894b603d32ee	1832	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1833	{
saloutos	0:894b603d32ee	1834	return result;
saloutos	0:894b603d32ee	1835	}
saloutos	0:894b603d32ee	1836
saloutos	0:894b603d32ee	1837	// Check which extra bits need to be set in the Motion Event Control register
saloutos	0:894b603d32ee	1838	if ((delta & DMP_Data_Output_Control_1_Quat9) > 0)
saloutos	0:894b603d32ee	1839	{
saloutos	0:894b603d32ee	1840	inv_event_control \|= DMP_Motion_Event_Control_9axis;
saloutos	0:894b603d32ee	1841	}
saloutos	0:894b603d32ee	1842	if (((delta & DMP_Data_Output_Control_1_Step_Detector) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Step_Ind_0) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Step_Ind_1) > 0) \|\| ((delta & DMP_Data_Output_Control_1_Step_Ind_2) > 0))
saloutos	0:894b603d32ee	1843	{
saloutos	0:894b603d32ee	1844	inv_event_control \|= DMP_Motion_Event_Control_Pedometer_Interrupt;
saloutos	0:894b603d32ee	1845	}
saloutos	0:894b603d32ee	1846	if ((delta & DMP_Data_Output_Control_1_Geomag) > 0)
saloutos	0:894b603d32ee	1847	{
saloutos	0:894b603d32ee	1848	inv_event_control \|= DMP_Motion_Event_Control_Geomag;
saloutos	0:894b603d32ee	1849	}
saloutos	0:894b603d32ee	1850
saloutos	0:894b603d32ee	1851	// Set the MOTION_EVENT_CTL register
saloutos	0:894b603d32ee	1852	data_output_control_reg[0] = (unsigned char)(inv_event_control >> 8);
saloutos	0:894b603d32ee	1853	data_output_control_reg[1] = (unsigned char)(inv_event_control & 0xff);
saloutos	0:894b603d32ee	1854	pdev->_motionEventCtl = inv_event_control; // Diagnostics
saloutos	0:894b603d32ee	1855	result = inv_icm20948_write_mems(pdev, MOTION_EVENT_CTL, 2, (const unsigned char *)&data_output_control_reg);
saloutos	0:894b603d32ee	1856	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1857	{
saloutos	0:894b603d32ee	1858	return result;
saloutos	0:894b603d32ee	1859	}
saloutos	0:894b603d32ee	1860
saloutos	0:894b603d32ee	1861	result = ICM_20948_low_power(pdev, true); // Put chip into low power state
saloutos	0:894b603d32ee	1862	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1863	return result;
saloutos	0:894b603d32ee	1864
saloutos	0:894b603d32ee	1865	return result;
saloutos	0:894b603d32ee	1866	}
saloutos	0:894b603d32ee	1867
saloutos	0:894b603d32ee	1868	ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, int state)
saloutos	0:894b603d32ee	1869	{
saloutos	0:894b603d32ee	1870	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1871
saloutos	0:894b603d32ee	1872	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	1873	return ICM_20948_Stat_DMPNotSupported; // Bail if DMP is not supported
saloutos	0:894b603d32ee	1874
saloutos	0:894b603d32ee	1875	uint8_t androidSensor = sensor_type_2_android_sensor(sensor); // Convert sensor from enum inv_icm20948_sensor to Android numbering
saloutos	0:894b603d32ee	1876
saloutos	0:894b603d32ee	1877	if (androidSensor > ANDROID_SENSOR_NUM_MAX)
saloutos	0:894b603d32ee	1878	return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
saloutos	0:894b603d32ee	1879
saloutos	0:894b603d32ee	1880	// Convert the Android sensor into a bit mask for DATA_OUT_CTL1
saloutos	0:894b603d32ee	1881	uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
saloutos	0:894b603d32ee	1882	if (delta == 0xFFFF)
saloutos	0:894b603d32ee	1883	return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
saloutos	0:894b603d32ee	1884
saloutos	0:894b603d32ee	1885	// Convert the Android sensor number into a bitmask and set or clear that bit in _enabled_Android_intr_0 / _enabled_Android_intr_1
saloutos	0:894b603d32ee	1886	unsigned long androidSensorAsBitMask;
saloutos	0:894b603d32ee	1887	if (androidSensor < 32) // Sensors 0-31
saloutos	0:894b603d32ee	1888	{
saloutos	0:894b603d32ee	1889	androidSensorAsBitMask = 1L << androidSensor;
saloutos	0:894b603d32ee	1890	if (state == 0) // Should we disable the sensor interrupt?
saloutos	0:894b603d32ee	1891	{
saloutos	0:894b603d32ee	1892	pdev->_enabled_Android_intr_0 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor interrupt
saloutos	0:894b603d32ee	1893	}
saloutos	0:894b603d32ee	1894	else
saloutos	0:894b603d32ee	1895	{
saloutos	0:894b603d32ee	1896	pdev->_enabled_Android_intr_0 \|= androidSensorAsBitMask; // Set the bit to enable the sensor interrupt
saloutos	0:894b603d32ee	1897	}
saloutos	0:894b603d32ee	1898	}
saloutos	0:894b603d32ee	1899	else // Sensors 32-
saloutos	0:894b603d32ee	1900	{
saloutos	0:894b603d32ee	1901	androidSensorAsBitMask = 1L << (androidSensor - 32);
saloutos	0:894b603d32ee	1902	if (state == 0) // Should we disable the sensor?
saloutos	0:894b603d32ee	1903	{
saloutos	0:894b603d32ee	1904	pdev->_enabled_Android_intr_1 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor interrupt
saloutos	0:894b603d32ee	1905	}
saloutos	0:894b603d32ee	1906	else
saloutos	0:894b603d32ee	1907	{
saloutos	0:894b603d32ee	1908	pdev->_enabled_Android_intr_1 \|= androidSensorAsBitMask; // Set the bit to enable the sensor interrupt
saloutos	0:894b603d32ee	1909	}
saloutos	0:894b603d32ee	1910	}
saloutos	0:894b603d32ee	1911
saloutos	0:894b603d32ee	1912	// Now we know androidSensor is valid, reconstruct the value for DATA_INTR_CTL from _enabled_Android_intr_0 and _enabled_Android_intr_0
saloutos	0:894b603d32ee	1913	delta = 0; // Clear delta
saloutos	0:894b603d32ee	1914	for (int i = 0; i < 32; i++)
saloutos	0:894b603d32ee	1915	{
saloutos	0:894b603d32ee	1916	androidSensorAsBitMask = 1L << i;
saloutos	0:894b603d32ee	1917	if ((pdev->_enabled_Android_intr_0 & androidSensorAsBitMask) > 0) // Check if the Android sensor (0-31) interrupt is enabled
saloutos	0:894b603d32ee	1918	{
saloutos	0:894b603d32ee	1919	delta \|= inv_androidSensor_to_control_bits[i]; // If it is, or the required bits into delta
saloutos	0:894b603d32ee	1920	}
saloutos	0:894b603d32ee	1921	if ((pdev->_enabled_Android_intr_1 & androidSensorAsBitMask) > 0) // Check if the Android sensor (32-) interrupt is enabled
saloutos	0:894b603d32ee	1922	{
saloutos	0:894b603d32ee	1923	delta \|= inv_androidSensor_to_control_bits[i + 32]; // If it is, or the required bits into delta
saloutos	0:894b603d32ee	1924	}
saloutos	0:894b603d32ee	1925	}
saloutos	0:894b603d32ee	1926
saloutos	0:894b603d32ee	1927	result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
saloutos	0:894b603d32ee	1928	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1929	{
saloutos	0:894b603d32ee	1930	return result;
saloutos	0:894b603d32ee	1931	}
saloutos	0:894b603d32ee	1932
saloutos	0:894b603d32ee	1933	result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
saloutos	0:894b603d32ee	1934	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1935	{
saloutos	0:894b603d32ee	1936	return result;
saloutos	0:894b603d32ee	1937	}
saloutos	0:894b603d32ee	1938
saloutos	0:894b603d32ee	1939	unsigned char data_intr_ctl[2];
saloutos	0:894b603d32ee	1940
saloutos	0:894b603d32ee	1941	data_intr_ctl[0] = (unsigned char)(delta >> 8);
saloutos	0:894b603d32ee	1942	data_intr_ctl[1] = (unsigned char)(delta & 0xff);
saloutos	0:894b603d32ee	1943	pdev->_dataIntrCtl = delta; // Diagnostics
saloutos	0:894b603d32ee	1944
saloutos	0:894b603d32ee	1945	// Write the interrupt control bits into memory address DATA_INTR_CTL
saloutos	0:894b603d32ee	1946	result = inv_icm20948_write_mems(pdev, DATA_INTR_CTL, 2, (const unsigned char *)&data_intr_ctl);
saloutos	0:894b603d32ee	1947
saloutos	0:894b603d32ee	1948	result = ICM_20948_low_power(pdev, true); // Put chip into low power state
saloutos	0:894b603d32ee	1949	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1950	return result;
saloutos	0:894b603d32ee	1951
saloutos	0:894b603d32ee	1952	return result;
saloutos	0:894b603d32ee	1953	}
saloutos	0:894b603d32ee	1954
saloutos	0:894b603d32ee	1955	ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t pdev, icm_20948_DMP_data_t data)
saloutos	0:894b603d32ee	1956	{
saloutos	0:894b603d32ee	1957	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	1958	uint8_t fifoBytes[icm_20948_DMP_Maximum_Bytes]; // Interim storage for the FIFO data
saloutos	0:894b603d32ee	1959
saloutos	0:894b603d32ee	1960	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	1961	return ICM_20948_Stat_DMPNotSupported;
saloutos	0:894b603d32ee	1962
saloutos	0:894b603d32ee	1963	// Check how much data is in the FIFO
saloutos	0:894b603d32ee	1964	uint16_t fifo_count;
saloutos	0:894b603d32ee	1965	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	1966	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1967	return result;
saloutos	0:894b603d32ee	1968
saloutos	0:894b603d32ee	1969	if (fifo_count < icm_20948_DMP_Header_Bytes) // Has a 2-byte header arrived?
saloutos	0:894b603d32ee	1970	return ICM_20948_Stat_FIFONoDataAvail; // Bail if no header is available
saloutos	0:894b603d32ee	1971
saloutos	0:894b603d32ee	1972	// Read the header (2 bytes)
saloutos	0:894b603d32ee	1973	data->header = 0; // Clear the existing header
saloutos	0:894b603d32ee	1974	uint16_t aShort = 0;
saloutos	0:894b603d32ee	1975	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Header_Bytes);
saloutos	0:894b603d32ee	1976	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1977	return result;
saloutos	0:894b603d32ee	1978	for (int i = 0; i < icm_20948_DMP_Header_Bytes; i++)
saloutos	0:894b603d32ee	1979	{
saloutos	0:894b603d32ee	1980	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	1981	}
saloutos	0:894b603d32ee	1982	data->header = aShort; // Store the header in data->header
saloutos	0:894b603d32ee	1983	fifo_count -= icm_20948_DMP_Header_Bytes; // Decrement the count
saloutos	0:894b603d32ee	1984
saloutos	0:894b603d32ee	1985	// If the header indicates a header2 is present then read that now
saloutos	0:894b603d32ee	1986	data->header2 = 0; // Clear the existing header2
saloutos	0:894b603d32ee	1987	if ((data->header & DMP_header_bitmap_Header2) > 0) // If the header2 bit is set
saloutos	0:894b603d32ee	1988	{
saloutos	0:894b603d32ee	1989	if (fifo_count < icm_20948_DMP_Header2_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	1990	{
saloutos	0:894b603d32ee	1991	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	1992	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	1993	return result;
saloutos	0:894b603d32ee	1994	}
saloutos	0:894b603d32ee	1995	if (fifo_count < icm_20948_DMP_Header2_Bytes)
saloutos	0:894b603d32ee	1996	return ICM_20948_Stat_FIFOIncompleteData; // Bail if no header2 is available
saloutos	0:894b603d32ee	1997	// Read the header (2 bytes)
saloutos	0:894b603d32ee	1998	aShort = 0;
saloutos	0:894b603d32ee	1999	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Header2_Bytes);
saloutos	0:894b603d32ee	2000	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2001	return result;
saloutos	0:894b603d32ee	2002	for (int i = 0; i < icm_20948_DMP_Header2_Bytes; i++)
saloutos	0:894b603d32ee	2003	{
saloutos	0:894b603d32ee	2004	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2005	}
saloutos	0:894b603d32ee	2006	data->header2 = aShort; // Store the header2 in data->header2
saloutos	0:894b603d32ee	2007	fifo_count -= icm_20948_DMP_Header2_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2008	}
saloutos	0:894b603d32ee	2009
saloutos	0:894b603d32ee	2010	if ((data->header & DMP_header_bitmap_Accel) > 0) // case DMP_header_bitmap_Accel:
saloutos	0:894b603d32ee	2011	{
saloutos	0:894b603d32ee	2012	if (fifo_count < icm_20948_DMP_Raw_Accel_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2013	{
saloutos	0:894b603d32ee	2014	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2015	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2016	return result;
saloutos	0:894b603d32ee	2017	}
saloutos	0:894b603d32ee	2018	if (fifo_count < icm_20948_DMP_Raw_Accel_Bytes)
saloutos	0:894b603d32ee	2019	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2020	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Raw_Accel_Bytes);
saloutos	0:894b603d32ee	2021	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2022	return result;
saloutos	0:894b603d32ee	2023	for (int i = 0; i < icm_20948_DMP_Raw_Accel_Bytes; i++)
saloutos	0:894b603d32ee	2024	{
saloutos	0:894b603d32ee	2025	data->Raw_Accel.Bytes[DMP_PQuat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2026	}
saloutos	0:894b603d32ee	2027	fifo_count -= icm_20948_DMP_Raw_Accel_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2028	}
saloutos	0:894b603d32ee	2029
saloutos	0:894b603d32ee	2030	if ((data->header & DMP_header_bitmap_Gyro) > 0) // case DMP_header_bitmap_Gyro:
saloutos	0:894b603d32ee	2031	{
saloutos	0:894b603d32ee	2032	if (fifo_count < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes)) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2033	{
saloutos	0:894b603d32ee	2034	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2035	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2036	return result;
saloutos	0:894b603d32ee	2037	}
saloutos	0:894b603d32ee	2038	if (fifo_count < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes))
saloutos	0:894b603d32ee	2039	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2040	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes));
saloutos	0:894b603d32ee	2041	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2042	return result;
saloutos	0:894b603d32ee	2043	for (int i = 0; i < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes); i++)
saloutos	0:894b603d32ee	2044	{
saloutos	0:894b603d32ee	2045	data->Raw_Gyro.Bytes[DMP_Raw_Gyro_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2046	}
saloutos	0:894b603d32ee	2047	fifo_count -= (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes); // Decrement the count
saloutos	0:894b603d32ee	2048	}
saloutos	0:894b603d32ee	2049
saloutos	0:894b603d32ee	2050	if ((data->header & DMP_header_bitmap_Compass) > 0) // case DMP_header_bitmap_Compass:
saloutos	0:894b603d32ee	2051	{
saloutos	0:894b603d32ee	2052	if (fifo_count < icm_20948_DMP_Compass_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2053	{
saloutos	0:894b603d32ee	2054	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2055	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2056	return result;
saloutos	0:894b603d32ee	2057	}
saloutos	0:894b603d32ee	2058	if (fifo_count < icm_20948_DMP_Compass_Bytes)
saloutos	0:894b603d32ee	2059	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2060	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Bytes);
saloutos	0:894b603d32ee	2061	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2062	return result;
saloutos	0:894b603d32ee	2063	for (int i = 0; i < icm_20948_DMP_Compass_Bytes; i++)
saloutos	0:894b603d32ee	2064	{
saloutos	0:894b603d32ee	2065	data->Compass.Bytes[DMP_PQuat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2066	}
saloutos	0:894b603d32ee	2067	fifo_count -= icm_20948_DMP_Compass_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2068	}
saloutos	0:894b603d32ee	2069
saloutos	0:894b603d32ee	2070	if ((data->header & DMP_header_bitmap_ALS) > 0) // case DMP_header_bitmap_ALS:
saloutos	0:894b603d32ee	2071	{
saloutos	0:894b603d32ee	2072	if (fifo_count < icm_20948_DMP_ALS_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2073	{
saloutos	0:894b603d32ee	2074	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2075	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2076	return result;
saloutos	0:894b603d32ee	2077	}
saloutos	0:894b603d32ee	2078	if (fifo_count < icm_20948_DMP_ALS_Bytes)
saloutos	0:894b603d32ee	2079	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2080	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_ALS_Bytes);
saloutos	0:894b603d32ee	2081	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2082	return result;
saloutos	0:894b603d32ee	2083	for (int i = 0; i < icm_20948_DMP_ALS_Bytes; i++)
saloutos	0:894b603d32ee	2084	{
saloutos	0:894b603d32ee	2085	data->ALS[i] = fifoBytes[i];
saloutos	0:894b603d32ee	2086	}
saloutos	0:894b603d32ee	2087	fifo_count -= icm_20948_DMP_ALS_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2088	}
saloutos	0:894b603d32ee	2089
saloutos	0:894b603d32ee	2090	if ((data->header & DMP_header_bitmap_Quat6) > 0) // case DMP_header_bitmap_Quat6:
saloutos	0:894b603d32ee	2091	{
saloutos	0:894b603d32ee	2092	if (fifo_count < icm_20948_DMP_Quat6_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2093	{
saloutos	0:894b603d32ee	2094	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2095	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2096	return result;
saloutos	0:894b603d32ee	2097	}
saloutos	0:894b603d32ee	2098	if (fifo_count < icm_20948_DMP_Quat6_Bytes)
saloutos	0:894b603d32ee	2099	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2100	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Quat6_Bytes);
saloutos	0:894b603d32ee	2101	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2102	return result;
saloutos	0:894b603d32ee	2103	for (int i = 0; i < icm_20948_DMP_Quat6_Bytes; i++)
saloutos	0:894b603d32ee	2104	{
saloutos	0:894b603d32ee	2105	data->Quat6.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2106	}
saloutos	0:894b603d32ee	2107	fifo_count -= icm_20948_DMP_Quat6_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2108	}
saloutos	0:894b603d32ee	2109
saloutos	0:894b603d32ee	2110	if ((data->header & DMP_header_bitmap_Quat9) > 0) // case DMP_header_bitmap_Quat9:
saloutos	0:894b603d32ee	2111	{
saloutos	0:894b603d32ee	2112	if (fifo_count < icm_20948_DMP_Quat9_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2113	{
saloutos	0:894b603d32ee	2114	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2115	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2116	return result;
saloutos	0:894b603d32ee	2117	}
saloutos	0:894b603d32ee	2118	if (fifo_count < icm_20948_DMP_Quat9_Bytes)
saloutos	0:894b603d32ee	2119	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2120	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Quat9_Bytes);
saloutos	0:894b603d32ee	2121	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2122	return result;
saloutos	0:894b603d32ee	2123	for (int i = 0; i < icm_20948_DMP_Quat9_Bytes; i++)
saloutos	0:894b603d32ee	2124	{
saloutos	0:894b603d32ee	2125	data->Quat9.Bytes[DMP_Quat9_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2126	}
saloutos	0:894b603d32ee	2127	fifo_count -= icm_20948_DMP_Quat9_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2128	}
saloutos	0:894b603d32ee	2129
saloutos	0:894b603d32ee	2130	if ((data->header & DMP_header_bitmap_PQuat6) > 0) // case DMP_header_bitmap_PQuat6:
saloutos	0:894b603d32ee	2131	{
saloutos	0:894b603d32ee	2132	if (fifo_count < icm_20948_DMP_PQuat6_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2133	{
saloutos	0:894b603d32ee	2134	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2135	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2136	return result;
saloutos	0:894b603d32ee	2137	}
saloutos	0:894b603d32ee	2138	if (fifo_count < icm_20948_DMP_PQuat6_Bytes)
saloutos	0:894b603d32ee	2139	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2140	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_PQuat6_Bytes);
saloutos	0:894b603d32ee	2141	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2142	return result;
saloutos	0:894b603d32ee	2143	for (int i = 0; i < icm_20948_DMP_PQuat6_Bytes; i++)
saloutos	0:894b603d32ee	2144	{
saloutos	0:894b603d32ee	2145	data->PQuat6.Bytes[DMP_PQuat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2146	}
saloutos	0:894b603d32ee	2147	fifo_count -= icm_20948_DMP_PQuat6_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2148	}
saloutos	0:894b603d32ee	2149
saloutos	0:894b603d32ee	2150	if ((data->header & DMP_header_bitmap_Geomag) > 0) // case DMP_header_bitmap_Geomag:
saloutos	0:894b603d32ee	2151	{
saloutos	0:894b603d32ee	2152	if (fifo_count < icm_20948_DMP_Geomag_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2153	{
saloutos	0:894b603d32ee	2154	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2155	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2156	return result;
saloutos	0:894b603d32ee	2157	}
saloutos	0:894b603d32ee	2158	if (fifo_count < icm_20948_DMP_Geomag_Bytes)
saloutos	0:894b603d32ee	2159	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2160	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Geomag_Bytes);
saloutos	0:894b603d32ee	2161	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2162	return result;
saloutos	0:894b603d32ee	2163	for (int i = 0; i < icm_20948_DMP_Geomag_Bytes; i++)
saloutos	0:894b603d32ee	2164	{
saloutos	0:894b603d32ee	2165	data->Geomag.Bytes[DMP_Quat9_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2166	}
saloutos	0:894b603d32ee	2167	fifo_count -= icm_20948_DMP_Geomag_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2168	}
saloutos	0:894b603d32ee	2169
saloutos	0:894b603d32ee	2170	if ((data->header & DMP_header_bitmap_Pressure) > 0) // case DMP_header_bitmap_Pressure:
saloutos	0:894b603d32ee	2171	{
saloutos	0:894b603d32ee	2172	if (fifo_count < icm_20948_DMP_Pressure_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2173	{
saloutos	0:894b603d32ee	2174	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2175	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2176	return result;
saloutos	0:894b603d32ee	2177	}
saloutos	0:894b603d32ee	2178	if (fifo_count < icm_20948_DMP_Pressure_Bytes)
saloutos	0:894b603d32ee	2179	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2180	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Pressure_Bytes);
saloutos	0:894b603d32ee	2181	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2182	return result;
saloutos	0:894b603d32ee	2183	for (int i = 0; i < icm_20948_DMP_Pressure_Bytes; i++)
saloutos	0:894b603d32ee	2184	{
saloutos	0:894b603d32ee	2185	data->Pressure[i] = fifoBytes[i];
saloutos	0:894b603d32ee	2186	}
saloutos	0:894b603d32ee	2187	fifo_count -= icm_20948_DMP_Pressure_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2188	}
saloutos	0:894b603d32ee	2189
saloutos	0:894b603d32ee	2190	if ((data->header & DMP_header_bitmap_Gyro_Calibr) > 0) // case DMP_header_bitmap_Gyro_Calibr:
saloutos	0:894b603d32ee	2191	{
saloutos	0:894b603d32ee	2192	// lcm20948MPUFifoControl.c suggests icm_20948_DMP_Gyro_Calibr_Bytes is not supported
saloutos	0:894b603d32ee	2193	// and looking at DMP frames which have the Gyro_Calibr bit set, that certainly seems to be true.
saloutos	0:894b603d32ee	2194	// So, we'll skip this...:
saloutos	0:894b603d32ee	2195	/*
saloutos	0:894b603d32ee	2196	if (fifo_count < icm_20948_DMP_Gyro_Calibr_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2197	{
saloutos	0:894b603d32ee	2198	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2199	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2200	return result;
saloutos	0:894b603d32ee	2201	}
saloutos	0:894b603d32ee	2202	if (fifo_count < icm_20948_DMP_Gyro_Calibr_Bytes)
saloutos	0:894b603d32ee	2203	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2204	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Gyro_Calibr_Bytes);
saloutos	0:894b603d32ee	2205	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2206	return result;
saloutos	0:894b603d32ee	2207	for (int i = 0; i < icm_20948_DMP_Gyro_Calibr_Bytes; i++)
saloutos	0:894b603d32ee	2208	{
saloutos	0:894b603d32ee	2209	data->Gyro_Calibr.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2210	}
saloutos	0:894b603d32ee	2211	fifo_count -= icm_20948_DMP_Gyro_Calibr_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2212	*/
saloutos	0:894b603d32ee	2213	}
saloutos	0:894b603d32ee	2214
saloutos	0:894b603d32ee	2215	if ((data->header & DMP_header_bitmap_Compass_Calibr) > 0) // case DMP_header_bitmap_Compass_Calibr:
saloutos	0:894b603d32ee	2216	{
saloutos	0:894b603d32ee	2217	if (fifo_count < icm_20948_DMP_Compass_Calibr_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2218	{
saloutos	0:894b603d32ee	2219	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2220	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2221	return result;
saloutos	0:894b603d32ee	2222	}
saloutos	0:894b603d32ee	2223	if (fifo_count < icm_20948_DMP_Compass_Calibr_Bytes)
saloutos	0:894b603d32ee	2224	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2225	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Calibr_Bytes);
saloutos	0:894b603d32ee	2226	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2227	return result;
saloutos	0:894b603d32ee	2228	for (int i = 0; i < icm_20948_DMP_Compass_Calibr_Bytes; i++)
saloutos	0:894b603d32ee	2229	{
saloutos	0:894b603d32ee	2230	data->Compass_Calibr.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
saloutos	0:894b603d32ee	2231	}
saloutos	0:894b603d32ee	2232	fifo_count -= icm_20948_DMP_Compass_Calibr_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2233	}
saloutos	0:894b603d32ee	2234
saloutos	0:894b603d32ee	2235	if ((data->header & DMP_header_bitmap_Step_Detector) > 0) // case DMP_header_bitmap_Step_Detector:
saloutos	0:894b603d32ee	2236	{
saloutos	0:894b603d32ee	2237	if (fifo_count < icm_20948_DMP_Step_Detector_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2238	{
saloutos	0:894b603d32ee	2239	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2240	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2241	return result;
saloutos	0:894b603d32ee	2242	}
saloutos	0:894b603d32ee	2243	if (fifo_count < icm_20948_DMP_Step_Detector_Bytes)
saloutos	0:894b603d32ee	2244	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2245	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Step_Detector_Bytes);
saloutos	0:894b603d32ee	2246	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2247	return result;
saloutos	0:894b603d32ee	2248	uint32_t aWord = 0;
saloutos	0:894b603d32ee	2249	for (int i = 0; i < icm_20948_DMP_Step_Detector_Bytes; i++)
saloutos	0:894b603d32ee	2250	{
saloutos	0:894b603d32ee	2251	aWord \|= ((uint32_t)fifoBytes[i]) << (24 - (i * 8));
saloutos	0:894b603d32ee	2252	}
saloutos	0:894b603d32ee	2253	data->Pedometer_Timestamp = aWord;
saloutos	0:894b603d32ee	2254	fifo_count -= icm_20948_DMP_Step_Detector_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2255	}
saloutos	0:894b603d32ee	2256
saloutos	0:894b603d32ee	2257	// Now check for header2 features
saloutos	0:894b603d32ee	2258
saloutos	0:894b603d32ee	2259	if ((data->header2 & DMP_header2_bitmap_Accel_Accuracy) > 0) // case DMP_header2_bitmap_Accel_Accuracy:
saloutos	0:894b603d32ee	2260	{
saloutos	0:894b603d32ee	2261	if (fifo_count < icm_20948_DMP_Accel_Accuracy_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2262	{
saloutos	0:894b603d32ee	2263	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2264	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2265	return result;
saloutos	0:894b603d32ee	2266	}
saloutos	0:894b603d32ee	2267	if (fifo_count < icm_20948_DMP_Accel_Accuracy_Bytes)
saloutos	0:894b603d32ee	2268	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2269	aShort = 0;
saloutos	0:894b603d32ee	2270	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Accel_Accuracy_Bytes);
saloutos	0:894b603d32ee	2271	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2272	return result;
saloutos	0:894b603d32ee	2273	for (int i = 0; i < icm_20948_DMP_Accel_Accuracy_Bytes; i++)
saloutos	0:894b603d32ee	2274	{
saloutos	0:894b603d32ee	2275	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2276	}
saloutos	0:894b603d32ee	2277	data->Accel_Accuracy = aShort;
saloutos	0:894b603d32ee	2278	fifo_count -= icm_20948_DMP_Accel_Accuracy_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2279	}
saloutos	0:894b603d32ee	2280
saloutos	0:894b603d32ee	2281	if ((data->header2 & DMP_header2_bitmap_Gyro_Accuracy) > 0) // case DMP_header2_bitmap_Gyro_Accuracy:
saloutos	0:894b603d32ee	2282	{
saloutos	0:894b603d32ee	2283	if (fifo_count < icm_20948_DMP_Gyro_Accuracy_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2284	{
saloutos	0:894b603d32ee	2285	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2286	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2287	return result;
saloutos	0:894b603d32ee	2288	}
saloutos	0:894b603d32ee	2289	if (fifo_count < icm_20948_DMP_Gyro_Accuracy_Bytes)
saloutos	0:894b603d32ee	2290	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2291	aShort = 0;
saloutos	0:894b603d32ee	2292	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Gyro_Accuracy_Bytes);
saloutos	0:894b603d32ee	2293	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2294	return result;
saloutos	0:894b603d32ee	2295	for (int i = 0; i < icm_20948_DMP_Gyro_Accuracy_Bytes; i++)
saloutos	0:894b603d32ee	2296	{
saloutos	0:894b603d32ee	2297	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2298	}
saloutos	0:894b603d32ee	2299	data->Gyro_Accuracy = aShort;
saloutos	0:894b603d32ee	2300	fifo_count -= icm_20948_DMP_Gyro_Accuracy_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2301	}
saloutos	0:894b603d32ee	2302
saloutos	0:894b603d32ee	2303	if ((data->header2 & DMP_header2_bitmap_Compass_Accuracy) > 0) // case DMP_header2_bitmap_Compass_Accuracy:
saloutos	0:894b603d32ee	2304	{
saloutos	0:894b603d32ee	2305	if (fifo_count < icm_20948_DMP_Compass_Accuracy_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2306	{
saloutos	0:894b603d32ee	2307	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2308	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2309	return result;
saloutos	0:894b603d32ee	2310	}
saloutos	0:894b603d32ee	2311	if (fifo_count < icm_20948_DMP_Compass_Accuracy_Bytes)
saloutos	0:894b603d32ee	2312	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2313	aShort = 0;
saloutos	0:894b603d32ee	2314	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Accuracy_Bytes);
saloutos	0:894b603d32ee	2315	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2316	return result;
saloutos	0:894b603d32ee	2317	for (int i = 0; i < icm_20948_DMP_Compass_Accuracy_Bytes; i++)
saloutos	0:894b603d32ee	2318	{
saloutos	0:894b603d32ee	2319	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2320	}
saloutos	0:894b603d32ee	2321	data->Compass_Accuracy = aShort;
saloutos	0:894b603d32ee	2322	fifo_count -= icm_20948_DMP_Compass_Accuracy_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2323	}
saloutos	0:894b603d32ee	2324
saloutos	0:894b603d32ee	2325	if ((data->header2 & DMP_header2_bitmap_Fsync) > 0) // case DMP_header2_bitmap_Fsync:
saloutos	0:894b603d32ee	2326	{
saloutos	0:894b603d32ee	2327	// lcm20948MPUFifoControl.c suggests icm_20948_DMP_Fsync_Detection_Bytes is not supported.
saloutos	0:894b603d32ee	2328	// So, we'll skip this just in case...:
saloutos	0:894b603d32ee	2329	/*
saloutos	0:894b603d32ee	2330	if (fifo_count < icm_20948_DMP_Fsync_Detection_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2331	{
saloutos	0:894b603d32ee	2332	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2333	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2334	return result;
saloutos	0:894b603d32ee	2335	}
saloutos	0:894b603d32ee	2336	if (fifo_count < icm_20948_DMP_Fsync_Detection_Bytes)
saloutos	0:894b603d32ee	2337	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2338	aShort = 0;
saloutos	0:894b603d32ee	2339	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Fsync_Detection_Bytes);
saloutos	0:894b603d32ee	2340	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2341	return result;
saloutos	0:894b603d32ee	2342	for (int i = 0; i < icm_20948_DMP_Fsync_Detection_Bytes; i++)
saloutos	0:894b603d32ee	2343	{
saloutos	0:894b603d32ee	2344	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2345	}
saloutos	0:894b603d32ee	2346	data->Fsync_Delay_Time = aShort;
saloutos	0:894b603d32ee	2347	fifo_count -= icm_20948_DMP_Fsync_Detection_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2348	*/
saloutos	0:894b603d32ee	2349	}
saloutos	0:894b603d32ee	2350
saloutos	0:894b603d32ee	2351	if ((data->header2 & DMP_header2_bitmap_Pickup) > 0) // case DMP_header2_bitmap_Pickup:
saloutos	0:894b603d32ee	2352	{
saloutos	0:894b603d32ee	2353	if (fifo_count < icm_20948_DMP_Pickup_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2354	{
saloutos	0:894b603d32ee	2355	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2356	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2357	return result;
saloutos	0:894b603d32ee	2358	}
saloutos	0:894b603d32ee	2359	if (fifo_count < icm_20948_DMP_Pickup_Bytes)
saloutos	0:894b603d32ee	2360	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2361	aShort = 0;
saloutos	0:894b603d32ee	2362	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Pickup_Bytes);
saloutos	0:894b603d32ee	2363	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2364	return result;
saloutos	0:894b603d32ee	2365	for (int i = 0; i < icm_20948_DMP_Pickup_Bytes; i++)
saloutos	0:894b603d32ee	2366	{
saloutos	0:894b603d32ee	2367	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2368	}
saloutos	0:894b603d32ee	2369	data->Pickup = aShort;
saloutos	0:894b603d32ee	2370	fifo_count -= icm_20948_DMP_Pickup_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2371	}
saloutos	0:894b603d32ee	2372
saloutos	0:894b603d32ee	2373	if ((data->header2 & DMP_header2_bitmap_Activity_Recog) > 0) // case DMP_header2_bitmap_Activity_Recog:
saloutos	0:894b603d32ee	2374	{
saloutos	0:894b603d32ee	2375	if (fifo_count < icm_20948_DMP_Activity_Recognition_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2376	{
saloutos	0:894b603d32ee	2377	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2378	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2379	return result;
saloutos	0:894b603d32ee	2380	}
saloutos	0:894b603d32ee	2381	if (fifo_count < icm_20948_DMP_Activity_Recognition_Bytes)
saloutos	0:894b603d32ee	2382	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2383	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Activity_Recognition_Bytes);
saloutos	0:894b603d32ee	2384	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2385	return result;
saloutos	0:894b603d32ee	2386	for (int i = 0; i < icm_20948_DMP_Activity_Recognition_Bytes; i++)
saloutos	0:894b603d32ee	2387	{
saloutos	0:894b603d32ee	2388	data->Activity_Recognition.Bytes[DMP_Activity_Recognition_Byte_Ordering[i]] = fifoBytes[i];
saloutos	0:894b603d32ee	2389	}
saloutos	0:894b603d32ee	2390	fifo_count -= icm_20948_DMP_Activity_Recognition_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2391	}
saloutos	0:894b603d32ee	2392
saloutos	0:894b603d32ee	2393	if ((data->header2 & DMP_header2_bitmap_Secondary_On_Off) > 0) // case DMP_header2_bitmap_Secondary_On_Off:
saloutos	0:894b603d32ee	2394	{
saloutos	0:894b603d32ee	2395	if (fifo_count < icm_20948_DMP_Secondary_On_Off_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2396	{
saloutos	0:894b603d32ee	2397	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2398	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2399	return result;
saloutos	0:894b603d32ee	2400	}
saloutos	0:894b603d32ee	2401	if (fifo_count < icm_20948_DMP_Secondary_On_Off_Bytes)
saloutos	0:894b603d32ee	2402	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2403	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Secondary_On_Off_Bytes);
saloutos	0:894b603d32ee	2404	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2405	return result;
saloutos	0:894b603d32ee	2406	for (int i = 0; i < icm_20948_DMP_Secondary_On_Off_Bytes; i++)
saloutos	0:894b603d32ee	2407	{
saloutos	0:894b603d32ee	2408	data->Secondary_On_Off.Bytes[DMP_Secondary_On_Off_Byte_Ordering[i]] = fifoBytes[i];
saloutos	0:894b603d32ee	2409	}
saloutos	0:894b603d32ee	2410	fifo_count -= icm_20948_DMP_Secondary_On_Off_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2411	}
saloutos	0:894b603d32ee	2412
saloutos	0:894b603d32ee	2413	// Finally, extract the footer (gyro count)
saloutos	0:894b603d32ee	2414	if (fifo_count < icm_20948_DMP_Footer_Bytes) // Check if we need to read the FIFO count again
saloutos	0:894b603d32ee	2415	{
saloutos	0:894b603d32ee	2416	result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
saloutos	0:894b603d32ee	2417	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2418	return result;
saloutos	0:894b603d32ee	2419	}
saloutos	0:894b603d32ee	2420	if (fifo_count < icm_20948_DMP_Footer_Bytes)
saloutos	0:894b603d32ee	2421	return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
saloutos	0:894b603d32ee	2422	aShort = 0;
saloutos	0:894b603d32ee	2423	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Footer_Bytes);
saloutos	0:894b603d32ee	2424	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2425	return result;
saloutos	0:894b603d32ee	2426	for (int i = 0; i < icm_20948_DMP_Footer_Bytes; i++)
saloutos	0:894b603d32ee	2427	{
saloutos	0:894b603d32ee	2428	aShort \|= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
saloutos	0:894b603d32ee	2429	}
saloutos	0:894b603d32ee	2430	data->Footer = aShort;
saloutos	0:894b603d32ee	2431	fifo_count -= icm_20948_DMP_Footer_Bytes; // Decrement the count
saloutos	0:894b603d32ee	2432
saloutos	0:894b603d32ee	2433	if (fifo_count > 0) // Check if there is still data waiting to be read
saloutos	0:894b603d32ee	2434	return ICM_20948_Stat_FIFOMoreDataAvail;
saloutos	0:894b603d32ee	2435
saloutos	0:894b603d32ee	2436	return result;
saloutos	0:894b603d32ee	2437	}
saloutos	0:894b603d32ee	2438
saloutos	0:894b603d32ee	2439	uint8_t sensor_type_2_android_sensor(enum inv_icm20948_sensor sensor)
saloutos	0:894b603d32ee	2440	{
saloutos	0:894b603d32ee	2441	switch (sensor)
saloutos	0:894b603d32ee	2442	{
saloutos	0:894b603d32ee	2443	case INV_ICM20948_SENSOR_ACCELEROMETER:
saloutos	0:894b603d32ee	2444	return ANDROID_SENSOR_ACCELEROMETER; // 1
saloutos	0:894b603d32ee	2445	case INV_ICM20948_SENSOR_GYROSCOPE:
saloutos	0:894b603d32ee	2446	return ANDROID_SENSOR_GYROSCOPE; // 4
saloutos	0:894b603d32ee	2447	case INV_ICM20948_SENSOR_RAW_ACCELEROMETER:
saloutos	0:894b603d32ee	2448	return ANDROID_SENSOR_RAW_ACCELEROMETER; // 42
saloutos	0:894b603d32ee	2449	case INV_ICM20948_SENSOR_RAW_GYROSCOPE:
saloutos	0:894b603d32ee	2450	return ANDROID_SENSOR_RAW_GYROSCOPE; // 43
saloutos	0:894b603d32ee	2451	case INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
saloutos	0:894b603d32ee	2452	return ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED; // 14
saloutos	0:894b603d32ee	2453	case INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED:
saloutos	0:894b603d32ee	2454	return ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED; // 16
saloutos	0:894b603d32ee	2455	case INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON:
saloutos	0:894b603d32ee	2456	return ANDROID_SENSOR_ACTIVITY_CLASSIFICATON; // 47
saloutos	0:894b603d32ee	2457	case INV_ICM20948_SENSOR_STEP_DETECTOR:
saloutos	0:894b603d32ee	2458	return ANDROID_SENSOR_STEP_DETECTOR; // 18
saloutos	0:894b603d32ee	2459	case INV_ICM20948_SENSOR_STEP_COUNTER:
saloutos	0:894b603d32ee	2460	return ANDROID_SENSOR_STEP_COUNTER; // 19
saloutos	0:894b603d32ee	2461	case INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR:
saloutos	0:894b603d32ee	2462	return ANDROID_SENSOR_GAME_ROTATION_VECTOR; // 15
saloutos	0:894b603d32ee	2463	case INV_ICM20948_SENSOR_ROTATION_VECTOR:
saloutos	0:894b603d32ee	2464	return ANDROID_SENSOR_ROTATION_VECTOR; // 11
saloutos	0:894b603d32ee	2465	case INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
saloutos	0:894b603d32ee	2466	return ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR; // 20
saloutos	0:894b603d32ee	2467	case INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD:
saloutos	0:894b603d32ee	2468	return ANDROID_SENSOR_GEOMAGNETIC_FIELD; // 2
saloutos	0:894b603d32ee	2469	case INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION:
saloutos	0:894b603d32ee	2470	return ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION; // 17
saloutos	0:894b603d32ee	2471	case INV_ICM20948_SENSOR_FLIP_PICKUP:
saloutos	0:894b603d32ee	2472	return ANDROID_SENSOR_FLIP_PICKUP; // 46
saloutos	0:894b603d32ee	2473	case INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR:
saloutos	0:894b603d32ee	2474	return ANDROID_SENSOR_WAKEUP_TILT_DETECTOR; // 41
saloutos	0:894b603d32ee	2475	case INV_ICM20948_SENSOR_GRAVITY:
saloutos	0:894b603d32ee	2476	return ANDROID_SENSOR_GRAVITY; // 9
saloutos	0:894b603d32ee	2477	case INV_ICM20948_SENSOR_LINEAR_ACCELERATION:
saloutos	0:894b603d32ee	2478	return ANDROID_SENSOR_LINEAR_ACCELERATION; // 10
saloutos	0:894b603d32ee	2479	case INV_ICM20948_SENSOR_ORIENTATION:
saloutos	0:894b603d32ee	2480	return ANDROID_SENSOR_ORIENTATION; // 3
saloutos	0:894b603d32ee	2481	case INV_ICM20948_SENSOR_B2S:
saloutos	0:894b603d32ee	2482	return ANDROID_SENSOR_B2S; // 45
saloutos	0:894b603d32ee	2483	default:
saloutos	0:894b603d32ee	2484	return ANDROID_SENSOR_NUM_MAX;
saloutos	0:894b603d32ee	2485	}
saloutos	0:894b603d32ee	2486	}
saloutos	0:894b603d32ee	2487
saloutos	0:894b603d32ee	2488	enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor)
saloutos	0:894b603d32ee	2489	{
saloutos	0:894b603d32ee	2490	switch (sensor)
saloutos	0:894b603d32ee	2491	{
saloutos	0:894b603d32ee	2492	case ANDROID_SENSOR_ACCELEROMETER:
saloutos	0:894b603d32ee	2493	return INV_ICM20948_SENSOR_ACCELEROMETER;
saloutos	0:894b603d32ee	2494	case ANDROID_SENSOR_GYROSCOPE:
saloutos	0:894b603d32ee	2495	return INV_ICM20948_SENSOR_GYROSCOPE;
saloutos	0:894b603d32ee	2496	case ANDROID_SENSOR_RAW_ACCELEROMETER:
saloutos	0:894b603d32ee	2497	return INV_ICM20948_SENSOR_RAW_ACCELEROMETER;
saloutos	0:894b603d32ee	2498	case ANDROID_SENSOR_RAW_GYROSCOPE:
saloutos	0:894b603d32ee	2499	return INV_ICM20948_SENSOR_RAW_GYROSCOPE;
saloutos	0:894b603d32ee	2500	case ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
saloutos	0:894b603d32ee	2501	return INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED;
saloutos	0:894b603d32ee	2502	case ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED:
saloutos	0:894b603d32ee	2503	return INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED;
saloutos	0:894b603d32ee	2504	case ANDROID_SENSOR_ACTIVITY_CLASSIFICATON:
saloutos	0:894b603d32ee	2505	return INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON;
saloutos	0:894b603d32ee	2506	case ANDROID_SENSOR_STEP_DETECTOR:
saloutos	0:894b603d32ee	2507	return INV_ICM20948_SENSOR_STEP_DETECTOR;
saloutos	0:894b603d32ee	2508	case ANDROID_SENSOR_STEP_COUNTER:
saloutos	0:894b603d32ee	2509	return INV_ICM20948_SENSOR_STEP_COUNTER;
saloutos	0:894b603d32ee	2510	case ANDROID_SENSOR_GAME_ROTATION_VECTOR:
saloutos	0:894b603d32ee	2511	return INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR;
saloutos	0:894b603d32ee	2512	case ANDROID_SENSOR_ROTATION_VECTOR:
saloutos	0:894b603d32ee	2513	return INV_ICM20948_SENSOR_ROTATION_VECTOR;
saloutos	0:894b603d32ee	2514	case ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
saloutos	0:894b603d32ee	2515	return INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR;
saloutos	0:894b603d32ee	2516	case ANDROID_SENSOR_GEOMAGNETIC_FIELD:
saloutos	0:894b603d32ee	2517	return INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD;
saloutos	0:894b603d32ee	2518	case ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION:
saloutos	0:894b603d32ee	2519	return INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION;
saloutos	0:894b603d32ee	2520	case ANDROID_SENSOR_FLIP_PICKUP:
saloutos	0:894b603d32ee	2521	return INV_ICM20948_SENSOR_FLIP_PICKUP;
saloutos	0:894b603d32ee	2522	case ANDROID_SENSOR_WAKEUP_TILT_DETECTOR:
saloutos	0:894b603d32ee	2523	return INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR;
saloutos	0:894b603d32ee	2524	case ANDROID_SENSOR_GRAVITY:
saloutos	0:894b603d32ee	2525	return INV_ICM20948_SENSOR_GRAVITY;
saloutos	0:894b603d32ee	2526	case ANDROID_SENSOR_LINEAR_ACCELERATION:
saloutos	0:894b603d32ee	2527	return INV_ICM20948_SENSOR_LINEAR_ACCELERATION;
saloutos	0:894b603d32ee	2528	case ANDROID_SENSOR_ORIENTATION:
saloutos	0:894b603d32ee	2529	return INV_ICM20948_SENSOR_ORIENTATION;
saloutos	0:894b603d32ee	2530	case ANDROID_SENSOR_B2S:
saloutos	0:894b603d32ee	2531	return INV_ICM20948_SENSOR_B2S;
saloutos	0:894b603d32ee	2532	default:
saloutos	0:894b603d32ee	2533	return INV_ICM20948_SENSOR_MAX;
saloutos	0:894b603d32ee	2534	}
saloutos	0:894b603d32ee	2535	}
saloutos	0:894b603d32ee	2536
saloutos	0:894b603d32ee	2537	ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned char div, int gyro_level)
saloutos	0:894b603d32ee	2538	{
saloutos	0:894b603d32ee	2539	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
saloutos	0:894b603d32ee	2540
saloutos	0:894b603d32ee	2541	if (pdev->_dmp_firmware_available == false)
saloutos	0:894b603d32ee	2542	return ICM_20948_Stat_DMPNotSupported;
saloutos	0:894b603d32ee	2543
saloutos	0:894b603d32ee	2544	// gyro_level should be set to 4 regardless of fullscale, due to the addition of API dmp_icm20648_set_gyro_fsr()
saloutos	0:894b603d32ee	2545	gyro_level = 4;
saloutos	0:894b603d32ee	2546
saloutos	0:894b603d32ee	2547	// First read the TIMEBASE_CORRECTION_PLL register from Bank 1
saloutos	0:894b603d32ee	2548	int8_t pll; // Signed. Typical value is 0x18
saloutos	0:894b603d32ee	2549	result = ICM_20948_set_bank(pdev, 1);
saloutos	0:894b603d32ee	2550	result = ICM_20948_execute_r(pdev, AGB1_REG_TIMEBASE_CORRECTION_PLL, (uint8_t *)&pll, 1);
saloutos	0:894b603d32ee	2551	if (result != ICM_20948_Stat_Ok)
saloutos	0:894b603d32ee	2552	{
saloutos	0:894b603d32ee	2553	return result;
saloutos	0:894b603d32ee	2554	}
saloutos	0:894b603d32ee	2555
saloutos	0:894b603d32ee	2556	pdev->_gyroSFpll = pll; // Record the PLL value so we can debug print it
saloutos	0:894b603d32ee	2557
saloutos	0:894b603d32ee	2558	// Now calculate the Gyro SF using code taken from the InvenSense example (inv_icm20948_set_gyro_sf)
saloutos	0:894b603d32ee	2559
saloutos	0:894b603d32ee	2560	long gyro_sf;
saloutos	0:894b603d32ee	2561
saloutos	0:894b603d32ee	2562	unsigned long long const MagicConstant = 264446880937391LL;
saloutos	0:894b603d32ee	2563	unsigned long long const MagicConstantScale = 100000LL;
saloutos	0:894b603d32ee	2564	unsigned long long ResultLL;
saloutos	0:894b603d32ee	2565
saloutos	0:894b603d32ee	2566	if (pll & 0x80)
saloutos	0:894b603d32ee	2567	{
saloutos	0:894b603d32ee	2568	ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 - (pll & 0x7F)) / MagicConstantScale);
saloutos	0:894b603d32ee	2569	}
saloutos	0:894b603d32ee	2570	else
saloutos	0:894b603d32ee	2571	{
saloutos	0:894b603d32ee	2572	ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 + pll) / MagicConstantScale);
saloutos	0:894b603d32ee	2573	}
saloutos	0:894b603d32ee	2574	/*
saloutos	0:894b603d32ee	2575	In above deprecated FP version, worst case arguments can produce a result that overflows a signed long.
saloutos	0:894b603d32ee	2576	Here, for such cases, we emulate the FP behavior of setting the result to the maximum positive value, as
saloutos	0:894b603d32ee	2577	the compiler's conversion of a u64 to an s32 is simple truncation of the u64's high half, sadly....
saloutos	0:894b603d32ee	2578	*/
saloutos	0:894b603d32ee	2579	if (ResultLL > 0x7FFFFFFF)
saloutos	0:894b603d32ee	2580	gyro_sf = 0x7FFFFFFF;
saloutos	0:894b603d32ee	2581	else
saloutos	0:894b603d32ee	2582	gyro_sf = (long)ResultLL;
saloutos	0:894b603d32ee	2583
saloutos	0:894b603d32ee	2584	pdev->_gyroSF = gyro_sf; // Record value so we can debug print it
saloutos	0:894b603d32ee	2585
saloutos	0:894b603d32ee	2586	// Finally, write the value to the DMP GYRO_SF register
saloutos	0:894b603d32ee	2587	unsigned char gyro_sf_reg[4];
saloutos	0:894b603d32ee	2588	gyro_sf_reg[0] = (unsigned char)(gyro_sf >> 24);
saloutos	0:894b603d32ee	2589	gyro_sf_reg[1] = (unsigned char)(gyro_sf >> 16);
saloutos	0:894b603d32ee	2590	gyro_sf_reg[2] = (unsigned char)(gyro_sf >> 8);
saloutos	0:894b603d32ee	2591	gyro_sf_reg[3] = (unsigned char)(gyro_sf & 0xff);
saloutos	0:894b603d32ee	2592	result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char *)&gyro_sf_reg);
saloutos	0:894b603d32ee	2593
saloutos	0:894b603d32ee	2594	return result;
saloutos	0:894b603d32ee	2595	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	31 Jan 2022
Imports:	0
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	1

util/ICM_20948_C.c@0:894b603d32ee, 2022-01-31 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning