SENtral_SimpleSerialHostInterface - SENtral Simple Serial Host interface for PNI Sens…

PNI » Code » SENtral_SimpleSerialHostInterface

PNI / Mbed 2 deprecated SENtral_SimpleSerialHostInterface

SENtral Simple Serial Host interface for PNI Sensor Corp SENtral-A2 motion coprocessor. For use with the RM3100RTI Arduino shield module on top of an STM4 serial mbed board. Will work with an PNI RM3100RTI module or M&M motion modules. Interaction with unit using built in USB serial serial port set for 115200 baud. Send '?' char for menu. Presently requires SENtral firmware to either be loaded in the RM3100RTI Arduino shield SD Card or preloaded in the RM3100RTI or M&M module's EEPROM. Firmware is typically preloaded on the module's EEPROM by PNI. PNI Sensor, 2019 www.pnicorp.com

Dependencies: mbed SDFileSystemVSG

em7186.cpp@1:b0a205c9b958, 2016-07-22 (annotated)

Committer:: JoeMiller
Date:: Fri Jul 22 18:13:26 2016 +0000
Revision:: 1:b0a205c9b958
Parent:: 0:02c0c2cbc3df
Child:: 3:69239f60d620

Removed Nucleo RED LED. It interfered with SD Card Operation

Who changed what in which revision?

User	Revision	Line number	New contents of line
JoeMiller	0:02c0c2cbc3df	1	/**
JoeMiller	0:02c0c2cbc3df	2	* @file em7186.c
JoeMiller	0:02c0c2cbc3df	3	*
JoeMiller	0:02c0c2cbc3df	4	* @brief Sample interface for the em7186.
JoeMiller	0:02c0c2cbc3df	5	*
JoeMiller	0:02c0c2cbc3df	6	* @authors David Vincent, Joe Miller
JoeMiller	0:02c0c2cbc3df	7	* @date 05/12/2016
JoeMiller	0:02c0c2cbc3df	8	* @copyright (C) 2015, 2016 PNI Corp
JoeMiller	0:02c0c2cbc3df	9	*
JoeMiller	0:02c0c2cbc3df	10	* @copyright Disclosure to third parties or reproduction in any form
JoeMiller	0:02c0c2cbc3df	11	* whatsoever, without prior written consent, is strictly forbidden
JoeMiller	0:02c0c2cbc3df	12	*
JoeMiller	0:02c0c2cbc3df	13	*/
JoeMiller	0:02c0c2cbc3df	14	#include "em7186.h"
JoeMiller	0:02c0c2cbc3df	15
JoeMiller	0:02c0c2cbc3df	16
JoeMiller	0:02c0c2cbc3df	17	float em7186_sensor_scale[128];
JoeMiller	0:02c0c2cbc3df	18	u32 timestampNonWake;
JoeMiller	0:02c0c2cbc3df	19	u32 timestampWake;
JoeMiller	0:02c0c2cbc3df	20	u8 printData = 1; // a switch to enable/disable display data (vs logData)
JoeMiller	0:02c0c2cbc3df	21	u8 logData = 0; // a switch to enable/disable log data to SD Card
JoeMiller	0:02c0c2cbc3df	22	u8 sensorEnabled[64];
JoeMiller	0:02c0c2cbc3df	23	u8 haveSensorInfo = 0;
JoeMiller	0:02c0c2cbc3df	24	u16 magMaxRate = 0;
JoeMiller	0:02c0c2cbc3df	25	u16 accelMaxRate = 0;
JoeMiller	0:02c0c2cbc3df	26	u16 gyroMaxRate = 0;
JoeMiller	0:02c0c2cbc3df	27	u32 timestamp;
JoeMiller	0:02c0c2cbc3df	28	u16 timestampPtr[2];
JoeMiller	0:02c0c2cbc3df	29
JoeMiller	0:02c0c2cbc3df	30	SensorDescriptor sensorInformation[128];
JoeMiller	0:02c0c2cbc3df	31	SensorConfiguration sensorConfiguration[127];
JoeMiller	0:02c0c2cbc3df	32
JoeMiller	0:02c0c2cbc3df	33
JoeMiller	0:02c0c2cbc3df	34	ParamInfo sensorInfoParamList[128] =
JoeMiller	0:02c0c2cbc3df	35	{
JoeMiller	0:02c0c2cbc3df	36	{ 0, 16 },
JoeMiller	0:02c0c2cbc3df	37	{ 1, 16 },
JoeMiller	0:02c0c2cbc3df	38	{ 2, 16 },
JoeMiller	0:02c0c2cbc3df	39	{ 3, 16 },
JoeMiller	0:02c0c2cbc3df	40	{ 4, 16 },
JoeMiller	0:02c0c2cbc3df	41	{ 5, 16 },
JoeMiller	0:02c0c2cbc3df	42	{ 6, 16 },
JoeMiller	0:02c0c2cbc3df	43	{ 7, 16 },
JoeMiller	0:02c0c2cbc3df	44	{ 8, 16 },
JoeMiller	0:02c0c2cbc3df	45	{ 9, 16 },
JoeMiller	0:02c0c2cbc3df	46	{ 10, 16 },
JoeMiller	0:02c0c2cbc3df	47	{ 11, 16 },
JoeMiller	0:02c0c2cbc3df	48	{ 12, 16 },
JoeMiller	0:02c0c2cbc3df	49	{ 13, 16 },
JoeMiller	0:02c0c2cbc3df	50	{ 14, 16 },
JoeMiller	0:02c0c2cbc3df	51	{ 15, 16 },
JoeMiller	0:02c0c2cbc3df	52	{ 16, 16 },
JoeMiller	0:02c0c2cbc3df	53	{ 17, 16 },
JoeMiller	0:02c0c2cbc3df	54	{ 18, 16 },
JoeMiller	0:02c0c2cbc3df	55	{ 19, 16 },
JoeMiller	0:02c0c2cbc3df	56	{ 20, 16 },
JoeMiller	0:02c0c2cbc3df	57	{ 21, 16 },
JoeMiller	0:02c0c2cbc3df	58	{ 22, 16 },
JoeMiller	0:02c0c2cbc3df	59	{ 23, 16 },
JoeMiller	0:02c0c2cbc3df	60	{ 24, 16 },
JoeMiller	0:02c0c2cbc3df	61	{ 25, 16 },
JoeMiller	0:02c0c2cbc3df	62	{ 26, 16 },
JoeMiller	0:02c0c2cbc3df	63	{ 27, 16 },
JoeMiller	0:02c0c2cbc3df	64	{ 28, 16 },
JoeMiller	0:02c0c2cbc3df	65	{ 29, 16 },
JoeMiller	0:02c0c2cbc3df	66	{ 30, 16 },
JoeMiller	0:02c0c2cbc3df	67	{ 31, 16 },
JoeMiller	0:02c0c2cbc3df	68	{ 32, 16 },
JoeMiller	0:02c0c2cbc3df	69	{ 33, 16 },
JoeMiller	0:02c0c2cbc3df	70	{ 34, 16 },
JoeMiller	0:02c0c2cbc3df	71	{ 35, 16 },
JoeMiller	0:02c0c2cbc3df	72	{ 36, 16 },
JoeMiller	0:02c0c2cbc3df	73	{ 37, 16 },
JoeMiller	0:02c0c2cbc3df	74	{ 38, 16 },
JoeMiller	0:02c0c2cbc3df	75	{ 39, 16 },
JoeMiller	0:02c0c2cbc3df	76	{ 40, 16 },
JoeMiller	0:02c0c2cbc3df	77	{ 41, 16 },
JoeMiller	0:02c0c2cbc3df	78	{ 42, 16 },
JoeMiller	0:02c0c2cbc3df	79	{ 43, 16 },
JoeMiller	0:02c0c2cbc3df	80	{ 44, 16 },
JoeMiller	0:02c0c2cbc3df	81	{ 45, 16 },
JoeMiller	0:02c0c2cbc3df	82	{ 46, 16 },
JoeMiller	0:02c0c2cbc3df	83	{ 47, 16 },
JoeMiller	0:02c0c2cbc3df	84	{ 48, 16 },
JoeMiller	0:02c0c2cbc3df	85	{ 49, 16 },
JoeMiller	0:02c0c2cbc3df	86	{ 50, 16 },
JoeMiller	0:02c0c2cbc3df	87	{ 51, 16 },
JoeMiller	0:02c0c2cbc3df	88	{ 52, 16 },
JoeMiller	0:02c0c2cbc3df	89	{ 53, 16 },
JoeMiller	0:02c0c2cbc3df	90	{ 54, 16 },
JoeMiller	0:02c0c2cbc3df	91	{ 55, 16 },
JoeMiller	0:02c0c2cbc3df	92	{ 56, 16 },
JoeMiller	0:02c0c2cbc3df	93	{ 57, 16 },
JoeMiller	0:02c0c2cbc3df	94	{ 58, 16 },
JoeMiller	0:02c0c2cbc3df	95	{ 59, 16 },
JoeMiller	0:02c0c2cbc3df	96	{ 60, 16 },
JoeMiller	0:02c0c2cbc3df	97	{ 61, 16 },
JoeMiller	0:02c0c2cbc3df	98	{ 62, 16 },
JoeMiller	0:02c0c2cbc3df	99	{ 63, 16 },
JoeMiller	0:02c0c2cbc3df	100	{ 64, 16 },
JoeMiller	0:02c0c2cbc3df	101	{ 65, 16 },
JoeMiller	0:02c0c2cbc3df	102	{ 66, 16 },
JoeMiller	0:02c0c2cbc3df	103	{ 67, 16 },
JoeMiller	0:02c0c2cbc3df	104	{ 68, 16 },
JoeMiller	0:02c0c2cbc3df	105	{ 69, 16 },
JoeMiller	0:02c0c2cbc3df	106	{ 70, 16 },
JoeMiller	0:02c0c2cbc3df	107	{ 71, 16 },
JoeMiller	0:02c0c2cbc3df	108	{ 72, 16 },
JoeMiller	0:02c0c2cbc3df	109	{ 73, 16 },
JoeMiller	0:02c0c2cbc3df	110	{ 74, 16 },
JoeMiller	0:02c0c2cbc3df	111	{ 75, 16 },
JoeMiller	0:02c0c2cbc3df	112	{ 76, 16 },
JoeMiller	0:02c0c2cbc3df	113	{ 77, 16 },
JoeMiller	0:02c0c2cbc3df	114	{ 78, 16 },
JoeMiller	0:02c0c2cbc3df	115	{ 79, 16 },
JoeMiller	0:02c0c2cbc3df	116	{ 80, 16 },
JoeMiller	0:02c0c2cbc3df	117	{ 81, 16 },
JoeMiller	0:02c0c2cbc3df	118	{ 82, 16 },
JoeMiller	0:02c0c2cbc3df	119	{ 83, 16 },
JoeMiller	0:02c0c2cbc3df	120	{ 84, 16 },
JoeMiller	0:02c0c2cbc3df	121	{ 85, 16 },
JoeMiller	0:02c0c2cbc3df	122	{ 86, 16 },
JoeMiller	0:02c0c2cbc3df	123	{ 87, 16 },
JoeMiller	0:02c0c2cbc3df	124	{ 88, 16 },
JoeMiller	0:02c0c2cbc3df	125	{ 89, 16 },
JoeMiller	0:02c0c2cbc3df	126	{ 90, 16 },
JoeMiller	0:02c0c2cbc3df	127	{ 91, 16 },
JoeMiller	0:02c0c2cbc3df	128	{ 92, 16 },
JoeMiller	0:02c0c2cbc3df	129	{ 93, 16 },
JoeMiller	0:02c0c2cbc3df	130	{ 94, 16 },
JoeMiller	0:02c0c2cbc3df	131	{ 95, 16 },
JoeMiller	0:02c0c2cbc3df	132	{ 96, 16 },
JoeMiller	0:02c0c2cbc3df	133	{ 97, 16 },
JoeMiller	0:02c0c2cbc3df	134	{ 98, 16 },
JoeMiller	0:02c0c2cbc3df	135	{ 99, 16 },
JoeMiller	0:02c0c2cbc3df	136	{ 100, 16 },
JoeMiller	0:02c0c2cbc3df	137	{ 101, 16 },
JoeMiller	0:02c0c2cbc3df	138	{ 102, 16 },
JoeMiller	0:02c0c2cbc3df	139	{ 103, 16 },
JoeMiller	0:02c0c2cbc3df	140	{ 104, 16 },
JoeMiller	0:02c0c2cbc3df	141	{ 105, 16 },
JoeMiller	0:02c0c2cbc3df	142	{ 106, 16 },
JoeMiller	0:02c0c2cbc3df	143	{ 107, 16 },
JoeMiller	0:02c0c2cbc3df	144	{ 108, 16 },
JoeMiller	0:02c0c2cbc3df	145	{ 109, 16 },
JoeMiller	0:02c0c2cbc3df	146	{ 110, 16 },
JoeMiller	0:02c0c2cbc3df	147	{ 111, 16 },
JoeMiller	0:02c0c2cbc3df	148	{ 112, 16 },
JoeMiller	0:02c0c2cbc3df	149	{ 113, 16 },
JoeMiller	0:02c0c2cbc3df	150	{ 114, 16 },
JoeMiller	0:02c0c2cbc3df	151	{ 115, 16 },
JoeMiller	0:02c0c2cbc3df	152	{ 116, 16 },
JoeMiller	0:02c0c2cbc3df	153	{ 117, 16 },
JoeMiller	0:02c0c2cbc3df	154	{ 118, 16 },
JoeMiller	0:02c0c2cbc3df	155	{ 119, 16 },
JoeMiller	0:02c0c2cbc3df	156	{ 120, 16 },
JoeMiller	0:02c0c2cbc3df	157	{ 121, 16 },
JoeMiller	0:02c0c2cbc3df	158	{ 122, 16 },
JoeMiller	0:02c0c2cbc3df	159	{ 123, 16 },
JoeMiller	0:02c0c2cbc3df	160	{ 124, 16 },
JoeMiller	0:02c0c2cbc3df	161	{ 125, 16 },
JoeMiller	0:02c0c2cbc3df	162	{ 126, 16 },
JoeMiller	0:02c0c2cbc3df	163	{ 127, 16 },
JoeMiller	0:02c0c2cbc3df	164	};
JoeMiller	0:02c0c2cbc3df	165
JoeMiller	0:02c0c2cbc3df	166
JoeMiller	0:02c0c2cbc3df	167	u32 em7186_i2c_init()
JoeMiller	0:02c0c2cbc3df	168	{
JoeMiller	0:02c0c2cbc3df	169	u8 buffer[1];
JoeMiller	0:02c0c2cbc3df	170	em7186_i2c_read(PRODUCT_ID_REG, buffer, 1);
JoeMiller	0:02c0c2cbc3df	171
JoeMiller	0:02c0c2cbc3df	172	switch(buffer[0])
JoeMiller	0:02c0c2cbc3df	173	{
JoeMiller	0:02c0c2cbc3df	174	case PRODUCT_ID_7180:
JoeMiller	0:02c0c2cbc3df	175	printf("SENtral found\n\r");
JoeMiller	0:02c0c2cbc3df	176	break;
JoeMiller	0:02c0c2cbc3df	177	case PRODUCT_ID_7184:
JoeMiller	0:02c0c2cbc3df	178	printf("SENtral-A found\n\r");
JoeMiller	0:02c0c2cbc3df	179	break;
JoeMiller	0:02c0c2cbc3df	180	case PRODUCT_ID_7186:
JoeMiller	0:02c0c2cbc3df	181	printf("SENtral-A2 found\n\r");
JoeMiller	0:02c0c2cbc3df	182	break;
JoeMiller	0:02c0c2cbc3df	183	default:
JoeMiller	0:02c0c2cbc3df	184	printf("SENtral NOT FOUND, ID returned = %u\n\r",buffer[0]);
JoeMiller	0:02c0c2cbc3df	185	return 0;
JoeMiller	0:02c0c2cbc3df	186
JoeMiller	0:02c0c2cbc3df	187	}
JoeMiller	0:02c0c2cbc3df	188	return 1;
JoeMiller	0:02c0c2cbc3df	189	}
JoeMiller	0:02c0c2cbc3df	190
JoeMiller	0:02c0c2cbc3df	191	void firmwareTransfer(char srcDestCode)
JoeMiller	0:02c0c2cbc3df	192	{
JoeMiller	0:02c0c2cbc3df	193	switch (srcDestCode) {
JoeMiller	0:02c0c2cbc3df	194	// case 'r':
JoeMiller	0:02c0c2cbc3df	195	// if (displayText) printf("begin Upload of firmware to SENtral RAM\n\r");
JoeMiller	0:02c0c2cbc3df	196	// SENtral_InterruptPin.disable_irq();
JoeMiller	0:02c0c2cbc3df	197	// if (!) {
JoeMiller	0:02c0c2cbc3df	198	// if (displayText) printf("Error uploading SENtral firmware to RAM\n\r");
JoeMiller	0:02c0c2cbc3df	199	// }
JoeMiller	0:02c0c2cbc3df	200	// SENtral_InterruptPin.enable_irq();
JoeMiller	0:02c0c2cbc3df	201	// break;
JoeMiller	0:02c0c2cbc3df	202	//
JoeMiller	0:02c0c2cbc3df	203	// case 'e':
JoeMiller	0:02c0c2cbc3df	204	// if (displayText) printf("begin Upload of firmware to PNI Module EEPROM\n\r");
JoeMiller	0:02c0c2cbc3df	205	// SENtral_InterruptPin.disable_irq();
JoeMiller	0:02c0c2cbc3df	206	// if (!) {
JoeMiller	0:02c0c2cbc3df	207	// if (displayText) printf("Error uploading SENtral firmware to EEPROM\n\r");
JoeMiller	0:02c0c2cbc3df	208	// }
JoeMiller	0:02c0c2cbc3df	209	// SENtral_InterruptPin.enable_irq();
JoeMiller	0:02c0c2cbc3df	210	// break;
JoeMiller	0:02c0c2cbc3df	211	//
JoeMiller	0:02c0c2cbc3df	212	// case 's':
JoeMiller	0:02c0c2cbc3df	213	// if (displayText) printf("begin Upload of firmware to SDCard\n\r");
JoeMiller	0:02c0c2cbc3df	214	// SENtral_InterruptPin.disable_irq();
JoeMiller	0:02c0c2cbc3df	215	// if (!) {
JoeMiller	0:02c0c2cbc3df	216	// if (displayText) printf("Error uploading SENtral firmware to SDCard\n\r");
JoeMiller	0:02c0c2cbc3df	217	// }
JoeMiller	0:02c0c2cbc3df	218	// SENtral_InterruptPin.enable_irq();
JoeMiller	0:02c0c2cbc3df	219	// break;
JoeMiller	0:02c0c2cbc3df	220
JoeMiller	0:02c0c2cbc3df	221	case 'R':
JoeMiller	0:02c0c2cbc3df	222	if (displayText) printf("Transfering firmware from SDCard to SENtral RAM\n\r");
JoeMiller	0:02c0c2cbc3df	223	SENtral_InterruptPin.disable_irq();
JoeMiller	0:02c0c2cbc3df	224	if (!em7186_firmware_Transfer2RAM(fw)) {
JoeMiller	0:02c0c2cbc3df	225	if (displayText) printf("Error transfering SENtral firmware to RAM\n\r");
JoeMiller	0:02c0c2cbc3df	226	break;
JoeMiller	0:02c0c2cbc3df	227	}
JoeMiller	0:02c0c2cbc3df	228	// Enable CPU
JoeMiller	0:02c0c2cbc3df	229	em7186_i2c_write_value(CHIP_CONTROL_REG, CHIP_CONTROL_CPU_RUN);
JoeMiller	0:02c0c2cbc3df	230	em7186_set_scale_factors();
JoeMiller	0:02c0c2cbc3df	231	SENtral_InterruptPin.enable_irq();
JoeMiller	0:02c0c2cbc3df	232	break;
JoeMiller	0:02c0c2cbc3df	233
JoeMiller	0:02c0c2cbc3df	234	case 'E':
JoeMiller	0:02c0c2cbc3df	235	if (displayText) printf("Transfering firmware from SDCard to PNI Module EEPROM\n\r");
JoeMiller	0:02c0c2cbc3df	236	SENtral_InterruptPin.disable_irq();
JoeMiller	0:02c0c2cbc3df	237	if (!em7186_firmware_Transfer2EE(fw)) {
JoeMiller	0:02c0c2cbc3df	238	if (displayText) printf("Error transfering SENtral firmware to EEPROM\n\r");
JoeMiller	0:02c0c2cbc3df	239	}
JoeMiller	0:02c0c2cbc3df	240	break;
JoeMiller	0:02c0c2cbc3df	241
JoeMiller	0:02c0c2cbc3df	242	default:
JoeMiller	0:02c0c2cbc3df	243	serialCommandMode = 0;
JoeMiller	0:02c0c2cbc3df	244
JoeMiller	0:02c0c2cbc3df	245	}
JoeMiller	0:02c0c2cbc3df	246
JoeMiller	0:02c0c2cbc3df	247
JoeMiller	0:02c0c2cbc3df	248	}
JoeMiller	0:02c0c2cbc3df	249
JoeMiller	0:02c0c2cbc3df	250
JoeMiller	0:02c0c2cbc3df	251
JoeMiller	0:02c0c2cbc3df	252
JoeMiller	0:02c0c2cbc3df	253	u32 em7186_i2c_write_value(u8 registerAddress, u8 value)
JoeMiller	0:02c0c2cbc3df	254	{
JoeMiller	0:02c0c2cbc3df	255	u32 status = em7186_i2c_write(registerAddress, &value, 1);
JoeMiller	0:02c0c2cbc3df	256	return status;
JoeMiller	0:02c0c2cbc3df	257	}
JoeMiller	0:02c0c2cbc3df	258
JoeMiller	0:02c0c2cbc3df	259
JoeMiller	0:02c0c2cbc3df	260	//=========================================================================
JoeMiller	0:02c0c2cbc3df	261	// Helper functions
JoeMiller	0:02c0c2cbc3df	262	//=========================================================================
JoeMiller	0:02c0c2cbc3df	263	u8 get_3_axis_sensor_data(SensorData3Axis data, float scale, u8 buffer)
JoeMiller	0:02c0c2cbc3df	264	{
JoeMiller	0:02c0c2cbc3df	265	SensorData3AxisRaw rawData;
JoeMiller	0:02c0c2cbc3df	266	memcpy(&rawData, &buffer[1], sizeof(rawData));
JoeMiller	0:02c0c2cbc3df	267	data->x = (float)rawData.x * scale;
JoeMiller	0:02c0c2cbc3df	268	data->y = (float)rawData.y * scale;
JoeMiller	0:02c0c2cbc3df	269	data->z = (float)rawData.z * scale;
JoeMiller	0:02c0c2cbc3df	270	data->extra = rawData.status;
JoeMiller	0:02c0c2cbc3df	271
JoeMiller	0:02c0c2cbc3df	272	return 1;
JoeMiller	0:02c0c2cbc3df	273	}
JoeMiller	0:02c0c2cbc3df	274	u8 get_3_axis_uncal_sensor_data(SensorData6Axis data, float scale, u8 buffer)
JoeMiller	0:02c0c2cbc3df	275	{
JoeMiller	0:02c0c2cbc3df	276	SensorData3AxisUncalRaw rawData;
JoeMiller	0:02c0c2cbc3df	277	memcpy(&rawData, &buffer[1], sizeof(rawData));
JoeMiller	0:02c0c2cbc3df	278	data->x = (float)rawData.x * scale;
JoeMiller	0:02c0c2cbc3df	279	data->y = (float)rawData.y * scale;
JoeMiller	0:02c0c2cbc3df	280	data->z = (float)rawData.z * scale;
JoeMiller	0:02c0c2cbc3df	281	data->x_bias = (float)rawData.x_bias * scale;
JoeMiller	0:02c0c2cbc3df	282	data->y_bias = (float)rawData.y_bias * scale;
JoeMiller	0:02c0c2cbc3df	283	data->z_bias = (float)rawData.z_bias * scale;
JoeMiller	0:02c0c2cbc3df	284	data->extra = rawData.status;
JoeMiller	0:02c0c2cbc3df	285
JoeMiller	0:02c0c2cbc3df	286	return 1;
JoeMiller	0:02c0c2cbc3df	287	}
JoeMiller	0:02c0c2cbc3df	288	u8 get_rotation_vector(SensorData4Axis rv, float quaternionScale, u8 buffer)
JoeMiller	0:02c0c2cbc3df	289	{
JoeMiller	0:02c0c2cbc3df	290	RotationVectorRaw rawData;
JoeMiller	0:02c0c2cbc3df	291	memcpy(&rawData, &buffer[1], sizeof(rawData));
JoeMiller	0:02c0c2cbc3df	292	rv->x = (float)rawData.x * quaternionScale;
JoeMiller	0:02c0c2cbc3df	293	rv->y = (float)rawData.y * quaternionScale;
JoeMiller	0:02c0c2cbc3df	294	rv->z = (float)rawData.z * quaternionScale;
JoeMiller	0:02c0c2cbc3df	295	rv->w = (float)rawData.w * quaternionScale;
JoeMiller	0:02c0c2cbc3df	296	rv->extra = (float)rawData.accuracy * ((float)M_PI / powf(2.0f, 14.0f));
JoeMiller	0:02c0c2cbc3df	297
JoeMiller	0:02c0c2cbc3df	298	return 1;
JoeMiller	0:02c0c2cbc3df	299	}
JoeMiller	0:02c0c2cbc3df	300
JoeMiller	0:02c0c2cbc3df	301	//=========================================================================
JoeMiller	0:02c0c2cbc3df	302	// Core functions
JoeMiller	0:02c0c2cbc3df	303	//=========================================================================
JoeMiller	0:02c0c2cbc3df	304	u32 em7186_firmware_Transfer2RAM(const u8 *firmwareName)
JoeMiller	0:02c0c2cbc3df	305	{
JoeMiller	0:02c0c2cbc3df	306
JoeMiller	0:02c0c2cbc3df	307	// reset Sentral
JoeMiller	0:02c0c2cbc3df	308	em7186_i2c_write_value(RESET_REQ_REG, 1);
JoeMiller	0:02c0c2cbc3df	309	em7186_i2c_write_value(CHIP_CONTROL_REG, CHIP_CONTROL_HOST_UPLOAD);
JoeMiller	0:02c0c2cbc3df	310
JoeMiller	0:02c0c2cbc3df	311	///////////////////////////////////////////////////////////////////////////
JoeMiller	0:02c0c2cbc3df	312	// Load firmware from file
JoeMiller	0:02c0c2cbc3df	313	///////////////////////////////////////////////////////////////////////////
JoeMiller	0:02c0c2cbc3df	314	// Open firmware file
JoeMiller	0:02c0c2cbc3df	315	FILE *fw_h = fopen(firmwareName, "rb");
JoeMiller	0:02c0c2cbc3df	316	if (!fw_h)
JoeMiller	0:02c0c2cbc3df	317	{
JoeMiller	0:02c0c2cbc3df	318	if (displayText) printf("ERROR: Unable to open file\n\r");
JoeMiller	0:02c0c2cbc3df	319	return 0;
JoeMiller	0:02c0c2cbc3df	320	}
JoeMiller	0:02c0c2cbc3df	321
JoeMiller	0:02c0c2cbc3df	322	if (displayText) printf("Firmware file found\n\r");
JoeMiller	0:02c0c2cbc3df	323
JoeMiller	0:02c0c2cbc3df	324	// Read the firmware header
JoeMiller	0:02c0c2cbc3df	325	struct fwHeader
JoeMiller	0:02c0c2cbc3df	326	{
JoeMiller	0:02c0c2cbc3df	327	u8 imageSignatureLsb;
JoeMiller	0:02c0c2cbc3df	328	u8 imageSignatureMsb;
JoeMiller	0:02c0c2cbc3df	329	u16 flags;
JoeMiller	0:02c0c2cbc3df	330	u32 crc;
JoeMiller	0:02c0c2cbc3df	331	u32 reserved;
JoeMiller	0:02c0c2cbc3df	332	u16 imageLength;
JoeMiller	0:02c0c2cbc3df	333	u16 reserved2;
JoeMiller	0:02c0c2cbc3df	334	} fwHeader;
JoeMiller	0:02c0c2cbc3df	335	int hsize = fread(&fwHeader, 1, FIRMWARE_HEADER_SIZE, fw_h);
JoeMiller	0:02c0c2cbc3df	336	if ( hsize != FIRMWARE_HEADER_SIZE)
JoeMiller	0:02c0c2cbc3df	337	{
JoeMiller	0:02c0c2cbc3df	338	if (displayText) printf("ERROR: File smaller than expected header size %d\n\r", hsize);
JoeMiller	0:02c0c2cbc3df	339	fclose(fw_h);
JoeMiller	0:02c0c2cbc3df	340	return 0;
JoeMiller	0:02c0c2cbc3df	341	}
JoeMiller	0:02c0c2cbc3df	342
JoeMiller	0:02c0c2cbc3df	343	// Validate firmware
JoeMiller	0:02c0c2cbc3df	344	if (fwHeader.imageSignatureLsb != IMAGE_SIGNATURE_LSB \|\| fwHeader.imageSignatureMsb != IMAGE_SIGNATURE_MSG)
JoeMiller	0:02c0c2cbc3df	345	{
JoeMiller	0:02c0c2cbc3df	346	if (displayText) printf("ERROR: Firmware version doesn't match\n\r");
JoeMiller	0:02c0c2cbc3df	347	fclose(fw_h);
JoeMiller	0:02c0c2cbc3df	348	return 0;
JoeMiller	0:02c0c2cbc3df	349	}
JoeMiller	0:02c0c2cbc3df	350
JoeMiller	0:02c0c2cbc3df	351	// TODO: ensure that firmware version matches rom version
JoeMiller	0:02c0c2cbc3df	352
JoeMiller	0:02c0c2cbc3df	353	// Read the firmware image
JoeMiller	0:02c0c2cbc3df	354	u8 fw = (u8 )malloc(fwHeader.imageLength);
JoeMiller	0:02c0c2cbc3df	355	u32 firmwareSize = fread(fw, 1, fwHeader.imageLength, fw_h);
JoeMiller	0:02c0c2cbc3df	356	fclose(fw_h);
JoeMiller	0:02c0c2cbc3df	357	if (firmwareSize != fwHeader.imageLength \|\| firmwareSize % 4)
JoeMiller	0:02c0c2cbc3df	358	{
JoeMiller	0:02c0c2cbc3df	359	if (displayText) printf("ERROR: Firmware size must break on 4 byte boundary\n\r");
JoeMiller	0:02c0c2cbc3df	360	free(fw);
JoeMiller	0:02c0c2cbc3df	361	return 0;
JoeMiller	0:02c0c2cbc3df	362	}
JoeMiller	0:02c0c2cbc3df	363
JoeMiller	0:02c0c2cbc3df	364	///////////////////////////////////////////////////////////////////////////
JoeMiller	0:02c0c2cbc3df	365	// Upload firmware to RAM
JoeMiller	0:02c0c2cbc3df	366	///////////////////////////////////////////////////////////////////////////
JoeMiller	0:02c0c2cbc3df	367	// TODO: set upload address if needed. (zero by default)
JoeMiller	0:02c0c2cbc3df	368
JoeMiller	0:02c0c2cbc3df	369	if (displayText) printf("Uploading Firmware to SENtral RAM...\n\r");
JoeMiller	0:02c0c2cbc3df	370
JoeMiller	0:02c0c2cbc3df	371	s32 bytesWritten = 0,
JoeMiller	0:02c0c2cbc3df	372	bytesRemaining = firmwareSize;
JoeMiller	0:02c0c2cbc3df	373	u8 bytesToWrite, i,
JoeMiller	0:02c0c2cbc3df	374	buf[MAX_I2C_WRITE],
JoeMiller	0:02c0c2cbc3df	375	maxI2cWrite = MAX_I2C_WRITE;
JoeMiller	0:02c0c2cbc3df	376
JoeMiller	0:02c0c2cbc3df	377	while (bytesRemaining > 0)
JoeMiller	0:02c0c2cbc3df	378	{
JoeMiller	0:02c0c2cbc3df	379	if (bytesRemaining < MAX_I2C_WRITE)
JoeMiller	0:02c0c2cbc3df	380	bytesToWrite = bytesRemaining;
JoeMiller	0:02c0c2cbc3df	381	else
JoeMiller	0:02c0c2cbc3df	382	bytesToWrite = maxI2cWrite;
JoeMiller	0:02c0c2cbc3df	383
JoeMiller	0:02c0c2cbc3df	384	// Reverse byte order per word
JoeMiller	0:02c0c2cbc3df	385	for (i = 0; i < bytesToWrite; i += 4)
JoeMiller	0:02c0c2cbc3df	386	{
JoeMiller	0:02c0c2cbc3df	387	buf[i + 0] = fw[bytesWritten + i + 3];
JoeMiller	0:02c0c2cbc3df	388	buf[i + 1] = fw[bytesWritten + i + 2];
JoeMiller	0:02c0c2cbc3df	389	buf[i + 2] = fw[bytesWritten + i + 1];
JoeMiller	0:02c0c2cbc3df	390	buf[i + 3] = fw[bytesWritten + i + 0];
JoeMiller	0:02c0c2cbc3df	391	}
JoeMiller	0:02c0c2cbc3df	392
JoeMiller	0:02c0c2cbc3df	393	if (!em7186_i2c_write(SR_UPLOAD_DATA_REG, buf, bytesToWrite))
JoeMiller	0:02c0c2cbc3df	394	{
JoeMiller	0:02c0c2cbc3df	395	if (displayText) printf("ERROR: Problem writing to sentral\n\r");
JoeMiller	0:02c0c2cbc3df	396	free(fw);
JoeMiller	0:02c0c2cbc3df	397	return 0;
JoeMiller	0:02c0c2cbc3df	398	}
JoeMiller	0:02c0c2cbc3df	399
JoeMiller	0:02c0c2cbc3df	400	bytesRemaining -= bytesToWrite;
JoeMiller	0:02c0c2cbc3df	401	bytesWritten += bytesToWrite;
JoeMiller	0:02c0c2cbc3df	402	}
JoeMiller	0:02c0c2cbc3df	403
JoeMiller	0:02c0c2cbc3df	404	free(fw);
JoeMiller	0:02c0c2cbc3df	405
JoeMiller	0:02c0c2cbc3df	406	if (displayText) printf("Firmware Uploaded......");
JoeMiller	0:02c0c2cbc3df	407
JoeMiller	0:02c0c2cbc3df	408	// Read and verify CRC
JoeMiller	0:02c0c2cbc3df	409	u32 hostCRC = 0;
JoeMiller	0:02c0c2cbc3df	410	em7186_i2c_read(HOST_CRC_REG, (u8*)&hostCRC, 4);
JoeMiller	0:02c0c2cbc3df	411	u32 fwHeaderCRC = fwHeader.crc;//((u32*)fwHeader)[1];
JoeMiller	0:02c0c2cbc3df	412	if (hostCRC != fwHeaderCRC)
JoeMiller	0:02c0c2cbc3df	413	{
JoeMiller	0:02c0c2cbc3df	414	if (displayText) printf("ERROR: CRC doesn't match\n\r");
JoeMiller	0:02c0c2cbc3df	415	return 0;
JoeMiller	0:02c0c2cbc3df	416	}
JoeMiller	0:02c0c2cbc3df	417
JoeMiller	0:02c0c2cbc3df	418	if (displayText) printf(" CRC Match!!\n\r");
JoeMiller	0:02c0c2cbc3df	419	return 1;
JoeMiller	0:02c0c2cbc3df	420	}
JoeMiller	0:02c0c2cbc3df	421
JoeMiller	0:02c0c2cbc3df	422
JoeMiller	0:02c0c2cbc3df	423
JoeMiller	0:02c0c2cbc3df	424
JoeMiller	0:02c0c2cbc3df	425	u32 em7186_firmware_Transfer2EE(const u8 *firmwareName)
JoeMiller	0:02c0c2cbc3df	426	{
JoeMiller	0:02c0c2cbc3df	427	u8 buffer[16];
JoeMiller	0:02c0c2cbc3df	428	u8 trys = 1;
JoeMiller	0:02c0c2cbc3df	429	u16 count;
JoeMiller	0:02c0c2cbc3df	430	// Open firmware file
JoeMiller	0:02c0c2cbc3df	431	FILE *fw_h = fopen(firmwareName, "rb");
JoeMiller	0:02c0c2cbc3df	432	if (!fw_h) {
JoeMiller	0:02c0c2cbc3df	433	if (displayText) printf("ERROR: Unable to open file\n\r");
JoeMiller	0:02c0c2cbc3df	434	return 0;
JoeMiller	0:02c0c2cbc3df	435	}
JoeMiller	0:02c0c2cbc3df	436
JoeMiller	0:02c0c2cbc3df	437	if (displayText) printf("Firmware file found\n\r");
JoeMiller	0:02c0c2cbc3df	438
JoeMiller	0:02c0c2cbc3df	439	// Read the firmware header
JoeMiller	0:02c0c2cbc3df	440	struct fwHeader {
JoeMiller	0:02c0c2cbc3df	441	u8 imageSignatureLsb;
JoeMiller	0:02c0c2cbc3df	442	u8 imageSignatureMsb;
JoeMiller	0:02c0c2cbc3df	443	u16 flags;
JoeMiller	0:02c0c2cbc3df	444	u32 crc;
JoeMiller	0:02c0c2cbc3df	445	u32 reserved;
JoeMiller	0:02c0c2cbc3df	446	u16 imageLength;
JoeMiller	0:02c0c2cbc3df	447	u16 reserved2;
JoeMiller	0:02c0c2cbc3df	448	} fwHeader;
JoeMiller	0:02c0c2cbc3df	449
JoeMiller	0:02c0c2cbc3df	450	int hsize = fread(&fwHeader, 1, FIRMWARE_HEADER_SIZE, fw_h);
JoeMiller	0:02c0c2cbc3df	451	if ( hsize != FIRMWARE_HEADER_SIZE) {
JoeMiller	0:02c0c2cbc3df	452	if (displayText) printf("ERROR: File smaller than expected header size %d\n\r", hsize);
JoeMiller	0:02c0c2cbc3df	453	fclose(fw_h);
JoeMiller	0:02c0c2cbc3df	454	return 0;
JoeMiller	0:02c0c2cbc3df	455	}
JoeMiller	0:02c0c2cbc3df	456
JoeMiller	0:02c0c2cbc3df	457	// Validate firmware
JoeMiller	0:02c0c2cbc3df	458	if (fwHeader.imageSignatureLsb != IMAGE_SIGNATURE_LSB \|\| fwHeader.imageSignatureMsb != IMAGE_SIGNATURE_MSG) {
JoeMiller	0:02c0c2cbc3df	459	if (displayText) printf("ERROR: Firmware version doesn't match\n\r");
JoeMiller	0:02c0c2cbc3df	460	fclose(fw_h);
JoeMiller	0:02c0c2cbc3df	461	return 0;
JoeMiller	0:02c0c2cbc3df	462	}
JoeMiller	0:02c0c2cbc3df	463
JoeMiller	0:02c0c2cbc3df	464	// Read the firmware image From the SD Card
JoeMiller	0:02c0c2cbc3df	465	u8 fw = (u8 )malloc(fwHeader.imageLength);
JoeMiller	0:02c0c2cbc3df	466	u32 firmwareSize = fread(fw, 1, fwHeader.imageLength, fw_h);
JoeMiller	0:02c0c2cbc3df	467	fclose(fw_h);
JoeMiller	0:02c0c2cbc3df	468	if (firmwareSize != fwHeader.imageLength \|\| firmwareSize % 4) {
JoeMiller	0:02c0c2cbc3df	469	if (displayText) printf("ERROR: Firmware size must break on 4 byte boundary\n\r");
JoeMiller	0:02c0c2cbc3df	470	free(fw);
JoeMiller	0:02c0c2cbc3df	471	return 0;
JoeMiller	0:02c0c2cbc3df	472	}
JoeMiller	0:02c0c2cbc3df	473	if (displayText) printf("Firmware size = %u\n\r",firmwareSize);
JoeMiller	0:02c0c2cbc3df	474
JoeMiller	0:02c0c2cbc3df	475
JoeMiller	0:02c0c2cbc3df	476	u8 headerBytes[FIRMWARE_HEADER_SIZE];
JoeMiller	0:02c0c2cbc3df	477	memcpy(headerBytes,(u8*)&fwHeader,FIRMWARE_HEADER_SIZE); // create a bytewise copy of fwHeader
JoeMiller	0:02c0c2cbc3df	478
JoeMiller	0:02c0c2cbc3df	479
JoeMiller	0:02c0c2cbc3df	480	///////////////////////////////////////////////////////////////////////////
JoeMiller	0:02c0c2cbc3df	481	// Upload firmware to EEPROM
JoeMiller	0:02c0c2cbc3df	482	///////////////////////////////////////////////////////////////////////////
JoeMiller	0:02c0c2cbc3df	483
JoeMiller	0:02c0c2cbc3df	484	do {
JoeMiller	0:02c0c2cbc3df	485	// request SENtral passthrough mode
JoeMiller	0:02c0c2cbc3df	486	em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 1);
JoeMiller	0:02c0c2cbc3df	487	em7186_i2c_read(PASS_THROUGH_RDY_REG, buffer, 1);
JoeMiller	0:02c0c2cbc3df	488	} while((buffer[0] != 1) && (++trys < 20));
JoeMiller	0:02c0c2cbc3df	489
JoeMiller	0:02c0c2cbc3df	490
JoeMiller	0:02c0c2cbc3df	491	if (trys <20) {
JoeMiller	0:02c0c2cbc3df	492
JoeMiller	0:02c0c2cbc3df	493	// Reverse byte order per word
JoeMiller	0:02c0c2cbc3df	494
JoeMiller	0:02c0c2cbc3df	495
JoeMiller	0:02c0c2cbc3df	496	if (displayText) printf("SENtral confirmed passthrough mode after %u try(s)\n\r",trys);
JoeMiller	0:02c0c2cbc3df	497
JoeMiller	0:02c0c2cbc3df	498
JoeMiller	0:02c0c2cbc3df	499	// write header portion to EEPROM from SDCard
JoeMiller	0:02c0c2cbc3df	500	//key: EE_Write(u8 I2C_Addr, u16 EE_MemAddr, u8*buffer, u16 length)
JoeMiller	0:02c0c2cbc3df	501	u32 status = EE_Write(0xA0, 0, (u8*)&fwHeader, FIRMWARE_HEADER_SIZE);
JoeMiller	0:02c0c2cbc3df	502	if (displayText) printf("Header Sent to EEPROM......\n\r",firmwareSize);
JoeMiller	0:02c0c2cbc3df	503	wait_ms(5);
JoeMiller	0:02c0c2cbc3df	504	EE_Read(0xA0, 0, buffer, FIRMWARE_HEADER_SIZE);
JoeMiller	0:02c0c2cbc3df	505	status = 0;
JoeMiller	0:02c0c2cbc3df	506
JoeMiller	0:02c0c2cbc3df	507	// Readback EEPROM to verify header write
JoeMiller	0:02c0c2cbc3df	508	for (count = 0; count<FIRMWARE_HEADER_SIZE; count++) {
JoeMiller	0:02c0c2cbc3df	509	if (headerBytes[count] != buffer[count]) {
JoeMiller	0:02c0c2cbc3df	510	status = 1; // 1 = fail
JoeMiller	0:02c0c2cbc3df	511	if (displayText) printf("Failed Header readback from EEPROM at %u, value=%u\n\r",count,headerBytes[count]);
JoeMiller	0:02c0c2cbc3df	512	break;
JoeMiller	0:02c0c2cbc3df	513	}
JoeMiller	0:02c0c2cbc3df	514	}
JoeMiller	0:02c0c2cbc3df	515
JoeMiller	0:02c0c2cbc3df	516	if (!status) {
JoeMiller	0:02c0c2cbc3df	517	u32 bytesWritten = 0;
JoeMiller	0:02c0c2cbc3df	518	u32 bytesRemaining = firmwareSize;
JoeMiller	0:02c0c2cbc3df	519	u8 bytesToWrite,
JoeMiller	0:02c0c2cbc3df	520	maxI2cWrite = 16; // kind of small but ensures page alignment
JoeMiller	0:02c0c2cbc3df	521
JoeMiller	0:02c0c2cbc3df	522	while (bytesRemaining > 0) {
JoeMiller	0:02c0c2cbc3df	523	if (bytesRemaining < maxI2cWrite)
JoeMiller	0:02c0c2cbc3df	524	bytesToWrite = bytesRemaining;
JoeMiller	0:02c0c2cbc3df	525	else
JoeMiller	0:02c0c2cbc3df	526	bytesToWrite = maxI2cWrite;
JoeMiller	0:02c0c2cbc3df	527
JoeMiller	0:02c0c2cbc3df	528	if (!EE_Write(0xA0, (bytesWritten+FIRMWARE_HEADER_SIZE), &fw[bytesWritten], bytesToWrite)) {
JoeMiller	0:02c0c2cbc3df	529	if (displayText) printf("\n\rCould not write to EEPROM\n\r");
JoeMiller	0:02c0c2cbc3df	530	free(fw);
JoeMiller	0:02c0c2cbc3df	531	wait_ms(5);
JoeMiller	0:02c0c2cbc3df	532	em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
JoeMiller	0:02c0c2cbc3df	533	return 0;
JoeMiller	0:02c0c2cbc3df	534	}
JoeMiller	0:02c0c2cbc3df	535	bytesWritten += bytesToWrite;
JoeMiller	0:02c0c2cbc3df	536	bytesRemaining -= bytesToWrite;
JoeMiller	0:02c0c2cbc3df	537	if (displayText) printf("\r%u",bytesWritten);
JoeMiller	0:02c0c2cbc3df	538	wait_ms(5);
JoeMiller	0:02c0c2cbc3df	539	}
JoeMiller	0:02c0c2cbc3df	540
JoeMiller	0:02c0c2cbc3df	541	if (displayText) printf("\n\rFirmware Transfered from SDCard to EEPROM. Now Verifying.....\n\r");
JoeMiller	0:02c0c2cbc3df	542
JoeMiller	0:02c0c2cbc3df	543	u32 bytesRead = 0;
JoeMiller	0:02c0c2cbc3df	544	bytesRemaining = firmwareSize;
JoeMiller	0:02c0c2cbc3df	545	u8 bytesToRead,
JoeMiller	0:02c0c2cbc3df	546	maxI2cRead = 16;
JoeMiller	0:02c0c2cbc3df	547	status = 0;
JoeMiller	0:02c0c2cbc3df	548
JoeMiller	0:02c0c2cbc3df	549	while ((bytesRemaining > 0) && (status == 0)) {
JoeMiller	0:02c0c2cbc3df	550	if (bytesRemaining < maxI2cRead)
JoeMiller	0:02c0c2cbc3df	551	bytesToRead = bytesRemaining;
JoeMiller	0:02c0c2cbc3df	552	else
JoeMiller	0:02c0c2cbc3df	553	bytesToRead = maxI2cRead;
JoeMiller	0:02c0c2cbc3df	554
JoeMiller	0:02c0c2cbc3df	555	if (!EE_Read(0xA0, (bytesRead+FIRMWARE_HEADER_SIZE), buffer, bytesToRead)) {
JoeMiller	0:02c0c2cbc3df	556	if (displayText) printf("\n\rCould not Read EEPROM\n\r");
JoeMiller	0:02c0c2cbc3df	557	free(fw);
JoeMiller	0:02c0c2cbc3df	558	wait_ms(5);
JoeMiller	0:02c0c2cbc3df	559	em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
JoeMiller	0:02c0c2cbc3df	560	return 0;
JoeMiller	0:02c0c2cbc3df	561	}
JoeMiller	0:02c0c2cbc3df	562
JoeMiller	0:02c0c2cbc3df	563	for (count = 0; count<bytesToRead; count++) {
JoeMiller	0:02c0c2cbc3df	564	if (fw[bytesRead+count] != buffer[count]) {
JoeMiller	0:02c0c2cbc3df	565	status = 1; // 1 = fail
JoeMiller	0:02c0c2cbc3df	566	if (displayText) printf("Failed firmware readback from EEPROM at %u, value=%u\n\r",bytesRead+FIRMWARE_HEADER_SIZE+count,buffer[count]);
JoeMiller	0:02c0c2cbc3df	567	wait_ms(5);
JoeMiller	0:02c0c2cbc3df	568	em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
JoeMiller	0:02c0c2cbc3df	569	return 0;
JoeMiller	0:02c0c2cbc3df	570	}
JoeMiller	0:02c0c2cbc3df	571	}
JoeMiller	0:02c0c2cbc3df	572	bytesRead += bytesToRead;
JoeMiller	0:02c0c2cbc3df	573	bytesRemaining -= bytesToRead;
JoeMiller	0:02c0c2cbc3df	574	if (displayText) printf("\r%u",bytesRead);
JoeMiller	0:02c0c2cbc3df	575	wait_ms(1);
JoeMiller	0:02c0c2cbc3df	576	}
JoeMiller	0:02c0c2cbc3df	577	if(status == 0)
JoeMiller	0:02c0c2cbc3df	578	{
JoeMiller	0:02c0c2cbc3df	579	if (displayText) printf("\n\rVerify EEPROM ** SUCCESSFUL ***\n\r");
JoeMiller	0:02c0c2cbc3df	580	}
JoeMiller	0:02c0c2cbc3df	581	} else {
JoeMiller	0:02c0c2cbc3df	582	if (displayText) printf("\r\nCould not write to EEPROM (Header)\n\r");
JoeMiller	0:02c0c2cbc3df	583	wait_ms(5);
JoeMiller	0:02c0c2cbc3df	584	em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
JoeMiller	0:02c0c2cbc3df	585	return 0;
JoeMiller	0:02c0c2cbc3df	586	}
JoeMiller	0:02c0c2cbc3df	587
JoeMiller	0:02c0c2cbc3df	588	free(fw);
JoeMiller	0:02c0c2cbc3df	589
JoeMiller	0:02c0c2cbc3df	590	} else {
JoeMiller	0:02c0c2cbc3df	591	if (displayText) printf("Could not confirm SENtral Passthrough mode\n\r");
JoeMiller	0:02c0c2cbc3df	592	return 0;
JoeMiller	0:02c0c2cbc3df	593	}
JoeMiller	0:02c0c2cbc3df	594
JoeMiller	0:02c0c2cbc3df	595	em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0); // turn off passthrough mode
JoeMiller	0:02c0c2cbc3df	596
JoeMiller	0:02c0c2cbc3df	597	return 1;
JoeMiller	0:02c0c2cbc3df	598	}
JoeMiller	0:02c0c2cbc3df	599
JoeMiller	0:02c0c2cbc3df	600
JoeMiller	0:02c0c2cbc3df	601
JoeMiller	0:02c0c2cbc3df	602
JoeMiller	0:02c0c2cbc3df	603
JoeMiller	0:02c0c2cbc3df	604	u32 em7186_param_read(u8 values, u8 page, ParamInfo paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	605	{
JoeMiller	0:02c0c2cbc3df	606
JoeMiller	0:02c0c2cbc3df	607	u8 i, paramAck, pageSelectValue;
JoeMiller	0:02c0c2cbc3df	608	u16 valIndex = 0;
JoeMiller	0:02c0c2cbc3df	609	for (i = 0; i < numParams; i++)
JoeMiller	0:02c0c2cbc3df	610	{
JoeMiller	0:02c0c2cbc3df	611	pageSelectValue = page \| (paramList[i].size << 4);
JoeMiller	0:02c0c2cbc3df	612	em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, pageSelectValue);
JoeMiller	0:02c0c2cbc3df	613	em7186_i2c_write_value(PARAM_REQUEST_REG, paramList[i].paramNo);
JoeMiller	0:02c0c2cbc3df	614	do
JoeMiller	0:02c0c2cbc3df	615	{
JoeMiller	0:02c0c2cbc3df	616	em7186_i2c_read(PARAM_ACK_REG, &paramAck, 1);
JoeMiller	0:02c0c2cbc3df	617	if (paramAck == 0x80)
JoeMiller	0:02c0c2cbc3df	618	{
JoeMiller	0:02c0c2cbc3df	619	em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
JoeMiller	0:02c0c2cbc3df	620	em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
JoeMiller	0:02c0c2cbc3df	621
JoeMiller	0:02c0c2cbc3df	622
JoeMiller	0:02c0c2cbc3df	623	return 0;
JoeMiller	0:02c0c2cbc3df	624	}
JoeMiller	0:02c0c2cbc3df	625	} while (paramAck != paramList[i].paramNo);
JoeMiller	0:02c0c2cbc3df	626	em7186_i2c_read(PARAM_SAVE_REG, &values[valIndex], paramList[i].size);
JoeMiller	0:02c0c2cbc3df	627	// printf("%u ", values[valIndex]);
JoeMiller	0:02c0c2cbc3df	628	valIndex += paramList[i].size;
JoeMiller	0:02c0c2cbc3df	629	}
JoeMiller	0:02c0c2cbc3df	630	em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
JoeMiller	0:02c0c2cbc3df	631	em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
JoeMiller	0:02c0c2cbc3df	632
JoeMiller	0:02c0c2cbc3df	633
JoeMiller	0:02c0c2cbc3df	634	return 1;
JoeMiller	0:02c0c2cbc3df	635	}
JoeMiller	0:02c0c2cbc3df	636	u32 em7186_param_write(u8 values, u8 page, ParamInfo paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	637	{
JoeMiller	0:02c0c2cbc3df	638
JoeMiller	0:02c0c2cbc3df	639
JoeMiller	0:02c0c2cbc3df	640	u8 i, paramAck, paramNum, pageSelectValue;
JoeMiller	0:02c0c2cbc3df	641	u16 valIndex = 0;
JoeMiller	0:02c0c2cbc3df	642	for (i = 0; i < numParams; i++)
JoeMiller	0:02c0c2cbc3df	643	{
JoeMiller	0:02c0c2cbc3df	644	pageSelectValue = page \| (paramList[i].size << 4);
JoeMiller	0:02c0c2cbc3df	645	em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, pageSelectValue);
JoeMiller	0:02c0c2cbc3df	646
JoeMiller	0:02c0c2cbc3df	647	em7186_i2c_write(PARAM_LOAD_REG, &values[valIndex], (u16)paramList[i].size);
JoeMiller	0:02c0c2cbc3df	648
JoeMiller	0:02c0c2cbc3df	649	paramNum = paramList[i].paramNo \| 0x80;
JoeMiller	0:02c0c2cbc3df	650	em7186_i2c_write_value(PARAM_REQUEST_REG, paramNum);
JoeMiller	0:02c0c2cbc3df	651	do
JoeMiller	0:02c0c2cbc3df	652	{
JoeMiller	0:02c0c2cbc3df	653	em7186_i2c_read(PARAM_ACK_REG, &paramAck, 1);
JoeMiller	0:02c0c2cbc3df	654	if (paramAck == 0x80)
JoeMiller	0:02c0c2cbc3df	655	{
JoeMiller	0:02c0c2cbc3df	656	em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
JoeMiller	0:02c0c2cbc3df	657	em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
JoeMiller	0:02c0c2cbc3df	658
JoeMiller	0:02c0c2cbc3df	659
JoeMiller	0:02c0c2cbc3df	660	return 0;
JoeMiller	0:02c0c2cbc3df	661	}
JoeMiller	0:02c0c2cbc3df	662	} while (paramAck != paramNum);
JoeMiller	0:02c0c2cbc3df	663
JoeMiller	0:02c0c2cbc3df	664	valIndex += paramList[i].size;
JoeMiller	0:02c0c2cbc3df	665	}
JoeMiller	0:02c0c2cbc3df	666
JoeMiller	0:02c0c2cbc3df	667	em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
JoeMiller	0:02c0c2cbc3df	668	em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
JoeMiller	0:02c0c2cbc3df	669
JoeMiller	0:02c0c2cbc3df	670
JoeMiller	0:02c0c2cbc3df	671	return 1;
JoeMiller	0:02c0c2cbc3df	672	}
JoeMiller	0:02c0c2cbc3df	673	u32 em7186_read_fifo(u8 *buffer)
JoeMiller	0:02c0c2cbc3df	674	{
JoeMiller	0:02c0c2cbc3df	675	// Check number of bytes available
JoeMiller	0:02c0c2cbc3df	676	u16 bytesAvailable, bytesRead = 0;
JoeMiller	0:02c0c2cbc3df	677
JoeMiller	0:02c0c2cbc3df	678	em7186_i2c_read(BYTES_REMANING_REG, (u8*)&bytesAvailable, 2);
JoeMiller	0:02c0c2cbc3df	679
JoeMiller	0:02c0c2cbc3df	680
JoeMiller	0:02c0c2cbc3df	681	//printf("FIFO bytesAvailable:%u\n\r",bytesAvailable);
JoeMiller	0:02c0c2cbc3df	682
JoeMiller	0:02c0c2cbc3df	683
JoeMiller	0:02c0c2cbc3df	684
JoeMiller	0:02c0c2cbc3df	685	#define CONTINUOUS_FIFO_READ
JoeMiller	0:02c0c2cbc3df	686	#ifdef CONTINUOUS_FIFO_READ
JoeMiller	0:02c0c2cbc3df	687	bytesRead = em7186_i2c_read(0, buffer, bytesAvailable);
JoeMiller	0:02c0c2cbc3df	688	#else
JoeMiller	0:02c0c2cbc3df	689	u16 i, bytesToRead;
JoeMiller	0:02c0c2cbc3df	690	while (bytesAvailable > 0)
JoeMiller	0:02c0c2cbc3df	691	{
JoeMiller	0:02c0c2cbc3df	692	// Break on 50 byte fifo register block
JoeMiller	0:02c0c2cbc3df	693	bytesToRead = bytesRead % 50 + I2C_MAX_READ > 50 ? 50 - bytesRead % 50 : I2C_MAX_READ;
JoeMiller	0:02c0c2cbc3df	694	// Make sure we don't read more than is available in the fifo
JoeMiller	0:02c0c2cbc3df	695	bytesToRead = min(bytesAvailable, bytesToRead);
JoeMiller	0:02c0c2cbc3df	696	if (!em7186_i2c_read(bytesRead % 50, &buffer[bytesRead], bytesToRead))
JoeMiller	0:02c0c2cbc3df	697	{
JoeMiller	0:02c0c2cbc3df	698
JoeMiller	0:02c0c2cbc3df	699	return 0;
JoeMiller	0:02c0c2cbc3df	700	}
JoeMiller	0:02c0c2cbc3df	701	bytesAvailable -= bytesToRead;
JoeMiller	0:02c0c2cbc3df	702	bytesRead += bytesToRead;
JoeMiller	0:02c0c2cbc3df	703	}
JoeMiller	0:02c0c2cbc3df	704	#endif
JoeMiller	0:02c0c2cbc3df	705
JoeMiller	0:02c0c2cbc3df	706	//printf("FIFO bytesRead:%u\n\r",bytesRead);
JoeMiller	0:02c0c2cbc3df	707
JoeMiller	0:02c0c2cbc3df	708	return bytesRead;
JoeMiller	0:02c0c2cbc3df	709	}
JoeMiller	0:02c0c2cbc3df	710	u32 em7186_parse_next_fifo_block(u8* buffer, u32 size)
JoeMiller	0:02c0c2cbc3df	711	{
JoeMiller	0:02c0c2cbc3df	712	u8 sensorId = buffer[0];
JoeMiller	0:02c0c2cbc3df	713
JoeMiller	0:02c0c2cbc3df	714	// if (sensorId < SENSOR_TYPE_ACCELEROMETER_WAKE \|\|
JoeMiller	0:02c0c2cbc3df	715	// sensorId == SENSOR_TYPE_DEBUG \|\|
JoeMiller	0:02c0c2cbc3df	716	// sensorId == SENSOR_TYPE_TIMESTAMP \|\|
JoeMiller	0:02c0c2cbc3df	717	// sensorId == SENSOR_TYPE_TIMESTAMP_OVERFLOW \|\|
JoeMiller	0:02c0c2cbc3df	718	// sensorId == SENSOR_TYPE_META \|\|
JoeMiller	0:02c0c2cbc3df	719	// sensorId == SENSOR_TYPE_RAW_GYRO \|\|
JoeMiller	0:02c0c2cbc3df	720	// sensorId == SENSOR_TYPE_RAW_MAG \|\|
JoeMiller	0:02c0c2cbc3df	721	// sensorId == SENSOR_TYPE_RAW_ACCEL
JoeMiller	0:02c0c2cbc3df	722	// )
JoeMiller	0:02c0c2cbc3df	723	// {
JoeMiller	0:02c0c2cbc3df	724	// }
JoeMiller	0:02c0c2cbc3df	725	// else
JoeMiller	0:02c0c2cbc3df	726	// {
JoeMiller	0:02c0c2cbc3df	727	// timestamp = timestampWake;
JoeMiller	0:02c0c2cbc3df	728	// timestampPtr = (u16*)&timestampWake;
JoeMiller	0:02c0c2cbc3df	729	// }
JoeMiller	0:02c0c2cbc3df	730
JoeMiller	0:02c0c2cbc3df	731	switch(sensorId)
JoeMiller	0:02c0c2cbc3df	732	{
JoeMiller	0:02c0c2cbc3df	733	case 0:
JoeMiller	0:02c0c2cbc3df	734	{
JoeMiller	0:02c0c2cbc3df	735	//printf("Padding: %d\n\r", size);
JoeMiller	0:02c0c2cbc3df	736	return size;
JoeMiller	0:02c0c2cbc3df	737	}
JoeMiller	0:02c0c2cbc3df	738	case SENSOR_TYPE_ACCELEROMETER:
JoeMiller	0:02c0c2cbc3df	739	case SENSOR_TYPE_ACCELEROMETER_WAKE:
JoeMiller	0:02c0c2cbc3df	740	case SENSOR_TYPE_MAGNETIC_FIELD:
JoeMiller	0:02c0c2cbc3df	741	case SENSOR_TYPE_MAGNETIC_FIELD_WAKE:
JoeMiller	0:02c0c2cbc3df	742	case SENSOR_TYPE_GYROSCOPE:
JoeMiller	0:02c0c2cbc3df	743	case SENSOR_TYPE_GYROSCOPE_WAKE:
JoeMiller	0:02c0c2cbc3df	744	case SENSOR_TYPE_GRAVITY:
JoeMiller	0:02c0c2cbc3df	745	case SENSOR_TYPE_GRAVITY_WAKE:
JoeMiller	0:02c0c2cbc3df	746	case SENSOR_TYPE_LINEAR_ACCELERATION:
JoeMiller	0:02c0c2cbc3df	747	case SENSOR_TYPE_LINEAR_ACCELERATION_WAKE:
JoeMiller	0:02c0c2cbc3df	748	case SENSOR_TYPE_ORIENTATION:
JoeMiller	0:02c0c2cbc3df	749	case SENSOR_TYPE_ORIENTATION_WAKE:
JoeMiller	0:02c0c2cbc3df	750	{
JoeMiller	0:02c0c2cbc3df	751	SensorData3Axis sensorData;
JoeMiller	0:02c0c2cbc3df	752	get_3_axis_sensor_data(&sensorData, em7186_sensor_scale[sensorId], buffer);
JoeMiller	0:02c0c2cbc3df	753	if (printData) printf("%u %s: %f, %f, %f, %f\n\r", timestamp,em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
JoeMiller	0:02c0c2cbc3df	754	if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
JoeMiller	0:02c0c2cbc3df	755
JoeMiller	0:02c0c2cbc3df	756	return 8;
JoeMiller	0:02c0c2cbc3df	757	}
JoeMiller	0:02c0c2cbc3df	758	case SENSOR_TYPE_LIGHT:
JoeMiller	0:02c0c2cbc3df	759	case SENSOR_TYPE_LIGHT_WAKE:
JoeMiller	0:02c0c2cbc3df	760	case SENSOR_TYPE_PROXIMITY:
JoeMiller	0:02c0c2cbc3df	761	case SENSOR_TYPE_PROXIMITY_WAKE:
JoeMiller	0:02c0c2cbc3df	762	case SENSOR_TYPE_RELATIVE_HUMIDITY:
JoeMiller	0:02c0c2cbc3df	763	case SENSOR_TYPE_RELATIVE_HUMIDITY_WAKE:
JoeMiller	0:02c0c2cbc3df	764	case SENSOR_TYPE_ACTIVITY:
JoeMiller	0:02c0c2cbc3df	765	case SENSOR_TYPE_ACTIVITY_WAKE:
JoeMiller	0:02c0c2cbc3df	766	{
JoeMiller	0:02c0c2cbc3df	767	float sensorData = (float)((buffer[2] << 8) + buffer[1]) * em7186_sensor_scale[sensorId];
JoeMiller	0:02c0c2cbc3df	768	if (printData) printf("%u %s: %fLux\n\r", timestamp,em7186_sensor_name[sensorId], sensorData);
JoeMiller	0:02c0c2cbc3df	769	if (logData) fprintf(flog,"%u,%u,%u,%f\n",timestamp, sensorId, sensorData);
JoeMiller	0:02c0c2cbc3df	770	return 3;
JoeMiller	0:02c0c2cbc3df	771	}
JoeMiller	0:02c0c2cbc3df	772	case SENSOR_TYPE_PRESSURE:
JoeMiller	0:02c0c2cbc3df	773	case SENSOR_TYPE_PRESSURE_WAKE:
JoeMiller	0:02c0c2cbc3df	774	{
JoeMiller	0:02c0c2cbc3df	775	float pressure = (float)((buffer[3] << 16) + (buffer[2] << 8) + buffer[1]) * em7186_sensor_scale[sensorId];
JoeMiller	0:02c0c2cbc3df	776	if (printData) printf("%u %s: %fPa\n\r", timestamp, em7186_sensor_name[sensorId], pressure);
JoeMiller	0:02c0c2cbc3df	777	if (logData) fprintf(flog,"%u,%u,%f\n",timestamp,sensorId,pressure);
JoeMiller	0:02c0c2cbc3df	778	return 4;
JoeMiller	0:02c0c2cbc3df	779	}
JoeMiller	0:02c0c2cbc3df	780	case SENSOR_TYPE_TEMPERATURE:
JoeMiller	0:02c0c2cbc3df	781	case SENSOR_TYPE_TEMPERATURE_WAKE:
JoeMiller	0:02c0c2cbc3df	782	case SENSOR_TYPE_AMBIENT_TEMPERATURE:
JoeMiller	0:02c0c2cbc3df	783	case SENSOR_TYPE_AMBIENT_TEMPERATURE_WAKE:
JoeMiller	0:02c0c2cbc3df	784	{
JoeMiller	0:02c0c2cbc3df	785	s16 sensorData = (s16)&buffer[1];
JoeMiller	0:02c0c2cbc3df	786	float temp = (float)(sensorData[0]) * em7186_sensor_scale[sensorId];
JoeMiller	0:02c0c2cbc3df	787	if (printData) printf("%u %s: %fC\n\r", timestamp, em7186_sensor_name[sensorId], temp);
JoeMiller	0:02c0c2cbc3df	788	if (logData) fprintf(flog,"%u,%u,%f\n",timestamp,sensorId, temp);
JoeMiller	0:02c0c2cbc3df	789	return 3;
JoeMiller	0:02c0c2cbc3df	790	}
JoeMiller	0:02c0c2cbc3df	791	case SENSOR_TYPE_ROTATION_VECTOR:
JoeMiller	0:02c0c2cbc3df	792	case SENSOR_TYPE_ROTATION_VECTOR_WAKE:
JoeMiller	0:02c0c2cbc3df	793	case SENSOR_TYPE_GAME_ROTATION_VECTOR:
JoeMiller	0:02c0c2cbc3df	794	case SENSOR_TYPE_GAME_ROTATION_VECTOR_WAKE:
JoeMiller	0:02c0c2cbc3df	795	case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
JoeMiller	0:02c0c2cbc3df	796	case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR_WAKE:
JoeMiller	0:02c0c2cbc3df	797	case SENSOR_TYPE_PDR:
JoeMiller	0:02c0c2cbc3df	798	case SENSOR_TYPE_PDR_WAKE:
JoeMiller	0:02c0c2cbc3df	799	case 60:
JoeMiller	0:02c0c2cbc3df	800	{
JoeMiller	0:02c0c2cbc3df	801	SensorData4Axis rotationVector;
JoeMiller	0:02c0c2cbc3df	802	get_rotation_vector(&rotationVector, em7186_sensor_scale[sensorId], buffer);
JoeMiller	0:02c0c2cbc3df	803	if (printData) printf("%u %s: %f, %f, %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], rotationVector.x, rotationVector.y, rotationVector.z, rotationVector.w, rotationVector.extra);
JoeMiller	0:02c0c2cbc3df	804	if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f,%f\n", timestamp,sensorId, rotationVector.x, rotationVector.y, rotationVector.z, rotationVector.w, rotationVector.extra);
JoeMiller	0:02c0c2cbc3df	805	return 11;
JoeMiller	0:02c0c2cbc3df	806	}
JoeMiller	0:02c0c2cbc3df	807	case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
JoeMiller	0:02c0c2cbc3df	808	case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED_WAKE:
JoeMiller	0:02c0c2cbc3df	809	case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
JoeMiller	0:02c0c2cbc3df	810	case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED_WAKE:
JoeMiller	0:02c0c2cbc3df	811	{
JoeMiller	0:02c0c2cbc3df	812	SensorData6Axis sensorDataUncal;
JoeMiller	0:02c0c2cbc3df	813	get_3_axis_uncal_sensor_data(&sensorDataUncal, em7186_sensor_scale[sensorId], buffer);
JoeMiller	0:02c0c2cbc3df	814	if (printData) printf("%u %s: %f, %f, %f, %f, %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], sensorDataUncal.x, sensorDataUncal.y, sensorDataUncal.z, sensorDataUncal.x_bias, sensorDataUncal.y_bias, sensorDataUncal.z_bias, sensorDataUncal.extra);
JoeMiller	0:02c0c2cbc3df	815	if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f,%f,%f,%f\n", timestamp,sensorId, sensorDataUncal.x, sensorDataUncal.y, sensorDataUncal.z, sensorDataUncal.x_bias, sensorDataUncal.y_bias, sensorDataUncal.z_bias, sensorDataUncal.extra);
JoeMiller	0:02c0c2cbc3df	816	return 14;
JoeMiller	0:02c0c2cbc3df	817	}
JoeMiller	0:02c0c2cbc3df	818	case SENSOR_TYPE_STEP_COUNTER:
JoeMiller	0:02c0c2cbc3df	819	case SENSOR_TYPE_STEP_COUNTER_WAKE:
JoeMiller	0:02c0c2cbc3df	820	{
JoeMiller	0:02c0c2cbc3df	821	u16 sensorData = (buffer[2] << 8) + buffer[1];
JoeMiller	0:02c0c2cbc3df	822	if (printData) printf("%u %s: %u\n\r", timestamp, em7186_sensor_name[sensorId], sensorData);
JoeMiller	0:02c0c2cbc3df	823	if (logData) fprintf(flog,"%u,%u,%u\n", timestamp,sensorId, sensorData);
JoeMiller	0:02c0c2cbc3df	824	return 3;
JoeMiller	0:02c0c2cbc3df	825	}
JoeMiller	0:02c0c2cbc3df	826	case SENSOR_TYPE_HEART_RATE:
JoeMiller	0:02c0c2cbc3df	827	case SENSOR_TYPE_HEART_RATE_WAKE:
JoeMiller	0:02c0c2cbc3df	828	{
JoeMiller	0:02c0c2cbc3df	829	u8 sensorData = buffer[1];
JoeMiller	0:02c0c2cbc3df	830
JoeMiller	0:02c0c2cbc3df	831	// Heart Rate
JoeMiller	0:02c0c2cbc3df	832	if (printData) printf("%u %s: %u\n\r", timestamp, em7186_sensor_name[sensorId], sensorData);
JoeMiller	0:02c0c2cbc3df	833	if (logData) fprintf(flog,"%u,%u,%u\n",timestamp,sensorId, sensorData);
JoeMiller	0:02c0c2cbc3df	834	return 3;
JoeMiller	0:02c0c2cbc3df	835	}
JoeMiller	0:02c0c2cbc3df	836	case SENSOR_TYPE_CAR_MAG_DATA:
JoeMiller	0:02c0c2cbc3df	837	case SENSOR_TYPE_CAR_MAG_DATA_WAKE:
JoeMiller	0:02c0c2cbc3df	838	{
JoeMiller	0:02c0c2cbc3df	839	// u8 SensorId, float[3] RawMagData, u8 Status, u8 expansion data
JoeMiller	0:02c0c2cbc3df	840	float RawMagData[3];
JoeMiller	0:02c0c2cbc3df	841	memcpy(RawMagData, &buffer[1], sizeof(RawMagData));
JoeMiller	0:02c0c2cbc3df	842
JoeMiller	0:02c0c2cbc3df	843	if (printData) printf("%u Car Detect Mag Data = %3.3f,%3.3f,%3.3f\n\r", timestamp, RawMagData[0], RawMagData[1], RawMagData[2]);
JoeMiller	0:02c0c2cbc3df	844	if (logData) fprintf(flog,"%u,%u,%f,%f,%f\n", timestamp,sensorId, RawMagData[0], RawMagData[1], RawMagData[2]);
JoeMiller	0:02c0c2cbc3df	845
JoeMiller	0:02c0c2cbc3df	846	return 15;
JoeMiller	0:02c0c2cbc3df	847	}
JoeMiller	0:02c0c2cbc3df	848
JoeMiller	0:02c0c2cbc3df	849	case SENSOR_TYPE_SIGNIFICANT_MOTION:
JoeMiller	0:02c0c2cbc3df	850	case SENSOR_TYPE_SIGNIFICANT_MOTION_WAKE:
JoeMiller	0:02c0c2cbc3df	851	case SENSOR_TYPE_STEP_DETECTOR:
JoeMiller	0:02c0c2cbc3df	852	case SENSOR_TYPE_STEP_DETECTOR_WAKE:
JoeMiller	0:02c0c2cbc3df	853	case SENSOR_TYPE_TILT_DETECTOR:
JoeMiller	0:02c0c2cbc3df	854	case SENSOR_TYPE_TILT_DETECTOR_WAKE:
JoeMiller	0:02c0c2cbc3df	855	case SENSOR_TYPE_PICK_UP_GESTURE:
JoeMiller	0:02c0c2cbc3df	856	case SENSOR_TYPE_PICK_UP_GESTURE_WAKE:
JoeMiller	0:02c0c2cbc3df	857	case SENSOR_TYPE_WAKE_GESTURE:
JoeMiller	0:02c0c2cbc3df	858	case SENSOR_TYPE_WAKE_GESTURE_WAKE:
JoeMiller	0:02c0c2cbc3df	859	{
JoeMiller	0:02c0c2cbc3df	860	if (printData) printf("%u %s\n\r", timestamp, em7186_sensor_name[sensorId]);
JoeMiller	0:02c0c2cbc3df	861	if (logData) fprintf(flog,"%u,%u\n", timestamp,sensorId);
JoeMiller	0:02c0c2cbc3df	862	return 1;
JoeMiller	0:02c0c2cbc3df	863	}
JoeMiller	0:02c0c2cbc3df	864	case SENSOR_TYPE_TIMESTAMP:
JoeMiller	0:02c0c2cbc3df	865	case SENSOR_TYPE_TIMESTAMP_WAKE:
JoeMiller	0:02c0c2cbc3df	866	{
JoeMiller	0:02c0c2cbc3df	867	u16* uPacket = (u16*)&buffer[1];
JoeMiller	0:02c0c2cbc3df	868	timestampPtr[0] = uPacket[0];
JoeMiller	0:02c0c2cbc3df	869	timestamp = (u32)timestampPtr;
JoeMiller	0:02c0c2cbc3df	870	return 3;
JoeMiller	0:02c0c2cbc3df	871	}
JoeMiller	0:02c0c2cbc3df	872	case SENSOR_TYPE_TIMESTAMP_OVERFLOW:
JoeMiller	0:02c0c2cbc3df	873	case SENSOR_TYPE_TIMESTAMP_OVERFLOW_WAKE:
JoeMiller	0:02c0c2cbc3df	874	{
JoeMiller	0:02c0c2cbc3df	875	u16* uPacket = (u16*)&buffer[1];
JoeMiller	0:02c0c2cbc3df	876	timestampPtr[1] = uPacket[0];
JoeMiller	0:02c0c2cbc3df	877	timestampPtr[0] = 0;
JoeMiller	0:02c0c2cbc3df	878	timestamp = (u32)timestampPtr;
JoeMiller	0:02c0c2cbc3df	879	return 3;
JoeMiller	0:02c0c2cbc3df	880	}
JoeMiller	0:02c0c2cbc3df	881	case SENSOR_TYPE_META:
JoeMiller	0:02c0c2cbc3df	882	case SENSOR_TYPE_META_WAKE:
JoeMiller	0:02c0c2cbc3df	883	{
JoeMiller	0:02c0c2cbc3df	884	if (reportMetaData)
JoeMiller	0:02c0c2cbc3df	885	{
JoeMiller	0:02c0c2cbc3df	886	if (printData)
JoeMiller	0:02c0c2cbc3df	887	{
JoeMiller	0:02c0c2cbc3df	888	if (sensorId == SENSOR_TYPE_META_WAKE)
JoeMiller	0:02c0c2cbc3df	889	{
JoeMiller	0:02c0c2cbc3df	890	printf("%u WAKEUP %s:, %s/%u, %u\n\r", timestamp, em7186_meta_event_name[buffer[1]], em7186_sensor_name[buffer[2]],buffer[2], buffer[3]);
JoeMiller	0:02c0c2cbc3df	891	}
JoeMiller	0:02c0c2cbc3df	892	else
JoeMiller	0:02c0c2cbc3df	893	{
JoeMiller	0:02c0c2cbc3df	894	printf("%u %s: %s/%u, %u\n\r", timestamp, em7186_meta_event_name[buffer[1]], em7186_sensor_name[buffer[2]], buffer[2], buffer[3]);
JoeMiller	0:02c0c2cbc3df	895	// special hook for sample script
JoeMiller	0:02c0c2cbc3df	896	if (buffer[2] == 14)
JoeMiller	0:02c0c2cbc3df	897	{
JoeMiller	0:02c0c2cbc3df	898	if (buffer[3] == 0 ) {
JoeMiller	0:02c0c2cbc3df	899	green_LED = 1;
JoeMiller	0:02c0c2cbc3df	900	printf("PASS **** The RM3100 Magnetometer ASIC and sensors are operating properly\n\r");
JoeMiller	0:02c0c2cbc3df	901	} else {
JoeMiller	0:02c0c2cbc3df	902	green_LED = 1;
JoeMiller	0:02c0c2cbc3df	903	wait(0.1);
JoeMiller	0:02c0c2cbc3df	904	green_LED = 0;
JoeMiller	0:02c0c2cbc3df	905	printf("FAIL **** The RM3100 Magnetometer ASIC and sensors fail code = %u\n\r",buffer[3]);
JoeMiller	0:02c0c2cbc3df	906	}
JoeMiller	0:02c0c2cbc3df	907	}
JoeMiller	0:02c0c2cbc3df	908	}
JoeMiller	0:02c0c2cbc3df	909	}
JoeMiller	0:02c0c2cbc3df	910	if (logData) fprintf(flog,"%u,%u,%u,%u,%u\n",timestamp,sensorId, buffer[1],buffer[2],buffer[3]);
JoeMiller	0:02c0c2cbc3df	911	}
JoeMiller	0:02c0c2cbc3df	912	return 4;
JoeMiller	0:02c0c2cbc3df	913	}
JoeMiller	0:02c0c2cbc3df	914	case SENSOR_TYPE_RAW_ACCEL:
JoeMiller	0:02c0c2cbc3df	915	case SENSOR_TYPE_RAW_GYRO:
JoeMiller	0:02c0c2cbc3df	916	SensorData3Axis sensorData;
JoeMiller	0:02c0c2cbc3df	917	float* fPacket = (float*)&buffer[7];
JoeMiller	0:02c0c2cbc3df	918	sensorData.x = 256 * (s8)buffer[2] + buffer[1]; //s16 will convert to float here
JoeMiller	0:02c0c2cbc3df	919	sensorData.y = 256 * (s8)buffer[4] + buffer[3];
JoeMiller	0:02c0c2cbc3df	920	sensorData.z = 256 * (s8)buffer[6] + buffer[5];
JoeMiller	0:02c0c2cbc3df	921	sensorData.extra = fPacket[0];
JoeMiller	0:02c0c2cbc3df	922	if (printData) printf("%u %s: %3.0f, %3.0f, %3.0f, %3.1f\n\r", timestamp, em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
JoeMiller	0:02c0c2cbc3df	923	if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
JoeMiller	0:02c0c2cbc3df	924	return 11;
JoeMiller	0:02c0c2cbc3df	925	case SENSOR_TYPE_RAW_MAG: //jm modified to s32 for RM3100
JoeMiller	0:02c0c2cbc3df	926	{
JoeMiller	0:02c0c2cbc3df	927	SensorData3Axis sensorData;
JoeMiller	0:02c0c2cbc3df	928	//s32* sPacket = (s32*)&buffer[1];
JoeMiller	0:02c0c2cbc3df	929	float* fPacket = (float*)&buffer[1];
JoeMiller	0:02c0c2cbc3df	930	//sensorData.x = sPacket[0]; //s32 will convert to float here
JoeMiller	0:02c0c2cbc3df	931	//sensorData.y = sPacket[1];
JoeMiller	0:02c0c2cbc3df	932	//sensorData.z = sPacket[2];
JoeMiller	0:02c0c2cbc3df	933	sensorData.x = (float)(buffer[1] + 256 * (buffer[2] + 256 * (buffer[3] + 256 * (buffer[4])))); //s32 will convert to float here
JoeMiller	0:02c0c2cbc3df	934	sensorData.y = buffer[5] + 256 * (buffer[6] + 256 * (buffer[7] + 256 * (buffer[8])));
JoeMiller	0:02c0c2cbc3df	935	sensorData.z = buffer[9] + 256 * (buffer[10] + 256 * (buffer[11] + 256 * (buffer[12])));
JoeMiller	0:02c0c2cbc3df	936	sensorData.extra = fPacket[3];
JoeMiller	0:02c0c2cbc3df	937	if (printData) printf("%u %s: %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z);
JoeMiller	0:02c0c2cbc3df	938	if (logData) fprintf(flog,"%u,%u,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z);
JoeMiller	0:02c0c2cbc3df	939	return 17;
JoeMiller	0:02c0c2cbc3df	940	}
JoeMiller	0:02c0c2cbc3df	941	case SENSOR_TYPE_DEBUG:
JoeMiller	0:02c0c2cbc3df	942	{
JoeMiller	0:02c0c2cbc3df	943	u8 packetSize = buffer[1] & 0x3F;
JoeMiller	0:02c0c2cbc3df	944	u8 i;
JoeMiller	0:02c0c2cbc3df	945	for (i = 0; i < packetSize; i++)
JoeMiller	0:02c0c2cbc3df	946	{
JoeMiller	0:02c0c2cbc3df	947	if (printData) printf("%c", (u8)buffer[2 + i]);
JoeMiller	0:02c0c2cbc3df	948	}
JoeMiller	0:02c0c2cbc3df	949	return 14;
JoeMiller	0:02c0c2cbc3df	950	}
JoeMiller	0:02c0c2cbc3df	951	default:
JoeMiller	0:02c0c2cbc3df	952	{
JoeMiller	0:02c0c2cbc3df	953	// Parsing error or unkown sensor type. Clear out the rest of the
JoeMiller	0:02c0c2cbc3df	954	// buffer and start clean on the next read.
JoeMiller	0:02c0c2cbc3df	955	if (printData) printf("%u Other: 0x%x: %d bytes skipped\n\r", timestamp, buffer[0], size);
JoeMiller	0:02c0c2cbc3df	956	break;
JoeMiller	0:02c0c2cbc3df	957	}
JoeMiller	0:02c0c2cbc3df	958
JoeMiller	0:02c0c2cbc3df	959	} // end switch(sensorId)
JoeMiller	0:02c0c2cbc3df	960
JoeMiller	0:02c0c2cbc3df	961	return 0;
JoeMiller	0:02c0c2cbc3df	962	}
JoeMiller	0:02c0c2cbc3df	963	u32 em7186_parse_fifo(u8* buffer, u32 size)
JoeMiller	0:02c0c2cbc3df	964	{
JoeMiller	0:02c0c2cbc3df	965	u32 index = 0;
JoeMiller	0:02c0c2cbc3df	966	u32 bytesUsed;
JoeMiller	0:02c0c2cbc3df	967	u32 bytesRemaining = size;
JoeMiller	0:02c0c2cbc3df	968
JoeMiller	0:02c0c2cbc3df	969	//if (displayText) printf("FIFO #Bytes to parse:%u\n\r",bytesRemaining);
JoeMiller	0:02c0c2cbc3df	970
JoeMiller	0:02c0c2cbc3df	971
JoeMiller	0:02c0c2cbc3df	972	if (size == 0) return size;
JoeMiller	0:02c0c2cbc3df	973
JoeMiller	0:02c0c2cbc3df	974	do {
JoeMiller	0:02c0c2cbc3df	975	bytesUsed = em7186_parse_next_fifo_block(&buffer[index], bytesRemaining);
JoeMiller	0:02c0c2cbc3df	976	index += bytesUsed;
JoeMiller	0:02c0c2cbc3df	977	bytesRemaining -= bytesUsed;
JoeMiller	0:02c0c2cbc3df	978	} while (bytesUsed > 0 && bytesRemaining > 0);
JoeMiller	0:02c0c2cbc3df	979
JoeMiller	0:02c0c2cbc3df	980	return size - bytesRemaining;
JoeMiller	0:02c0c2cbc3df	981	}
JoeMiller	0:02c0c2cbc3df	982	u32 em7186_set_sensor_rate(u8 sensorId, u16 rate)
JoeMiller	0:02c0c2cbc3df	983	{
JoeMiller	0:02c0c2cbc3df	984	#ifdef BHI160
JoeMiller	0:02c0c2cbc3df	985	u8 paramPage = PARAM_PAGE_SENSOR_INFO;
JoeMiller	0:02c0c2cbc3df	986	ParamInfo param[] = { sensorId + 64, 2 };
JoeMiller	0:02c0c2cbc3df	987	#else
JoeMiller	0:02c0c2cbc3df	988	u8 paramPage = PARAM_PAGE_SENSOR_CONF;
JoeMiller	0:02c0c2cbc3df	989	ParamInfo param[] = { sensorId, 2 };
JoeMiller	0:02c0c2cbc3df	990	#endif
JoeMiller	0:02c0c2cbc3df	991	return em7186_param_write((u8*)&rate, paramPage, param, 1);
JoeMiller	0:02c0c2cbc3df	992	}
JoeMiller	0:02c0c2cbc3df	993
JoeMiller	0:02c0c2cbc3df	994
JoeMiller	0:02c0c2cbc3df	995	u32 em7186_set_sensor_delay(u8 sensorId, u16 delay)
JoeMiller	0:02c0c2cbc3df	996	{
JoeMiller	0:02c0c2cbc3df	997	#ifdef BHI160
JoeMiller	0:02c0c2cbc3df	998	u8 paramPage = PARAM_PAGE_SENSOR_INFO;
JoeMiller	0:02c0c2cbc3df	999	ParamInfo param[] = { sensorId + 64, 2 };
JoeMiller	0:02c0c2cbc3df	1000	#else
JoeMiller	0:02c0c2cbc3df	1001	u8 paramPage = PARAM_PAGE_SENSOR_CONF;
JoeMiller	0:02c0c2cbc3df	1002	ParamInfo param[] = { sensorId, 2 };
JoeMiller	0:02c0c2cbc3df	1003	#endif
JoeMiller	0:02c0c2cbc3df	1004	u16 config[2];
JoeMiller	0:02c0c2cbc3df	1005
JoeMiller	0:02c0c2cbc3df	1006	em7186_param_read((u8*)config, paramPage, param, 1);
JoeMiller	0:02c0c2cbc3df	1007	config[1] = delay;
JoeMiller	0:02c0c2cbc3df	1008	return em7186_param_write((u8*)config, paramPage, param, 1);
JoeMiller	0:02c0c2cbc3df	1009	}
JoeMiller	0:02c0c2cbc3df	1010	u32 em7186_set_meta_event(u8 eventId, u8 enable, u8 enableInt)
JoeMiller	0:02c0c2cbc3df	1011	{
JoeMiller	0:02c0c2cbc3df	1012	unsigned long metaEventBits;
JoeMiller	0:02c0c2cbc3df	1013	ParamInfo param[] = { 1, 8 };
JoeMiller	0:02c0c2cbc3df	1014
JoeMiller	0:02c0c2cbc3df	1015	em7186_param_read((u8*)&metaEventBits, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1016	unsigned long bitMask = !(0x03 << ((eventId - 1) * 2));
JoeMiller	0:02c0c2cbc3df	1017	unsigned long setting = ((enable ? 0x02 : 0x00) \| (enableInt ? 0x01 : 0x00)) << ((eventId - 1) * 2);
JoeMiller	0:02c0c2cbc3df	1018	metaEventBits = (metaEventBits & bitMask) \| setting;
JoeMiller	0:02c0c2cbc3df	1019	em7186_param_write((u8*)&metaEventBits, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1020
JoeMiller	0:02c0c2cbc3df	1021	return 1;
JoeMiller	0:02c0c2cbc3df	1022	}
JoeMiller	0:02c0c2cbc3df	1023	u8 em7186_set_scale_factors()
JoeMiller	0:02c0c2cbc3df	1024	{
JoeMiller	0:02c0c2cbc3df	1025	// Fixed range
JoeMiller	0:02c0c2cbc3df	1026	em7186_sensor_scale[SENSOR_TYPE_ORIENTATION_WAKE] =
JoeMiller	0:02c0c2cbc3df	1027	em7186_sensor_scale[SENSOR_TYPE_ORIENTATION] =
JoeMiller	0:02c0c2cbc3df	1028	360.0f / powf(2.0f, 15.0f); // Fixed
JoeMiller	0:02c0c2cbc3df	1029
JoeMiller	0:02c0c2cbc3df	1030	em7186_sensor_scale[SENSOR_TYPE_ROTATION_VECTOR] =
JoeMiller	0:02c0c2cbc3df	1031	em7186_sensor_scale[SENSOR_TYPE_ROTATION_VECTOR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1032	em7186_sensor_scale[SENSOR_TYPE_GAME_ROTATION_VECTOR] =
JoeMiller	0:02c0c2cbc3df	1033	em7186_sensor_scale[SENSOR_TYPE_GAME_ROTATION_VECTOR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1034	em7186_sensor_scale[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR] =
JoeMiller	0:02c0c2cbc3df	1035	em7186_sensor_scale[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1036	em7186_sensor_scale[SENSOR_TYPE_PDR] =
JoeMiller	0:02c0c2cbc3df	1037	em7186_sensor_scale[SENSOR_TYPE_PDR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1038	em7186_sensor_scale[60] =
JoeMiller	0:02c0c2cbc3df	1039	1.0f / powf(2.0f, 14.0f); // Fixed
JoeMiller	0:02c0c2cbc3df	1040
JoeMiller	0:02c0c2cbc3df	1041
JoeMiller	0:02c0c2cbc3df	1042
JoeMiller	0:02c0c2cbc3df	1043	// Can change during system operation (if desired)
JoeMiller	0:02c0c2cbc3df	1044	float accelDynamicRange = 4.0f; // g
JoeMiller	0:02c0c2cbc3df	1045	float magDynamicRange = 2000.0f; // uT
JoeMiller	0:02c0c2cbc3df	1046	float gyroDynamicRange = 2000.0f; // d/s
JoeMiller	0:02c0c2cbc3df	1047
JoeMiller	0:02c0c2cbc3df	1048	// Change depending on dynamic range
JoeMiller	0:02c0c2cbc3df	1049	em7186_sensor_scale[SENSOR_TYPE_ACCELEROMETER] =
JoeMiller	0:02c0c2cbc3df	1050	em7186_sensor_scale[SENSOR_TYPE_ACCELEROMETER_WAKE] =
JoeMiller	0:02c0c2cbc3df	1051	em7186_sensor_scale[SENSOR_TYPE_GRAVITY] =
JoeMiller	0:02c0c2cbc3df	1052	em7186_sensor_scale[SENSOR_TYPE_GRAVITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1053	em7186_sensor_scale[SENSOR_TYPE_LINEAR_ACCELERATION] =
JoeMiller	0:02c0c2cbc3df	1054	em7186_sensor_scale[SENSOR_TYPE_LINEAR_ACCELERATION_WAKE] =
JoeMiller	0:02c0c2cbc3df	1055	9.81f * accelDynamicRange / powf(2.0f, 15.0f);
JoeMiller	0:02c0c2cbc3df	1056
JoeMiller	0:02c0c2cbc3df	1057	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD] =
JoeMiller	0:02c0c2cbc3df	1058	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_WAKE] =
JoeMiller	0:02c0c2cbc3df	1059	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED] =
JoeMiller	0:02c0c2cbc3df	1060	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED_WAKE] =
JoeMiller	0:02c0c2cbc3df	1061	magDynamicRange / powf(2.0f, 15.0f);
JoeMiller	0:02c0c2cbc3df	1062
JoeMiller	0:02c0c2cbc3df	1063	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE] =
JoeMiller	0:02c0c2cbc3df	1064	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1065	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_UNCALIBRATED] =
JoeMiller	0:02c0c2cbc3df	1066	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_UNCALIBRATED_WAKE] =
JoeMiller	0:02c0c2cbc3df	1067	(3.1415927f / 180.0f) * gyroDynamicRange / powf(2.0f, 15.0f);
JoeMiller	0:02c0c2cbc3df	1068
JoeMiller	0:02c0c2cbc3df	1069	// Could change depending on dynamic range of physical sensor
JoeMiller	0:02c0c2cbc3df	1070	em7186_sensor_scale[SENSOR_TYPE_PRESSURE] =
JoeMiller	0:02c0c2cbc3df	1071	em7186_sensor_scale[SENSOR_TYPE_PRESSURE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1072	1.0f / 128.0f; // Subject to change
JoeMiller	0:02c0c2cbc3df	1073
JoeMiller	0:02c0c2cbc3df	1074	em7186_sensor_scale[SENSOR_TYPE_LIGHT] =
JoeMiller	0:02c0c2cbc3df	1075	em7186_sensor_scale[SENSOR_TYPE_LIGHT_WAKE] =
JoeMiller	0:02c0c2cbc3df	1076	10000.0f / powf(2.0f, 16.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1077
JoeMiller	0:02c0c2cbc3df	1078	em7186_sensor_scale[SENSOR_TYPE_PROXIMITY] =
JoeMiller	0:02c0c2cbc3df	1079	em7186_sensor_scale[SENSOR_TYPE_PROXIMITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1080	100.0f / powf(2.0f, 16.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1081
JoeMiller	0:02c0c2cbc3df	1082	em7186_sensor_scale[SENSOR_TYPE_RELATIVE_HUMIDITY] =
JoeMiller	0:02c0c2cbc3df	1083	em7186_sensor_scale[SENSOR_TYPE_RELATIVE_HUMIDITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1084	100.0f / powf(2.0f, 16.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1085
JoeMiller	0:02c0c2cbc3df	1086	em7186_sensor_scale[SENSOR_TYPE_TEMPERATURE] =
JoeMiller	0:02c0c2cbc3df	1087	em7186_sensor_scale[SENSOR_TYPE_TEMPERATURE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1088	em7186_sensor_scale[SENSOR_TYPE_AMBIENT_TEMPERATURE] =
JoeMiller	0:02c0c2cbc3df	1089	em7186_sensor_scale[SENSOR_TYPE_AMBIENT_TEMPERATURE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1090	150.0f / powf(2.0f, 15.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1091
JoeMiller	0:02c0c2cbc3df	1092	em7186_sensor_scale[SENSOR_TYPE_ACTIVITY] =
JoeMiller	0:02c0c2cbc3df	1093	em7186_sensor_scale[SENSOR_TYPE_ACTIVITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1094	1.0f;
JoeMiller	0:02c0c2cbc3df	1095
JoeMiller	0:02c0c2cbc3df	1096	return 1;
JoeMiller	0:02c0c2cbc3df	1097	}
JoeMiller	0:02c0c2cbc3df	1098
JoeMiller	0:02c0c2cbc3df	1099	/*
JoeMiller	0:02c0c2cbc3df	1100	// Warm start parameter transfer. The parameter list and flags are presently going
JoeMiller	0:02c0c2cbc3df	1101	// through a major revision therefore, these functions are now on hold
JoeMiller	0:02c0c2cbc3df	1102
JoeMiller	0:02c0c2cbc3df	1103	u32 em7186_warm_start_load(const char *filename)
JoeMiller	0:02c0c2cbc3df	1104	{
JoeMiller	0:02c0c2cbc3df	1105	FILE *fin;
JoeMiller	0:02c0c2cbc3df	1106	fin = fopen(filename, "rb");
JoeMiller	0:02c0c2cbc3df	1107	if (!fin) return 0;
JoeMiller	0:02c0c2cbc3df	1108	WarmStartParams warmStartParams;
JoeMiller	0:02c0c2cbc3df	1109	fread(&warmStartParams, 1, sizeof(warmStartParams), fin);
JoeMiller	0:02c0c2cbc3df	1110	fclose(fin);
JoeMiller	0:02c0c2cbc3df	1111
JoeMiller	0:02c0c2cbc3df	1112	em7186_param_write((u8*)&warmStartParams, PARAM_PAGE_WARM_START, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1113
JoeMiller	0:02c0c2cbc3df	1114	return 1;
JoeMiller	0:02c0c2cbc3df	1115	}
JoeMiller	0:02c0c2cbc3df	1116	u32 em7186_warm_start_save(const char *filename)
JoeMiller	0:02c0c2cbc3df	1117	{
JoeMiller	0:02c0c2cbc3df	1118	WarmStartParams warmStartParams;
JoeMiller	0:02c0c2cbc3df	1119	em7186_param_read((u8*)&warmStartParams, PARAM_PAGE_WARM_START, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1120
JoeMiller	0:02c0c2cbc3df	1121	FILE *fout;
JoeMiller	0:02c0c2cbc3df	1122	fout = fopen(filename, "wb");
JoeMiller	0:02c0c2cbc3df	1123	if (!fout) return 0;
JoeMiller	0:02c0c2cbc3df	1124	fwrite(&warmStartParams, 1, sizeof(warmStartParams), fout);
JoeMiller	0:02c0c2cbc3df	1125	fclose(fout);
JoeMiller	0:02c0c2cbc3df	1126	if (displayText) printf("Warm start parmeters Saved to warmstart.dat\n\r");
JoeMiller	0:02c0c2cbc3df	1127	return 1;
JoeMiller	0:02c0c2cbc3df	1128	}*/
JoeMiller	0:02c0c2cbc3df	1129
JoeMiller	0:02c0c2cbc3df	1130
JoeMiller	0:02c0c2cbc3df	1131
JoeMiller	0:02c0c2cbc3df	1132	//=========================================================================
JoeMiller	0:02c0c2cbc3df	1133	// Additional Control Functions
JoeMiller	0:02c0c2cbc3df	1134	//=========================================================================
JoeMiller	0:02c0c2cbc3df	1135	u32 em7186_ap_suspend(u8 suspend)
JoeMiller	0:02c0c2cbc3df	1136	{
JoeMiller	0:02c0c2cbc3df	1137	u32 status = 0;
JoeMiller	0:02c0c2cbc3df	1138
JoeMiller	0:02c0c2cbc3df	1139	if (suspend) status = em7186_i2c_write_value(HOST_INTERFACE_CTRL_REG, HOST_INTERFACE_CTRL_AP_SUSPEND);
JoeMiller	0:02c0c2cbc3df	1140	else status = em7186_i2c_write_value(HOST_INTERFACE_CTRL_REG, 0);
JoeMiller	0:02c0c2cbc3df	1141
JoeMiller	0:02c0c2cbc3df	1142	return status;
JoeMiller	0:02c0c2cbc3df	1143	}
JoeMiller	0:02c0c2cbc3df	1144
JoeMiller	0:02c0c2cbc3df	1145	u32 em7186_flush_sensor(u8 sensorId)
JoeMiller	0:02c0c2cbc3df	1146	{
JoeMiller	0:02c0c2cbc3df	1147	return em7186_i2c_write_value(FIFO_FLUSH_REG, sensorId);
JoeMiller	0:02c0c2cbc3df	1148	}
JoeMiller	0:02c0c2cbc3df	1149
JoeMiller	0:02c0c2cbc3df	1150	void disableSensors()
JoeMiller	0:02c0c2cbc3df	1151	{
JoeMiller	0:02c0c2cbc3df	1152	u8 i;
JoeMiller	0:02c0c2cbc3df	1153	for ( i = 0; i<64 ;i++)
JoeMiller	0:02c0c2cbc3df	1154	{
JoeMiller	0:02c0c2cbc3df	1155	if (sensorEnabled[i] == TRUE)
JoeMiller	0:02c0c2cbc3df	1156	{
JoeMiller	0:02c0c2cbc3df	1157	em7186_set_sensor_rate(sensorEnabled[i+1], 0);
JoeMiller	0:02c0c2cbc3df	1158	sensorEnabled[i] = FALSE;
JoeMiller	0:02c0c2cbc3df	1159	}
JoeMiller	0:02c0c2cbc3df	1160	}
JoeMiller	0:02c0c2cbc3df	1161	}
JoeMiller	0:02c0c2cbc3df	1162
JoeMiller	0:02c0c2cbc3df	1163	u32 em7186_set_fifo_watermarks(u16 minRemainingWakeFifo, u16 minRemainingNonWakeFifo)
JoeMiller	0:02c0c2cbc3df	1164	{
JoeMiller	0:02c0c2cbc3df	1165	// if min remaining values are non-zero the watermark will be set that many bytes from the end of the fifo.
JoeMiller	0:02c0c2cbc3df	1166	// if a zero value is used the watermark is disabled.
JoeMiller	0:02c0c2cbc3df	1167	u16 config[4];
JoeMiller	0:02c0c2cbc3df	1168	ParamInfo param[1] = { { PARAM_FIFO_CONTROL, 8 } };
JoeMiller	0:02c0c2cbc3df	1169
JoeMiller	0:02c0c2cbc3df	1170	em7186_param_read((u8*)config, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1171	config[0] = minRemainingWakeFifo ? config[1] - minRemainingWakeFifo : 0;
JoeMiller	0:02c0c2cbc3df	1172	config[2] = minRemainingNonWakeFifo ? config[3] - minRemainingNonWakeFifo : 0;
JoeMiller	0:02c0c2cbc3df	1173	return em7186_param_write((u8*)config, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1174	}
JoeMiller	0:02c0c2cbc3df	1175
JoeMiller	0:02c0c2cbc3df	1176	u32 em7186_enable_raw_sensors(u8 enableMag, u8 enableAccel, u8 enableGyro)
JoeMiller	0:02c0c2cbc3df	1177	{
JoeMiller	0:02c0c2cbc3df	1178	u8 value = 0x00;
JoeMiller	0:02c0c2cbc3df	1179	if (enableMag) value \|= 0x04;
JoeMiller	0:02c0c2cbc3df	1180	if (enableAccel) value \|= 0x01;
JoeMiller	0:02c0c2cbc3df	1181	if (enableGyro) value \|= 0x02;
JoeMiller	0:02c0c2cbc3df	1182
JoeMiller	0:02c0c2cbc3df	1183	ParamInfo param[1] = { { 127, 1 } };
JoeMiller	0:02c0c2cbc3df	1184
JoeMiller	0:02c0c2cbc3df	1185	em7186_param_write(&value, PARAM_PAGE_WARM_START, param, 1);
JoeMiller	0:02c0c2cbc3df	1186
JoeMiller	0:02c0c2cbc3df	1187	return 1;
JoeMiller	0:02c0c2cbc3df	1188	}
JoeMiller	0:02c0c2cbc3df	1189
JoeMiller	0:02c0c2cbc3df	1190	void resetCpu()
JoeMiller	0:02c0c2cbc3df	1191	{
JoeMiller	0:02c0c2cbc3df	1192	disableSensors();
JoeMiller	0:02c0c2cbc3df	1193	em7186_i2c_write_value(0xB6, 63);
JoeMiller	0:02c0c2cbc3df	1194	}
JoeMiller	0:02c0c2cbc3df	1195
JoeMiller	0:02c0c2cbc3df	1196	u32 paramListSize(ParamInfo *paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	1197	{
JoeMiller	0:02c0c2cbc3df	1198	u8 i;
JoeMiller	0:02c0c2cbc3df	1199	u32 size = 0;
JoeMiller	0:02c0c2cbc3df	1200	for (i = 0; i < numParams; i++)
JoeMiller	0:02c0c2cbc3df	1201	{
JoeMiller	0:02c0c2cbc3df	1202	size += paramList[i].size;
JoeMiller	0:02c0c2cbc3df	1203	}
JoeMiller	0:02c0c2cbc3df	1204	return size;
JoeMiller	0:02c0c2cbc3df	1205	}
JoeMiller	0:02c0c2cbc3df	1206
JoeMiller	0:02c0c2cbc3df	1207	void displaySavedParams(u8 values, ParamInfo paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	1208	{
JoeMiller	0:02c0c2cbc3df	1209	u8 i;
JoeMiller	0:02c0c2cbc3df	1210	u16 valueIndex = 0;
JoeMiller	0:02c0c2cbc3df	1211	for (i = 0; i < numParams; i++)
JoeMiller	0:02c0c2cbc3df	1212	{
JoeMiller	0:02c0c2cbc3df	1213	float* floatVals = (float*)&values[valueIndex];
JoeMiller	0:02c0c2cbc3df	1214	u32* hexVals = (u32*)&values[valueIndex];
JoeMiller	0:02c0c2cbc3df	1215
JoeMiller	0:02c0c2cbc3df	1216	if (paramList[i].size >= 4)
JoeMiller	0:02c0c2cbc3df	1217	{
JoeMiller	0:02c0c2cbc3df	1218	printf("%d 1: %f, %08x\n\r", paramList[i].paramNo, floatVals[0], hexVals[0]);
JoeMiller	0:02c0c2cbc3df	1219	}
JoeMiller	0:02c0c2cbc3df	1220	if (paramList[i].size >= 8)
JoeMiller	0:02c0c2cbc3df	1221	{
JoeMiller	0:02c0c2cbc3df	1222	printf("%d 2: %f, %08x\n\r", paramList[i].paramNo, floatVals[1], hexVals[1]);
JoeMiller	0:02c0c2cbc3df	1223	}
JoeMiller	0:02c0c2cbc3df	1224	if (paramList[i].size == 1)
JoeMiller	0:02c0c2cbc3df	1225	{
JoeMiller	0:02c0c2cbc3df	1226	u8* hexVals = (u8*)&values[valueIndex];
JoeMiller	0:02c0c2cbc3df	1227	printf("%d 1: %d\n\r", paramList[i].paramNo, hexVals[0]);
JoeMiller	0:02c0c2cbc3df	1228	}
JoeMiller	0:02c0c2cbc3df	1229	valueIndex += paramList[i].size;
JoeMiller	0:02c0c2cbc3df	1230	}
JoeMiller	0:02c0c2cbc3df	1231	}
JoeMiller	0:02c0c2cbc3df	1232
JoeMiller	0:02c0c2cbc3df	1233	void displayParams(u8 paramPage, ParamInfo *paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	1234	{
JoeMiller	0:02c0c2cbc3df	1235	u32 size = paramListSize(paramList, numParams);
JoeMiller	0:02c0c2cbc3df	1236	u8 values = (u8 )malloc(size);
JoeMiller	0:02c0c2cbc3df	1237	em7186_param_read(values, paramPage, paramList, numParams);
JoeMiller	0:02c0c2cbc3df	1238	displaySavedParams(values, paramList, numParams);
JoeMiller	0:02c0c2cbc3df	1239	free(values);
JoeMiller	0:02c0c2cbc3df	1240	}
JoeMiller	0:02c0c2cbc3df	1241
JoeMiller	0:02c0c2cbc3df	1242	/*
JoeMiller	0:02c0c2cbc3df	1243	// Warm start parameter transfer. The parameter list and flags are presently going
JoeMiller	0:02c0c2cbc3df	1244	// through a major revision therefore, these functions are now on hold
JoeMiller	0:02c0c2cbc3df	1245
JoeMiller	0:02c0c2cbc3df	1246	void warmStart()
JoeMiller	0:02c0c2cbc3df	1247	{
JoeMiller	0:02c0c2cbc3df	1248	// Save warm start params
JoeMiller	0:02c0c2cbc3df	1249	em7186_warm_start_save(warmStartFile);
JoeMiller	0:02c0c2cbc3df	1250
JoeMiller	0:02c0c2cbc3df	1251	if (displayText) printf("\n\r\n\r-------------------- CPU Reset -------------------------------------\n\r");
JoeMiller	0:02c0c2cbc3df	1252	// Reset the cpu
JoeMiller	0:02c0c2cbc3df	1253	resetCpu();
JoeMiller	0:02c0c2cbc3df	1254
JoeMiller	0:02c0c2cbc3df	1255	if (displayText) printf("\n\r\n\r---------------- Parameters after reset ----------------------------\n\r");
JoeMiller	0:02c0c2cbc3df	1256	// Read the warmstart params after reset
JoeMiller	0:02c0c2cbc3df	1257	displayParams(2, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1258
JoeMiller	0:02c0c2cbc3df	1259	if (displayText) printf("\n\r\n\r---------------- Parameters after upload----------------------------\n\r");
JoeMiller	0:02c0c2cbc3df	1260	// Load warmstart parameters
JoeMiller	0:02c0c2cbc3df	1261	em7186_warm_start_load(warmStartFile);
JoeMiller	0:02c0c2cbc3df	1262
JoeMiller	0:02c0c2cbc3df	1263	// Read the warmstart params after warmstart
JoeMiller	0:02c0c2cbc3df	1264	displayParams(2, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1265	}*/
JoeMiller	0:02c0c2cbc3df	1266
JoeMiller	0:02c0c2cbc3df	1267	char* strBits(void const * const ptr, u8 numBytes, char* str)
JoeMiller	0:02c0c2cbc3df	1268	{
JoeMiller	0:02c0c2cbc3df	1269	u8 bytes = (u8)ptr;
JoeMiller	0:02c0c2cbc3df	1270	u8 i, j;
JoeMiller	0:02c0c2cbc3df	1271	for (i = 0; i < numBytes; i++)
JoeMiller	0:02c0c2cbc3df	1272	{
JoeMiller	0:02c0c2cbc3df	1273	for (j = 0; j < 8; j++)
JoeMiller	0:02c0c2cbc3df	1274	{
JoeMiller	0:02c0c2cbc3df	1275	str[i * 8 + (7 - j)] = bytes[(numBytes - 1) - i] & (1 << j) ? '1' : '0';
JoeMiller	0:02c0c2cbc3df	1276	}
JoeMiller	0:02c0c2cbc3df	1277	}
JoeMiller	0:02c0c2cbc3df	1278	str[numBytes * 8] = '\0';
JoeMiller	0:02c0c2cbc3df	1279	return str;
JoeMiller	0:02c0c2cbc3df	1280	}
JoeMiller	0:02c0c2cbc3df	1281
JoeMiller	0:02c0c2cbc3df	1282	void displayStatusRegisters()
JoeMiller	0:02c0c2cbc3df	1283	{
JoeMiller	0:02c0c2cbc3df	1284	u8 buf[4];
JoeMiller	0:02c0c2cbc3df	1285	char str[17];
JoeMiller	0:02c0c2cbc3df	1286	printf("\n------------ Displaying Status Registers -----------\n\r");
JoeMiller	0:02c0c2cbc3df	1287	em7186_i2c_read(HOST_STATUS_REG, buf, 3);
JoeMiller	0:02c0c2cbc3df	1288	printf("Host Status: % 5u, %s\n\r", buf[0], strBits(&buf[0], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1289	printf("Interrupt Status: % 5u, %s\n\r", buf[1], strBits(&buf[1], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1290	printf("Chip Status: % 5u, %s\n\r", buf[2], strBits(&buf[2], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1291	em7186_i2c_read(ERR_REG, buf, 4);
JoeMiller	0:02c0c2cbc3df	1292	printf("Error Register: % 5u, %s\n\r", buf[0], strBits(&buf[0], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1293	printf("Interrupt State: % 5u, %s\n\r", buf[1], strBits(&buf[1], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1294	printf("Debug Value: % 5u, %s\n\r", buf[2], strBits(&buf[2], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1295	printf("Debug State: % 5u, %s\n\r", buf[3], strBits(&buf[3], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1296	em7186_i2c_read(BYTES_REMANING_REG, buf, 2);
JoeMiller	0:02c0c2cbc3df	1297	u16* v = (u16*)&buf;
JoeMiller	0:02c0c2cbc3df	1298	printf("Bytes Remaining: % 5u, %s\n\n\r", v[0], strBits(&v[0], sizeof(v[0]), str));
JoeMiller	0:02c0c2cbc3df	1299	}
JoeMiller	0:02c0c2cbc3df	1300
JoeMiller	0:02c0c2cbc3df	1301	void displaySensorStatusBits(u8 id, const SensorStatus *status)
JoeMiller	0:02c0c2cbc3df	1302	{
JoeMiller	0:02c0c2cbc3df	1303	printf("\|% 4u \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1304	id, status->dataAvailable, status->i2cNack, status->deviceIdError,
JoeMiller	0:02c0c2cbc3df	1305	status->transientError, status->dataLost, status->powerMode);
JoeMiller	0:02c0c2cbc3df	1306	}
JoeMiller	0:02c0c2cbc3df	1307
JoeMiller	0:02c0c2cbc3df	1308	void displaySensorStatus()
JoeMiller	0:02c0c2cbc3df	1309	{
JoeMiller	0:02c0c2cbc3df	1310	SensorStatus sensorStatus[32];
JoeMiller	0:02c0c2cbc3df	1311	ParamInfo param[] = { PARAM_SENSOR_STATUS_BANK_0, 32 };
JoeMiller	0:02c0c2cbc3df	1312	em7186_param_read((u8*)sensorStatus, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1313	printf("+------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1314	printf("\| SENSOR STATUS \|\n\r");
JoeMiller	0:02c0c2cbc3df	1315	printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1316	printf("\| ID \| Data \| I2C \| DEVICE \| Transient \| Data \| Power \|\n\r");
JoeMiller	0:02c0c2cbc3df	1317	printf("\| \| Available \| NACK \| ID ERR \| Error \| Lost \| Mode \|\n\r");
JoeMiller	0:02c0c2cbc3df	1318	printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1319	u8 i;
JoeMiller	0:02c0c2cbc3df	1320	for (i = 0; i < 32; i++)
JoeMiller	0:02c0c2cbc3df	1321	{
JoeMiller	0:02c0c2cbc3df	1322	displaySensorStatusBits(i + 1, &sensorStatus[i]);
JoeMiller	0:02c0c2cbc3df	1323	}
JoeMiller	0:02c0c2cbc3df	1324	param[0].paramNo = PARAM_SENSOR_STATUS_BANK_0 + 4;
JoeMiller	0:02c0c2cbc3df	1325	em7186_param_read((u8*)sensorStatus, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1326	for (i = 0; i < 16; i++)
JoeMiller	0:02c0c2cbc3df	1327	{
JoeMiller	0:02c0c2cbc3df	1328	displaySensorStatusBits(i + 65, &sensorStatus[i]);
JoeMiller	0:02c0c2cbc3df	1329	}
JoeMiller	0:02c0c2cbc3df	1330	printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1331
JoeMiller	0:02c0c2cbc3df	1332	}
JoeMiller	0:02c0c2cbc3df	1333
JoeMiller	0:02c0c2cbc3df	1334	void getPhysicalSensorStatus()
JoeMiller	0:02c0c2cbc3df	1335	{
JoeMiller	0:02c0c2cbc3df	1336	ParamInfo param[] = { PARAM_PHYSICAL_SENSOR_STATUS, 15 };
JoeMiller	0:02c0c2cbc3df	1337	em7186_param_read((u8*)&physicalSensorStatus, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1338	}
JoeMiller	0:02c0c2cbc3df	1339
JoeMiller	0:02c0c2cbc3df	1340	void displayPhysicalSensorStatus()
JoeMiller	0:02c0c2cbc3df	1341	{
JoeMiller	0:02c0c2cbc3df	1342	getPhysicalSensorStatus();
JoeMiller	0:02c0c2cbc3df	1343	printf("+----------------------------------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1344	printf("\| PHYSICAL SENSOR STATUS \|\n\r");
JoeMiller	0:02c0c2cbc3df	1345	printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1346	printf("\| Sensor \| Sample \| Dynamic \| ID \| Data \| I2C \| DEVICE \| Transient \| Data \| Power \|\n\r");
JoeMiller	0:02c0c2cbc3df	1347	printf("\| \| Rate \| Range \| \| Available \| NACK \| ID ERR \| Error \| Lost \| Mode \|\n\r");
JoeMiller	0:02c0c2cbc3df	1348	printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1349
JoeMiller	0:02c0c2cbc3df	1350	printf("\| Accel \| % 4u \| % 4u ", physicalSensorStatus.accel.sampleRate, physicalSensorStatus.accel.dynamicRange);
JoeMiller	0:02c0c2cbc3df	1351	displaySensorStatusBits(1, &physicalSensorStatus.accel.status);
JoeMiller	0:02c0c2cbc3df	1352
JoeMiller	0:02c0c2cbc3df	1353	printf("\| Gyro \| % 4u \| % 4u ", physicalSensorStatus.gyro.sampleRate, physicalSensorStatus.gyro.dynamicRange);
JoeMiller	0:02c0c2cbc3df	1354	displaySensorStatusBits(2, &physicalSensorStatus.gyro.status);
JoeMiller	0:02c0c2cbc3df	1355
JoeMiller	0:02c0c2cbc3df	1356	printf("\| Mag \| % 4u \| % 4u ", physicalSensorStatus.mag.sampleRate, physicalSensorStatus.mag.dynamicRange);
JoeMiller	0:02c0c2cbc3df	1357	displaySensorStatusBits(3, &physicalSensorStatus.mag.status);
JoeMiller	0:02c0c2cbc3df	1358	printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1359	}
JoeMiller	0:02c0c2cbc3df	1360	void displayPhysicalSensorInformation()
JoeMiller	0:02c0c2cbc3df	1361	{
JoeMiller	0:02c0c2cbc3df	1362	typedef struct
JoeMiller	0:02c0c2cbc3df	1363	{
JoeMiller	0:02c0c2cbc3df	1364	u8 sensorType;
JoeMiller	0:02c0c2cbc3df	1365	u8 driverId;
JoeMiller	0:02c0c2cbc3df	1366	u8 driverVersion;
JoeMiller	0:02c0c2cbc3df	1367	u8 current;
JoeMiller	0:02c0c2cbc3df	1368	u16 currentDynamicRange;
JoeMiller	0:02c0c2cbc3df	1369	u8 flags;
JoeMiller	0:02c0c2cbc3df	1370	u8 reserved;
JoeMiller	0:02c0c2cbc3df	1371	u16 currentRate;
JoeMiller	0:02c0c2cbc3df	1372	u8 numAxes;
JoeMiller	0:02c0c2cbc3df	1373	u8 orientationMatrix[5];
JoeMiller	0:02c0c2cbc3df	1374	} PhysicalSensorInformation;
JoeMiller	0:02c0c2cbc3df	1375
JoeMiller	0:02c0c2cbc3df	1376	PhysicalSensorInformation s;
JoeMiller	0:02c0c2cbc3df	1377
JoeMiller	0:02c0c2cbc3df	1378	u64 physicalSensorPresent = 0;
JoeMiller	0:02c0c2cbc3df	1379	ParamInfo param = { 32, 4 };
JoeMiller	0:02c0c2cbc3df	1380	em7186_param_read((u8*)&physicalSensorPresent, PARAM_PAGE_SYSTEM, &param, 1);
JoeMiller	0:02c0c2cbc3df	1381
JoeMiller	0:02c0c2cbc3df	1382	u32 i;
JoeMiller	0:02c0c2cbc3df	1383	param.size = 16;
JoeMiller	0:02c0c2cbc3df	1384
JoeMiller	0:02c0c2cbc3df	1385	printf("\n+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1386	printf("\| Sensor \| Driver \| Driver \| Power \| Current \| Current \| Num \|\n\r");
JoeMiller	0:02c0c2cbc3df	1387	printf("\| \| ID \| Version \| \| Range \| Rate \| Axes \|\n\r");
JoeMiller	0:02c0c2cbc3df	1388	printf("+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1389	for (i = 0; i < 64; i++)
JoeMiller	0:02c0c2cbc3df	1390	{
JoeMiller	0:02c0c2cbc3df	1391	if (physicalSensorPresent & (1LL << i))
JoeMiller	0:02c0c2cbc3df	1392	{
JoeMiller	0:02c0c2cbc3df	1393	param.paramNo = i+32;
JoeMiller	0:02c0c2cbc3df	1394	em7186_param_read((u8*)&s, PARAM_PAGE_SYSTEM, &param, 1);
JoeMiller	0:02c0c2cbc3df	1395	printf("\| %-32s \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1396	em7186_sensor_name[s.sensorType], s.driverId, s.driverVersion, s.current, s.currentDynamicRange, s.currentRate, s.numAxes);
JoeMiller	0:02c0c2cbc3df	1397	}
JoeMiller	0:02c0c2cbc3df	1398	}
JoeMiller	0:02c0c2cbc3df	1399	printf("+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1400	}
JoeMiller	0:02c0c2cbc3df	1401
JoeMiller	0:02c0c2cbc3df	1402	void getSensorInformation()
JoeMiller	0:02c0c2cbc3df	1403	{
JoeMiller	0:02c0c2cbc3df	1404	em7186_param_read((u8*)sensorInformation, PARAM_PAGE_SENSOR_INFO, sensorInfoParamList, sizeof(sensorInfoParamList) / sizeof(sensorInfoParamList[0]));
JoeMiller	0:02c0c2cbc3df	1405	haveSensorInfo = 1;
JoeMiller	0:02c0c2cbc3df	1406
JoeMiller	0:02c0c2cbc3df	1407	magMaxRate = sensorInformation[SENSOR_TYPE_MAGNETIC_FIELD].maxRate;
JoeMiller	0:02c0c2cbc3df	1408	accelMaxRate = sensorInformation[SENSOR_TYPE_ACCELEROMETER].maxRate;
JoeMiller	0:02c0c2cbc3df	1409	gyroMaxRate = sensorInformation[SENSOR_TYPE_GYROSCOPE].maxRate;
JoeMiller	0:02c0c2cbc3df	1410	}
JoeMiller	0:02c0c2cbc3df	1411
JoeMiller	0:02c0c2cbc3df	1412	void getSensorConfiguration()
JoeMiller	0:02c0c2cbc3df	1413	{
JoeMiller	0:02c0c2cbc3df	1414	u8 i;
JoeMiller	0:02c0c2cbc3df	1415	ParamInfo param[1] = { 0, 8 };
JoeMiller	0:02c0c2cbc3df	1416	for (i = 1; i<sizeof(sensorInformation) / sizeof(sensorInformation[0]); i++)
JoeMiller	0:02c0c2cbc3df	1417	{
JoeMiller	0:02c0c2cbc3df	1418	if (sensorInformation[i].sensorId > 0)
JoeMiller	0:02c0c2cbc3df	1419	{
JoeMiller	0:02c0c2cbc3df	1420	param[0].paramNo = sensorInformation[i].sensorId;
JoeMiller	0:02c0c2cbc3df	1421	em7186_param_read((u8*)&sensorConfiguration[i], PARAM_PAGE_SENSOR_CONF, param, 1);
JoeMiller	0:02c0c2cbc3df	1422	}
JoeMiller	0:02c0c2cbc3df	1423	}
JoeMiller	0:02c0c2cbc3df	1424	}
JoeMiller	0:02c0c2cbc3df	1425
JoeMiller	0:02c0c2cbc3df	1426	void displaySensorConfiguration()
JoeMiller	0:02c0c2cbc3df	1427	{
JoeMiller	0:02c0c2cbc3df	1428	if (!haveSensorInfo) { getSensorInformation(); };
JoeMiller	0:02c0c2cbc3df	1429	getSensorConfiguration();
JoeMiller	0:02c0c2cbc3df	1430	printf("+-------------------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1431	printf("\| Sensor Configuration \|\n\r");
JoeMiller	0:02c0c2cbc3df	1432	printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
JoeMiller	0:02c0c2cbc3df	1433	printf("\| Sensor \| Rate \| Delay \| Sensitivity \| Range \|\n\r");
JoeMiller	0:02c0c2cbc3df	1434	printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
JoeMiller	0:02c0c2cbc3df	1435	u8 i;
JoeMiller	0:02c0c2cbc3df	1436	for (i = 0; i < sizeof(sensorInformation) / sizeof(sensorInformation[0]); i++)
JoeMiller	0:02c0c2cbc3df	1437	{
JoeMiller	0:02c0c2cbc3df	1438	if (sensorInformation[i].sensorId > 0)
JoeMiller	0:02c0c2cbc3df	1439	{
JoeMiller	0:02c0c2cbc3df	1440	printf("\| %-32s \| % 5u \| % 5u \| % 11u \| % 6u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1441	em7186_sensor_name[sensorInformation[i].sensorId],
JoeMiller	0:02c0c2cbc3df	1442	sensorConfiguration[i].sampleRate,
JoeMiller	0:02c0c2cbc3df	1443	sensorConfiguration[i].maxReportLatency,
JoeMiller	0:02c0c2cbc3df	1444	sensorConfiguration[i].changeSensitivity,
JoeMiller	0:02c0c2cbc3df	1445	sensorConfiguration[i].dynamicRange);
JoeMiller	0:02c0c2cbc3df	1446	}
JoeMiller	0:02c0c2cbc3df	1447	}
JoeMiller	0:02c0c2cbc3df	1448	printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
JoeMiller	0:02c0c2cbc3df	1449	}
JoeMiller	0:02c0c2cbc3df	1450
JoeMiller	0:02c0c2cbc3df	1451	void displaySensorInformation()
JoeMiller	0:02c0c2cbc3df	1452	{
JoeMiller	0:02c0c2cbc3df	1453	if (!haveSensorInfo) { getSensorInformation(); };
JoeMiller	0:02c0c2cbc3df	1454	printf("+------------------------------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1455	printf("\| Sensor Information \|\n\r");
JoeMiller	0:02c0c2cbc3df	1456	printf("+----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1457	printf("\| Sensor \| Driver \| Power \| Range \| Res \| Rate \| Size \|\n\r");
JoeMiller	0:02c0c2cbc3df	1458	printf("+----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1459	u8 i;
JoeMiller	0:02c0c2cbc3df	1460	for (i = 0; i < sizeof(sensorInfoParamList) / sizeof(sensorInfoParamList[0]); i++)
JoeMiller	0:02c0c2cbc3df	1461	{
JoeMiller	0:02c0c2cbc3df	1462	if (sensorInformation[i].sensorId > 0)
JoeMiller	0:02c0c2cbc3df	1463	{
JoeMiller	0:02c0c2cbc3df	1464	printf("\| %-32s \| % 3u.%-3u \| % 4u \| % 5u \| % 3u \| %-3u-% 4u \| % 3u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1465	em7186_sensor_name[sensorInformation[i].sensorId], sensorInformation[i].driverId,
JoeMiller	0:02c0c2cbc3df	1466	sensorInformation[i].driverVersion, sensorInformation[i].power,
JoeMiller	0:02c0c2cbc3df	1467	sensorInformation[i].maxRange, sensorInformation[i].resolution,
JoeMiller	0:02c0c2cbc3df	1468	sensorInformation[i].minRate, sensorInformation[i].maxRate,
JoeMiller	0:02c0c2cbc3df	1469	sensorInformation[i].eventSize);
JoeMiller	0:02c0c2cbc3df	1470	}
JoeMiller	0:02c0c2cbc3df	1471	}
JoeMiller	0:02c0c2cbc3df	1472	printf("+----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1473	}
JoeMiller	0:02c0c2cbc3df	1474
JoeMiller	0:02c0c2cbc3df	1475
JoeMiller	0:02c0c2cbc3df	1476	void read_BuildVersion(void) {
JoeMiller	0:02c0c2cbc3df	1477
JoeMiller	0:02c0c2cbc3df	1478	ParamInfo param; // elements: {u8 paramNo; u8 size;}
JoeMiller	0:02c0c2cbc3df	1479
JoeMiller	0:02c0c2cbc3df	1480	printf("\n\r\n\rSENtral Firmware build version information from PramIO Page 9\n\r");
JoeMiller	0:02c0c2cbc3df	1481
JoeMiller	0:02c0c2cbc3df	1482	struct pni_version {
JoeMiller	0:02c0c2cbc3df	1483	u8 major;
JoeMiller	0:02c0c2cbc3df	1484	u8 minor;
JoeMiller	0:02c0c2cbc3df	1485	u8 patch;
JoeMiller	0:02c0c2cbc3df	1486	u8 other;
JoeMiller	0:02c0c2cbc3df	1487	u32 build;
JoeMiller	0:02c0c2cbc3df	1488	};
JoeMiller	0:02c0c2cbc3df	1489
JoeMiller	0:02c0c2cbc3df	1490	struct pni_version pni_ver;
JoeMiller	0:02c0c2cbc3df	1491
JoeMiller	0:02c0c2cbc3df	1492
JoeMiller	0:02c0c2cbc3df	1493	param.paramNo = 1;
JoeMiller	0:02c0c2cbc3df	1494	param.size = 8;
JoeMiller	0:02c0c2cbc3df	1495	em7186_param_read((u8 *)&pni_ver, 9, &param, 1);
JoeMiller	0:02c0c2cbc3df	1496
JoeMiller	0:02c0c2cbc3df	1497	printf(" major = %u\n\r", pni_ver.major);
JoeMiller	0:02c0c2cbc3df	1498	printf(" minor = %u\n\r", pni_ver.minor);
JoeMiller	0:02c0c2cbc3df	1499	printf(" patch = %u\n\r", pni_ver.patch);
JoeMiller	0:02c0c2cbc3df	1500	printf(" other = %u\n\r", pni_ver.other);
JoeMiller	0:02c0c2cbc3df	1501	printf(" build = %u\n\r", pni_ver.build);
JoeMiller	0:02c0c2cbc3df	1502
JoeMiller	0:02c0c2cbc3df	1503
JoeMiller	0:02c0c2cbc3df	1504	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 Jul 2016
Imports:	31
Forks:	0
Commits:	10
Dependents:	0
Dependencies:	2
Followers:	16

Components

PNI M&M Amber Module

em7186.cpp@1:b0a205c9b958, 2016-07-22 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Components

Important Information for this Arm website

Access Warning