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@9:fd5bf0a4c774, 2019-08-07 (annotated)

Committer:: pni_olesik
Date:: Wed Aug 07 23:38:35 2019 +0000
Revision:: 9:fd5bf0a4c774
Parent:: 6:4ba3f9a8191f

em7186.cpp line 927 missing brackets around one of the cases causes a warning, but in later Mbed OS causes an Error.

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:
pni_olesik	9:fd5bf0a4c774	916	{
JoeMiller	0:02c0c2cbc3df	917	SensorData3Axis sensorData;
JoeMiller	0:02c0c2cbc3df	918	float* fPacket = (float*)&buffer[7];
JoeMiller	0:02c0c2cbc3df	919	sensorData.x = 256 * (s8)buffer[2] + buffer[1]; //s16 will convert to float here
JoeMiller	0:02c0c2cbc3df	920	sensorData.y = 256 * (s8)buffer[4] + buffer[3];
JoeMiller	0:02c0c2cbc3df	921	sensorData.z = 256 * (s8)buffer[6] + buffer[5];
JoeMiller	0:02c0c2cbc3df	922	sensorData.extra = fPacket[0];
JoeMiller	0:02c0c2cbc3df	923	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	924	if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
JoeMiller	0:02c0c2cbc3df	925	return 11;
pni_olesik	9:fd5bf0a4c774	926	}
JoeMiller	0:02c0c2cbc3df	927	case SENSOR_TYPE_RAW_MAG: //jm modified to s32 for RM3100
JoeMiller	0:02c0c2cbc3df	928	{
JoeMiller	0:02c0c2cbc3df	929	SensorData3Axis sensorData;
JoeMiller	0:02c0c2cbc3df	930	//s32* sPacket = (s32*)&buffer[1];
JoeMiller	0:02c0c2cbc3df	931	float* fPacket = (float*)&buffer[1];
JoeMiller	0:02c0c2cbc3df	932	//sensorData.x = sPacket[0]; //s32 will convert to float here
JoeMiller	0:02c0c2cbc3df	933	//sensorData.y = sPacket[1];
JoeMiller	0:02c0c2cbc3df	934	//sensorData.z = sPacket[2];
JoeMiller	0:02c0c2cbc3df	935	sensorData.x = (float)(buffer[1] + 256 * (buffer[2] + 256 * (buffer[3] + 256 * (buffer[4])))); //s32 will convert to float here
JoeMiller	0:02c0c2cbc3df	936	sensorData.y = buffer[5] + 256 * (buffer[6] + 256 * (buffer[7] + 256 * (buffer[8])));
JoeMiller	0:02c0c2cbc3df	937	sensorData.z = buffer[9] + 256 * (buffer[10] + 256 * (buffer[11] + 256 * (buffer[12])));
JoeMiller	0:02c0c2cbc3df	938	sensorData.extra = fPacket[3];
JoeMiller	0:02c0c2cbc3df	939	if (printData) printf("%u %s: %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z);
JoeMiller	0:02c0c2cbc3df	940	if (logData) fprintf(flog,"%u,%u,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z);
JoeMiller	0:02c0c2cbc3df	941	return 17;
JoeMiller	0:02c0c2cbc3df	942	}
JoeMiller	0:02c0c2cbc3df	943	case SENSOR_TYPE_DEBUG:
JoeMiller	0:02c0c2cbc3df	944	{
JoeMiller	0:02c0c2cbc3df	945	u8 packetSize = buffer[1] & 0x3F;
JoeMiller	0:02c0c2cbc3df	946	u8 i;
JoeMiller	0:02c0c2cbc3df	947	for (i = 0; i < packetSize; i++)
JoeMiller	0:02c0c2cbc3df	948	{
JoeMiller	0:02c0c2cbc3df	949	if (printData) printf("%c", (u8)buffer[2 + i]);
JoeMiller	0:02c0c2cbc3df	950	}
JoeMiller	0:02c0c2cbc3df	951	return 14;
JoeMiller	0:02c0c2cbc3df	952	}
JoeMiller	0:02c0c2cbc3df	953	default:
JoeMiller	0:02c0c2cbc3df	954	{
JoeMiller	0:02c0c2cbc3df	955	// Parsing error or unkown sensor type. Clear out the rest of the
JoeMiller	0:02c0c2cbc3df	956	// buffer and start clean on the next read.
JoeMiller	0:02c0c2cbc3df	957	if (printData) printf("%u Other: 0x%x: %d bytes skipped\n\r", timestamp, buffer[0], size);
JoeMiller	0:02c0c2cbc3df	958	break;
JoeMiller	0:02c0c2cbc3df	959	}
JoeMiller	0:02c0c2cbc3df	960
JoeMiller	0:02c0c2cbc3df	961	} // end switch(sensorId)
JoeMiller	0:02c0c2cbc3df	962
JoeMiller	0:02c0c2cbc3df	963	return 0;
JoeMiller	0:02c0c2cbc3df	964	}
JoeMiller	0:02c0c2cbc3df	965	u32 em7186_parse_fifo(u8* buffer, u32 size)
JoeMiller	0:02c0c2cbc3df	966	{
JoeMiller	0:02c0c2cbc3df	967	u32 index = 0;
JoeMiller	0:02c0c2cbc3df	968	u32 bytesUsed;
JoeMiller	0:02c0c2cbc3df	969	u32 bytesRemaining = size;
JoeMiller	0:02c0c2cbc3df	970
JoeMiller	0:02c0c2cbc3df	971	//if (displayText) printf("FIFO #Bytes to parse:%u\n\r",bytesRemaining);
JoeMiller	0:02c0c2cbc3df	972
JoeMiller	0:02c0c2cbc3df	973
JoeMiller	0:02c0c2cbc3df	974	if (size == 0) return size;
JoeMiller	0:02c0c2cbc3df	975
JoeMiller	0:02c0c2cbc3df	976	do {
JoeMiller	0:02c0c2cbc3df	977	bytesUsed = em7186_parse_next_fifo_block(&buffer[index], bytesRemaining);
JoeMiller	0:02c0c2cbc3df	978	index += bytesUsed;
JoeMiller	0:02c0c2cbc3df	979	bytesRemaining -= bytesUsed;
JoeMiller	0:02c0c2cbc3df	980	} while (bytesUsed > 0 && bytesRemaining > 0);
JoeMiller	0:02c0c2cbc3df	981
JoeMiller	0:02c0c2cbc3df	982	return size - bytesRemaining;
JoeMiller	0:02c0c2cbc3df	983	}
JoeMiller	0:02c0c2cbc3df	984	u32 em7186_set_sensor_rate(u8 sensorId, u16 rate)
JoeMiller	0:02c0c2cbc3df	985	{
JoeMiller	0:02c0c2cbc3df	986	#ifdef BHI160
JoeMiller	0:02c0c2cbc3df	987	u8 paramPage = PARAM_PAGE_SENSOR_INFO;
JoeMiller	0:02c0c2cbc3df	988	ParamInfo param[] = { sensorId + 64, 2 };
JoeMiller	0:02c0c2cbc3df	989	#else
JoeMiller	0:02c0c2cbc3df	990	u8 paramPage = PARAM_PAGE_SENSOR_CONF;
JoeMiller	0:02c0c2cbc3df	991	ParamInfo param[] = { sensorId, 2 };
JoeMiller	0:02c0c2cbc3df	992	#endif
JoeMiller	0:02c0c2cbc3df	993	return em7186_param_write((u8*)&rate, paramPage, param, 1);
JoeMiller	0:02c0c2cbc3df	994	}
JoeMiller	0:02c0c2cbc3df	995
JoeMiller	0:02c0c2cbc3df	996
JoeMiller	0:02c0c2cbc3df	997	u32 em7186_set_sensor_delay(u8 sensorId, u16 delay)
JoeMiller	0:02c0c2cbc3df	998	{
JoeMiller	0:02c0c2cbc3df	999	#ifdef BHI160
JoeMiller	0:02c0c2cbc3df	1000	u8 paramPage = PARAM_PAGE_SENSOR_INFO;
JoeMiller	0:02c0c2cbc3df	1001	ParamInfo param[] = { sensorId + 64, 2 };
JoeMiller	0:02c0c2cbc3df	1002	#else
JoeMiller	0:02c0c2cbc3df	1003	u8 paramPage = PARAM_PAGE_SENSOR_CONF;
JoeMiller	0:02c0c2cbc3df	1004	ParamInfo param[] = { sensorId, 2 };
JoeMiller	0:02c0c2cbc3df	1005	#endif
JoeMiller	0:02c0c2cbc3df	1006	u16 config[2];
JoeMiller	0:02c0c2cbc3df	1007
JoeMiller	0:02c0c2cbc3df	1008	em7186_param_read((u8*)config, paramPage, param, 1);
JoeMiller	0:02c0c2cbc3df	1009	config[1] = delay;
JoeMiller	0:02c0c2cbc3df	1010	return em7186_param_write((u8*)config, paramPage, param, 1);
JoeMiller	0:02c0c2cbc3df	1011	}
JoeMiller	0:02c0c2cbc3df	1012	u32 em7186_set_meta_event(u8 eventId, u8 enable, u8 enableInt)
JoeMiller	0:02c0c2cbc3df	1013	{
JoeMiller	0:02c0c2cbc3df	1014	unsigned long metaEventBits;
JoeMiller	0:02c0c2cbc3df	1015	ParamInfo param[] = { 1, 8 };
JoeMiller	0:02c0c2cbc3df	1016
JoeMiller	0:02c0c2cbc3df	1017	em7186_param_read((u8*)&metaEventBits, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1018	unsigned long bitMask = !(0x03 << ((eventId - 1) * 2));
JoeMiller	0:02c0c2cbc3df	1019	unsigned long setting = ((enable ? 0x02 : 0x00) \| (enableInt ? 0x01 : 0x00)) << ((eventId - 1) * 2);
JoeMiller	0:02c0c2cbc3df	1020	metaEventBits = (metaEventBits & bitMask) \| setting;
JoeMiller	0:02c0c2cbc3df	1021	em7186_param_write((u8*)&metaEventBits, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1022
JoeMiller	0:02c0c2cbc3df	1023	return 1;
JoeMiller	0:02c0c2cbc3df	1024	}
JoeMiller	0:02c0c2cbc3df	1025	u8 em7186_set_scale_factors()
JoeMiller	0:02c0c2cbc3df	1026	{
JoeMiller	0:02c0c2cbc3df	1027	// Fixed range
JoeMiller	0:02c0c2cbc3df	1028	em7186_sensor_scale[SENSOR_TYPE_ORIENTATION_WAKE] =
JoeMiller	0:02c0c2cbc3df	1029	em7186_sensor_scale[SENSOR_TYPE_ORIENTATION] =
JoeMiller	0:02c0c2cbc3df	1030	360.0f / powf(2.0f, 15.0f); // Fixed
JoeMiller	0:02c0c2cbc3df	1031
JoeMiller	0:02c0c2cbc3df	1032	em7186_sensor_scale[SENSOR_TYPE_ROTATION_VECTOR] =
JoeMiller	0:02c0c2cbc3df	1033	em7186_sensor_scale[SENSOR_TYPE_ROTATION_VECTOR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1034	em7186_sensor_scale[SENSOR_TYPE_GAME_ROTATION_VECTOR] =
JoeMiller	0:02c0c2cbc3df	1035	em7186_sensor_scale[SENSOR_TYPE_GAME_ROTATION_VECTOR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1036	em7186_sensor_scale[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR] =
JoeMiller	0:02c0c2cbc3df	1037	em7186_sensor_scale[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1038	em7186_sensor_scale[SENSOR_TYPE_PDR] =
JoeMiller	0:02c0c2cbc3df	1039	em7186_sensor_scale[SENSOR_TYPE_PDR_WAKE] =
JoeMiller	0:02c0c2cbc3df	1040	em7186_sensor_scale[60] =
JoeMiller	0:02c0c2cbc3df	1041	1.0f / powf(2.0f, 14.0f); // Fixed
JoeMiller	0:02c0c2cbc3df	1042
JoeMiller	0:02c0c2cbc3df	1043
JoeMiller	0:02c0c2cbc3df	1044
JoeMiller	0:02c0c2cbc3df	1045	// Can change during system operation (if desired)
JoeMiller	0:02c0c2cbc3df	1046	float accelDynamicRange = 4.0f; // g
JoeMiller	6:4ba3f9a8191f	1047	float magDynamicRange = 1000.0f; // uT
JoeMiller	0:02c0c2cbc3df	1048	float gyroDynamicRange = 2000.0f; // d/s
JoeMiller	0:02c0c2cbc3df	1049
JoeMiller	0:02c0c2cbc3df	1050	// Change depending on dynamic range
JoeMiller	0:02c0c2cbc3df	1051	em7186_sensor_scale[SENSOR_TYPE_ACCELEROMETER] =
JoeMiller	0:02c0c2cbc3df	1052	em7186_sensor_scale[SENSOR_TYPE_ACCELEROMETER_WAKE] =
JoeMiller	0:02c0c2cbc3df	1053	em7186_sensor_scale[SENSOR_TYPE_GRAVITY] =
JoeMiller	0:02c0c2cbc3df	1054	em7186_sensor_scale[SENSOR_TYPE_GRAVITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1055	em7186_sensor_scale[SENSOR_TYPE_LINEAR_ACCELERATION] =
JoeMiller	0:02c0c2cbc3df	1056	em7186_sensor_scale[SENSOR_TYPE_LINEAR_ACCELERATION_WAKE] =
JoeMiller	0:02c0c2cbc3df	1057	9.81f * accelDynamicRange / powf(2.0f, 15.0f);
JoeMiller	0:02c0c2cbc3df	1058
JoeMiller	0:02c0c2cbc3df	1059	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD] =
JoeMiller	0:02c0c2cbc3df	1060	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_WAKE] =
JoeMiller	0:02c0c2cbc3df	1061	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED] =
JoeMiller	0:02c0c2cbc3df	1062	em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED_WAKE] =
JoeMiller	0:02c0c2cbc3df	1063	magDynamicRange / powf(2.0f, 15.0f);
JoeMiller	0:02c0c2cbc3df	1064
JoeMiller	0:02c0c2cbc3df	1065	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE] =
JoeMiller	0:02c0c2cbc3df	1066	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1067	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_UNCALIBRATED] =
JoeMiller	0:02c0c2cbc3df	1068	em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_UNCALIBRATED_WAKE] =
JoeMiller	0:02c0c2cbc3df	1069	(3.1415927f / 180.0f) * gyroDynamicRange / powf(2.0f, 15.0f);
JoeMiller	0:02c0c2cbc3df	1070
JoeMiller	0:02c0c2cbc3df	1071	// Could change depending on dynamic range of physical sensor
JoeMiller	0:02c0c2cbc3df	1072	em7186_sensor_scale[SENSOR_TYPE_PRESSURE] =
JoeMiller	0:02c0c2cbc3df	1073	em7186_sensor_scale[SENSOR_TYPE_PRESSURE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1074	1.0f / 128.0f; // Subject to change
JoeMiller	0:02c0c2cbc3df	1075
JoeMiller	0:02c0c2cbc3df	1076	em7186_sensor_scale[SENSOR_TYPE_LIGHT] =
JoeMiller	0:02c0c2cbc3df	1077	em7186_sensor_scale[SENSOR_TYPE_LIGHT_WAKE] =
JoeMiller	0:02c0c2cbc3df	1078	10000.0f / powf(2.0f, 16.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1079
JoeMiller	0:02c0c2cbc3df	1080	em7186_sensor_scale[SENSOR_TYPE_PROXIMITY] =
JoeMiller	0:02c0c2cbc3df	1081	em7186_sensor_scale[SENSOR_TYPE_PROXIMITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1082	100.0f / powf(2.0f, 16.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1083
JoeMiller	0:02c0c2cbc3df	1084	em7186_sensor_scale[SENSOR_TYPE_RELATIVE_HUMIDITY] =
JoeMiller	0:02c0c2cbc3df	1085	em7186_sensor_scale[SENSOR_TYPE_RELATIVE_HUMIDITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1086	100.0f / powf(2.0f, 16.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1087
JoeMiller	0:02c0c2cbc3df	1088	em7186_sensor_scale[SENSOR_TYPE_TEMPERATURE] =
JoeMiller	0:02c0c2cbc3df	1089	em7186_sensor_scale[SENSOR_TYPE_TEMPERATURE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1090	em7186_sensor_scale[SENSOR_TYPE_AMBIENT_TEMPERATURE] =
JoeMiller	0:02c0c2cbc3df	1091	em7186_sensor_scale[SENSOR_TYPE_AMBIENT_TEMPERATURE_WAKE] =
JoeMiller	0:02c0c2cbc3df	1092	150.0f / powf(2.0f, 15.0f); // Subject to change
JoeMiller	0:02c0c2cbc3df	1093
JoeMiller	0:02c0c2cbc3df	1094	em7186_sensor_scale[SENSOR_TYPE_ACTIVITY] =
JoeMiller	0:02c0c2cbc3df	1095	em7186_sensor_scale[SENSOR_TYPE_ACTIVITY_WAKE] =
JoeMiller	0:02c0c2cbc3df	1096	1.0f;
JoeMiller	0:02c0c2cbc3df	1097
JoeMiller	0:02c0c2cbc3df	1098	return 1;
JoeMiller	0:02c0c2cbc3df	1099	}
JoeMiller	0:02c0c2cbc3df	1100
JoeMiller	0:02c0c2cbc3df	1101	/*
JoeMiller	0:02c0c2cbc3df	1102	// Warm start parameter transfer. The parameter list and flags are presently going
JoeMiller	0:02c0c2cbc3df	1103	// through a major revision therefore, these functions are now on hold
JoeMiller	0:02c0c2cbc3df	1104
JoeMiller	0:02c0c2cbc3df	1105	u32 em7186_warm_start_load(const char *filename)
JoeMiller	0:02c0c2cbc3df	1106	{
JoeMiller	0:02c0c2cbc3df	1107	FILE *fin;
JoeMiller	0:02c0c2cbc3df	1108	fin = fopen(filename, "rb");
JoeMiller	0:02c0c2cbc3df	1109	if (!fin) return 0;
JoeMiller	0:02c0c2cbc3df	1110	WarmStartParams warmStartParams;
JoeMiller	0:02c0c2cbc3df	1111	fread(&warmStartParams, 1, sizeof(warmStartParams), fin);
JoeMiller	0:02c0c2cbc3df	1112	fclose(fin);
JoeMiller	0:02c0c2cbc3df	1113
JoeMiller	0:02c0c2cbc3df	1114	em7186_param_write((u8*)&warmStartParams, PARAM_PAGE_WARM_START, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1115
JoeMiller	0:02c0c2cbc3df	1116	return 1;
JoeMiller	0:02c0c2cbc3df	1117	}
JoeMiller	0:02c0c2cbc3df	1118	u32 em7186_warm_start_save(const char *filename)
JoeMiller	0:02c0c2cbc3df	1119	{
JoeMiller	0:02c0c2cbc3df	1120	WarmStartParams warmStartParams;
JoeMiller	0:02c0c2cbc3df	1121	em7186_param_read((u8*)&warmStartParams, PARAM_PAGE_WARM_START, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1122
JoeMiller	0:02c0c2cbc3df	1123	FILE *fout;
JoeMiller	0:02c0c2cbc3df	1124	fout = fopen(filename, "wb");
JoeMiller	0:02c0c2cbc3df	1125	if (!fout) return 0;
JoeMiller	0:02c0c2cbc3df	1126	fwrite(&warmStartParams, 1, sizeof(warmStartParams), fout);
JoeMiller	0:02c0c2cbc3df	1127	fclose(fout);
JoeMiller	0:02c0c2cbc3df	1128	if (displayText) printf("Warm start parmeters Saved to warmstart.dat\n\r");
JoeMiller	0:02c0c2cbc3df	1129	return 1;
JoeMiller	0:02c0c2cbc3df	1130	}*/
JoeMiller	0:02c0c2cbc3df	1131
JoeMiller	0:02c0c2cbc3df	1132
JoeMiller	0:02c0c2cbc3df	1133
JoeMiller	0:02c0c2cbc3df	1134	//=========================================================================
JoeMiller	0:02c0c2cbc3df	1135	// Additional Control Functions
JoeMiller	0:02c0c2cbc3df	1136	//=========================================================================
JoeMiller	0:02c0c2cbc3df	1137	u32 em7186_ap_suspend(u8 suspend)
JoeMiller	0:02c0c2cbc3df	1138	{
JoeMiller	0:02c0c2cbc3df	1139	u32 status = 0;
JoeMiller	0:02c0c2cbc3df	1140
JoeMiller	0:02c0c2cbc3df	1141	if (suspend) status = em7186_i2c_write_value(HOST_INTERFACE_CTRL_REG, HOST_INTERFACE_CTRL_AP_SUSPEND);
JoeMiller	0:02c0c2cbc3df	1142	else status = em7186_i2c_write_value(HOST_INTERFACE_CTRL_REG, 0);
JoeMiller	0:02c0c2cbc3df	1143
JoeMiller	0:02c0c2cbc3df	1144	return status;
JoeMiller	0:02c0c2cbc3df	1145	}
JoeMiller	0:02c0c2cbc3df	1146
JoeMiller	0:02c0c2cbc3df	1147	u32 em7186_flush_sensor(u8 sensorId)
JoeMiller	0:02c0c2cbc3df	1148	{
JoeMiller	0:02c0c2cbc3df	1149	return em7186_i2c_write_value(FIFO_FLUSH_REG, sensorId);
JoeMiller	0:02c0c2cbc3df	1150	}
JoeMiller	0:02c0c2cbc3df	1151
JoeMiller	0:02c0c2cbc3df	1152	void disableSensors()
JoeMiller	0:02c0c2cbc3df	1153	{
JoeMiller	0:02c0c2cbc3df	1154	u8 i;
JoeMiller	0:02c0c2cbc3df	1155	for ( i = 0; i<64 ;i++)
JoeMiller	0:02c0c2cbc3df	1156	{
JoeMiller	0:02c0c2cbc3df	1157	if (sensorEnabled[i] == TRUE)
JoeMiller	0:02c0c2cbc3df	1158	{
JoeMiller	0:02c0c2cbc3df	1159	em7186_set_sensor_rate(sensorEnabled[i+1], 0);
JoeMiller	0:02c0c2cbc3df	1160	sensorEnabled[i] = FALSE;
JoeMiller	0:02c0c2cbc3df	1161	}
JoeMiller	0:02c0c2cbc3df	1162	}
JoeMiller	0:02c0c2cbc3df	1163	}
JoeMiller	0:02c0c2cbc3df	1164
JoeMiller	0:02c0c2cbc3df	1165	u32 em7186_set_fifo_watermarks(u16 minRemainingWakeFifo, u16 minRemainingNonWakeFifo)
JoeMiller	0:02c0c2cbc3df	1166	{
JoeMiller	0:02c0c2cbc3df	1167	// if min remaining values are non-zero the watermark will be set that many bytes from the end of the fifo.
JoeMiller	0:02c0c2cbc3df	1168	// if a zero value is used the watermark is disabled.
JoeMiller	0:02c0c2cbc3df	1169	u16 config[4];
JoeMiller	0:02c0c2cbc3df	1170	ParamInfo param[1] = { { PARAM_FIFO_CONTROL, 8 } };
JoeMiller	0:02c0c2cbc3df	1171
JoeMiller	0:02c0c2cbc3df	1172	em7186_param_read((u8*)config, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1173	config[0] = minRemainingWakeFifo ? config[1] - minRemainingWakeFifo : 0;
JoeMiller	0:02c0c2cbc3df	1174	config[2] = minRemainingNonWakeFifo ? config[3] - minRemainingNonWakeFifo : 0;
JoeMiller	0:02c0c2cbc3df	1175	return em7186_param_write((u8*)config, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1176	}
JoeMiller	0:02c0c2cbc3df	1177
JoeMiller	0:02c0c2cbc3df	1178	u32 em7186_enable_raw_sensors(u8 enableMag, u8 enableAccel, u8 enableGyro)
JoeMiller	0:02c0c2cbc3df	1179	{
JoeMiller	0:02c0c2cbc3df	1180	u8 value = 0x00;
JoeMiller	0:02c0c2cbc3df	1181	if (enableMag) value \|= 0x04;
JoeMiller	0:02c0c2cbc3df	1182	if (enableAccel) value \|= 0x01;
JoeMiller	0:02c0c2cbc3df	1183	if (enableGyro) value \|= 0x02;
JoeMiller	0:02c0c2cbc3df	1184
JoeMiller	0:02c0c2cbc3df	1185	ParamInfo param[1] = { { 127, 1 } };
JoeMiller	0:02c0c2cbc3df	1186
JoeMiller	0:02c0c2cbc3df	1187	em7186_param_write(&value, PARAM_PAGE_WARM_START, param, 1);
JoeMiller	0:02c0c2cbc3df	1188
JoeMiller	0:02c0c2cbc3df	1189	return 1;
JoeMiller	0:02c0c2cbc3df	1190	}
JoeMiller	0:02c0c2cbc3df	1191
JoeMiller	0:02c0c2cbc3df	1192	void resetCpu()
JoeMiller	0:02c0c2cbc3df	1193	{
JoeMiller	0:02c0c2cbc3df	1194	disableSensors();
JoeMiller	0:02c0c2cbc3df	1195	em7186_i2c_write_value(0xB6, 63);
JoeMiller	0:02c0c2cbc3df	1196	}
JoeMiller	0:02c0c2cbc3df	1197
JoeMiller	0:02c0c2cbc3df	1198	u32 paramListSize(ParamInfo *paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	1199	{
JoeMiller	0:02c0c2cbc3df	1200	u8 i;
JoeMiller	0:02c0c2cbc3df	1201	u32 size = 0;
JoeMiller	0:02c0c2cbc3df	1202	for (i = 0; i < numParams; i++)
JoeMiller	0:02c0c2cbc3df	1203	{
JoeMiller	0:02c0c2cbc3df	1204	size += paramList[i].size;
JoeMiller	0:02c0c2cbc3df	1205	}
JoeMiller	0:02c0c2cbc3df	1206	return size;
JoeMiller	0:02c0c2cbc3df	1207	}
JoeMiller	0:02c0c2cbc3df	1208
JoeMiller	0:02c0c2cbc3df	1209	void displaySavedParams(u8 values, ParamInfo paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	1210	{
JoeMiller	0:02c0c2cbc3df	1211	u8 i;
JoeMiller	0:02c0c2cbc3df	1212	u16 valueIndex = 0;
JoeMiller	0:02c0c2cbc3df	1213	for (i = 0; i < numParams; i++)
JoeMiller	0:02c0c2cbc3df	1214	{
JoeMiller	0:02c0c2cbc3df	1215	float* floatVals = (float*)&values[valueIndex];
JoeMiller	0:02c0c2cbc3df	1216	u32* hexVals = (u32*)&values[valueIndex];
JoeMiller	0:02c0c2cbc3df	1217
JoeMiller	0:02c0c2cbc3df	1218	if (paramList[i].size >= 4)
JoeMiller	0:02c0c2cbc3df	1219	{
JoeMiller	0:02c0c2cbc3df	1220	printf("%d 1: %f, %08x\n\r", paramList[i].paramNo, floatVals[0], hexVals[0]);
JoeMiller	0:02c0c2cbc3df	1221	}
JoeMiller	0:02c0c2cbc3df	1222	if (paramList[i].size >= 8)
JoeMiller	0:02c0c2cbc3df	1223	{
JoeMiller	0:02c0c2cbc3df	1224	printf("%d 2: %f, %08x\n\r", paramList[i].paramNo, floatVals[1], hexVals[1]);
JoeMiller	0:02c0c2cbc3df	1225	}
JoeMiller	0:02c0c2cbc3df	1226	if (paramList[i].size == 1)
JoeMiller	0:02c0c2cbc3df	1227	{
JoeMiller	0:02c0c2cbc3df	1228	u8* hexVals = (u8*)&values[valueIndex];
JoeMiller	0:02c0c2cbc3df	1229	printf("%d 1: %d\n\r", paramList[i].paramNo, hexVals[0]);
JoeMiller	0:02c0c2cbc3df	1230	}
JoeMiller	0:02c0c2cbc3df	1231	valueIndex += paramList[i].size;
JoeMiller	0:02c0c2cbc3df	1232	}
JoeMiller	0:02c0c2cbc3df	1233	}
JoeMiller	0:02c0c2cbc3df	1234
JoeMiller	0:02c0c2cbc3df	1235	void displayParams(u8 paramPage, ParamInfo *paramList, u8 numParams)
JoeMiller	0:02c0c2cbc3df	1236	{
JoeMiller	0:02c0c2cbc3df	1237	u32 size = paramListSize(paramList, numParams);
JoeMiller	0:02c0c2cbc3df	1238	u8 values = (u8 )malloc(size);
JoeMiller	0:02c0c2cbc3df	1239	em7186_param_read(values, paramPage, paramList, numParams);
JoeMiller	0:02c0c2cbc3df	1240	displaySavedParams(values, paramList, numParams);
JoeMiller	0:02c0c2cbc3df	1241	free(values);
JoeMiller	0:02c0c2cbc3df	1242	}
JoeMiller	0:02c0c2cbc3df	1243
JoeMiller	0:02c0c2cbc3df	1244	/*
JoeMiller	0:02c0c2cbc3df	1245	// Warm start parameter transfer. The parameter list and flags are presently going
JoeMiller	0:02c0c2cbc3df	1246	// through a major revision therefore, these functions are now on hold
JoeMiller	0:02c0c2cbc3df	1247
JoeMiller	0:02c0c2cbc3df	1248	void warmStart()
JoeMiller	0:02c0c2cbc3df	1249	{
JoeMiller	0:02c0c2cbc3df	1250	// Save warm start params
JoeMiller	0:02c0c2cbc3df	1251	em7186_warm_start_save(warmStartFile);
JoeMiller	0:02c0c2cbc3df	1252
JoeMiller	0:02c0c2cbc3df	1253	if (displayText) printf("\n\r\n\r-------------------- CPU Reset -------------------------------------\n\r");
JoeMiller	0:02c0c2cbc3df	1254	// Reset the cpu
JoeMiller	0:02c0c2cbc3df	1255	resetCpu();
JoeMiller	0:02c0c2cbc3df	1256
JoeMiller	0:02c0c2cbc3df	1257	if (displayText) printf("\n\r\n\r---------------- Parameters after reset ----------------------------\n\r");
JoeMiller	0:02c0c2cbc3df	1258	// Read the warmstart params after reset
JoeMiller	0:02c0c2cbc3df	1259	displayParams(2, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1260
JoeMiller	0:02c0c2cbc3df	1261	if (displayText) printf("\n\r\n\r---------------- Parameters after upload----------------------------\n\r");
JoeMiller	0:02c0c2cbc3df	1262	// Load warmstart parameters
JoeMiller	0:02c0c2cbc3df	1263	em7186_warm_start_load(warmStartFile);
JoeMiller	0:02c0c2cbc3df	1264
JoeMiller	0:02c0c2cbc3df	1265	// Read the warmstart params after warmstart
JoeMiller	0:02c0c2cbc3df	1266	displayParams(2, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
JoeMiller	0:02c0c2cbc3df	1267	}*/
JoeMiller	0:02c0c2cbc3df	1268
JoeMiller	0:02c0c2cbc3df	1269	char* strBits(void const * const ptr, u8 numBytes, char* str)
JoeMiller	0:02c0c2cbc3df	1270	{
JoeMiller	0:02c0c2cbc3df	1271	u8 bytes = (u8)ptr;
JoeMiller	0:02c0c2cbc3df	1272	u8 i, j;
JoeMiller	0:02c0c2cbc3df	1273	for (i = 0; i < numBytes; i++)
JoeMiller	0:02c0c2cbc3df	1274	{
JoeMiller	0:02c0c2cbc3df	1275	for (j = 0; j < 8; j++)
JoeMiller	0:02c0c2cbc3df	1276	{
JoeMiller	0:02c0c2cbc3df	1277	str[i * 8 + (7 - j)] = bytes[(numBytes - 1) - i] & (1 << j) ? '1' : '0';
JoeMiller	0:02c0c2cbc3df	1278	}
JoeMiller	0:02c0c2cbc3df	1279	}
JoeMiller	0:02c0c2cbc3df	1280	str[numBytes * 8] = '\0';
JoeMiller	0:02c0c2cbc3df	1281	return str;
JoeMiller	0:02c0c2cbc3df	1282	}
JoeMiller	0:02c0c2cbc3df	1283
JoeMiller	0:02c0c2cbc3df	1284	void displayStatusRegisters()
JoeMiller	0:02c0c2cbc3df	1285	{
JoeMiller	0:02c0c2cbc3df	1286	u8 buf[4];
JoeMiller	0:02c0c2cbc3df	1287	char str[17];
JoeMiller	0:02c0c2cbc3df	1288	printf("\n------------ Displaying Status Registers -----------\n\r");
JoeMiller	0:02c0c2cbc3df	1289	em7186_i2c_read(HOST_STATUS_REG, buf, 3);
JoeMiller	0:02c0c2cbc3df	1290	printf("Host Status: % 5u, %s\n\r", buf[0], strBits(&buf[0], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1291	printf("Interrupt Status: % 5u, %s\n\r", buf[1], strBits(&buf[1], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1292	printf("Chip Status: % 5u, %s\n\r", buf[2], strBits(&buf[2], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1293	em7186_i2c_read(ERR_REG, buf, 4);
JoeMiller	0:02c0c2cbc3df	1294	printf("Error Register: % 5u, %s\n\r", buf[0], strBits(&buf[0], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1295	printf("Interrupt State: % 5u, %s\n\r", buf[1], strBits(&buf[1], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1296	printf("Debug Value: % 5u, %s\n\r", buf[2], strBits(&buf[2], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1297	printf("Debug State: % 5u, %s\n\r", buf[3], strBits(&buf[3], sizeof(buf[0]), str));
JoeMiller	0:02c0c2cbc3df	1298	em7186_i2c_read(BYTES_REMANING_REG, buf, 2);
JoeMiller	0:02c0c2cbc3df	1299	u16* v = (u16*)&buf;
JoeMiller	0:02c0c2cbc3df	1300	printf("Bytes Remaining: % 5u, %s\n\n\r", v[0], strBits(&v[0], sizeof(v[0]), str));
JoeMiller	0:02c0c2cbc3df	1301	}
JoeMiller	0:02c0c2cbc3df	1302
JoeMiller	0:02c0c2cbc3df	1303	void displaySensorStatusBits(u8 id, const SensorStatus *status)
JoeMiller	0:02c0c2cbc3df	1304	{
JoeMiller	0:02c0c2cbc3df	1305	printf("\|% 4u \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1306	id, status->dataAvailable, status->i2cNack, status->deviceIdError,
JoeMiller	0:02c0c2cbc3df	1307	status->transientError, status->dataLost, status->powerMode);
JoeMiller	0:02c0c2cbc3df	1308	}
JoeMiller	0:02c0c2cbc3df	1309
JoeMiller	0:02c0c2cbc3df	1310	void displaySensorStatus()
JoeMiller	0:02c0c2cbc3df	1311	{
JoeMiller	0:02c0c2cbc3df	1312	SensorStatus sensorStatus[32];
JoeMiller	0:02c0c2cbc3df	1313	ParamInfo param[] = { PARAM_SENSOR_STATUS_BANK_0, 32 };
JoeMiller	0:02c0c2cbc3df	1314	em7186_param_read((u8*)sensorStatus, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1315	printf("+------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1316	printf("\| SENSOR STATUS \|\n\r");
JoeMiller	0:02c0c2cbc3df	1317	printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1318	printf("\| ID \| Data \| I2C \| DEVICE \| Transient \| Data \| Power \|\n\r");
JoeMiller	0:02c0c2cbc3df	1319	printf("\| \| Available \| NACK \| ID ERR \| Error \| Lost \| Mode \|\n\r");
JoeMiller	0:02c0c2cbc3df	1320	printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1321	u8 i;
JoeMiller	0:02c0c2cbc3df	1322	for (i = 0; i < 32; i++)
JoeMiller	0:02c0c2cbc3df	1323	{
JoeMiller	0:02c0c2cbc3df	1324	displaySensorStatusBits(i + 1, &sensorStatus[i]);
JoeMiller	0:02c0c2cbc3df	1325	}
JoeMiller	0:02c0c2cbc3df	1326	param[0].paramNo = PARAM_SENSOR_STATUS_BANK_0 + 4;
JoeMiller	0:02c0c2cbc3df	1327	em7186_param_read((u8*)sensorStatus, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1328	for (i = 0; i < 16; i++)
JoeMiller	0:02c0c2cbc3df	1329	{
JoeMiller	0:02c0c2cbc3df	1330	displaySensorStatusBits(i + 65, &sensorStatus[i]);
JoeMiller	0:02c0c2cbc3df	1331	}
JoeMiller	0:02c0c2cbc3df	1332	printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1333
JoeMiller	0:02c0c2cbc3df	1334	}
JoeMiller	0:02c0c2cbc3df	1335
JoeMiller	0:02c0c2cbc3df	1336	void getPhysicalSensorStatus()
JoeMiller	0:02c0c2cbc3df	1337	{
JoeMiller	0:02c0c2cbc3df	1338	ParamInfo param[] = { PARAM_PHYSICAL_SENSOR_STATUS, 15 };
JoeMiller	0:02c0c2cbc3df	1339	em7186_param_read((u8*)&physicalSensorStatus, PARAM_PAGE_SYSTEM, param, 1);
JoeMiller	0:02c0c2cbc3df	1340	}
JoeMiller	0:02c0c2cbc3df	1341
JoeMiller	0:02c0c2cbc3df	1342	void displayPhysicalSensorStatus()
JoeMiller	0:02c0c2cbc3df	1343	{
JoeMiller	0:02c0c2cbc3df	1344	getPhysicalSensorStatus();
JoeMiller	0:02c0c2cbc3df	1345	printf("+----------------------------------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1346	printf("\| PHYSICAL SENSOR STATUS \|\n\r");
JoeMiller	0:02c0c2cbc3df	1347	printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1348	printf("\| Sensor \| Sample \| Dynamic \| ID \| Data \| I2C \| DEVICE \| Transient \| Data \| Power \|\n\r");
JoeMiller	0:02c0c2cbc3df	1349	printf("\| \| Rate \| Range \| \| Available \| NACK \| ID ERR \| Error \| Lost \| Mode \|\n\r");
JoeMiller	0:02c0c2cbc3df	1350	printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1351
JoeMiller	0:02c0c2cbc3df	1352	printf("\| Accel \| % 4u \| % 4u ", physicalSensorStatus.accel.sampleRate, physicalSensorStatus.accel.dynamicRange);
JoeMiller	0:02c0c2cbc3df	1353	displaySensorStatusBits(1, &physicalSensorStatus.accel.status);
JoeMiller	0:02c0c2cbc3df	1354
JoeMiller	0:02c0c2cbc3df	1355	printf("\| Gyro \| % 4u \| % 4u ", physicalSensorStatus.gyro.sampleRate, physicalSensorStatus.gyro.dynamicRange);
JoeMiller	0:02c0c2cbc3df	1356	displaySensorStatusBits(2, &physicalSensorStatus.gyro.status);
JoeMiller	0:02c0c2cbc3df	1357
JoeMiller	0:02c0c2cbc3df	1358	printf("\| Mag \| % 4u \| % 4u ", physicalSensorStatus.mag.sampleRate, physicalSensorStatus.mag.dynamicRange);
JoeMiller	0:02c0c2cbc3df	1359	displaySensorStatusBits(3, &physicalSensorStatus.mag.status);
JoeMiller	0:02c0c2cbc3df	1360	printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
JoeMiller	0:02c0c2cbc3df	1361	}
JoeMiller	0:02c0c2cbc3df	1362	void displayPhysicalSensorInformation()
JoeMiller	0:02c0c2cbc3df	1363	{
JoeMiller	0:02c0c2cbc3df	1364	typedef struct
JoeMiller	0:02c0c2cbc3df	1365	{
JoeMiller	0:02c0c2cbc3df	1366	u8 sensorType;
JoeMiller	0:02c0c2cbc3df	1367	u8 driverId;
JoeMiller	0:02c0c2cbc3df	1368	u8 driverVersion;
JoeMiller	0:02c0c2cbc3df	1369	u8 current;
JoeMiller	0:02c0c2cbc3df	1370	u16 currentDynamicRange;
JoeMiller	0:02c0c2cbc3df	1371	u8 flags;
JoeMiller	0:02c0c2cbc3df	1372	u8 reserved;
JoeMiller	0:02c0c2cbc3df	1373	u16 currentRate;
JoeMiller	0:02c0c2cbc3df	1374	u8 numAxes;
JoeMiller	0:02c0c2cbc3df	1375	u8 orientationMatrix[5];
JoeMiller	0:02c0c2cbc3df	1376	} PhysicalSensorInformation;
JoeMiller	0:02c0c2cbc3df	1377
JoeMiller	0:02c0c2cbc3df	1378	PhysicalSensorInformation s;
JoeMiller	0:02c0c2cbc3df	1379
JoeMiller	0:02c0c2cbc3df	1380	u64 physicalSensorPresent = 0;
JoeMiller	0:02c0c2cbc3df	1381	ParamInfo param = { 32, 4 };
JoeMiller	0:02c0c2cbc3df	1382	em7186_param_read((u8*)&physicalSensorPresent, PARAM_PAGE_SYSTEM, &param, 1);
JoeMiller	0:02c0c2cbc3df	1383
JoeMiller	0:02c0c2cbc3df	1384	u32 i;
JoeMiller	0:02c0c2cbc3df	1385	param.size = 16;
JoeMiller	0:02c0c2cbc3df	1386
JoeMiller	0:02c0c2cbc3df	1387	printf("\n+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1388	printf("\| Sensor \| Driver \| Driver \| Power \| Current \| Current \| Num \|\n\r");
JoeMiller	0:02c0c2cbc3df	1389	printf("\| \| ID \| Version \| \| Range \| Rate \| Axes \|\n\r");
JoeMiller	0:02c0c2cbc3df	1390	printf("+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1391	for (i = 0; i < 64; i++)
JoeMiller	0:02c0c2cbc3df	1392	{
JoeMiller	0:02c0c2cbc3df	1393	if (physicalSensorPresent & (1LL << i))
JoeMiller	0:02c0c2cbc3df	1394	{
JoeMiller	0:02c0c2cbc3df	1395	param.paramNo = i+32;
JoeMiller	0:02c0c2cbc3df	1396	em7186_param_read((u8*)&s, PARAM_PAGE_SYSTEM, &param, 1);
JoeMiller	0:02c0c2cbc3df	1397	printf("\| %-32s \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \| % 4u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1398	em7186_sensor_name[s.sensorType], s.driverId, s.driverVersion, s.current, s.currentDynamicRange, s.currentRate, s.numAxes);
JoeMiller	0:02c0c2cbc3df	1399	}
JoeMiller	0:02c0c2cbc3df	1400	}
JoeMiller	0:02c0c2cbc3df	1401	printf("+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1402	}
JoeMiller	0:02c0c2cbc3df	1403
JoeMiller	0:02c0c2cbc3df	1404	void getSensorInformation()
JoeMiller	0:02c0c2cbc3df	1405	{
JoeMiller	0:02c0c2cbc3df	1406	em7186_param_read((u8*)sensorInformation, PARAM_PAGE_SENSOR_INFO, sensorInfoParamList, sizeof(sensorInfoParamList) / sizeof(sensorInfoParamList[0]));
JoeMiller	0:02c0c2cbc3df	1407	haveSensorInfo = 1;
JoeMiller	0:02c0c2cbc3df	1408
JoeMiller	0:02c0c2cbc3df	1409	magMaxRate = sensorInformation[SENSOR_TYPE_MAGNETIC_FIELD].maxRate;
JoeMiller	0:02c0c2cbc3df	1410	accelMaxRate = sensorInformation[SENSOR_TYPE_ACCELEROMETER].maxRate;
JoeMiller	0:02c0c2cbc3df	1411	gyroMaxRate = sensorInformation[SENSOR_TYPE_GYROSCOPE].maxRate;
JoeMiller	0:02c0c2cbc3df	1412	}
JoeMiller	0:02c0c2cbc3df	1413
JoeMiller	0:02c0c2cbc3df	1414	void getSensorConfiguration()
JoeMiller	0:02c0c2cbc3df	1415	{
JoeMiller	0:02c0c2cbc3df	1416	u8 i;
JoeMiller	0:02c0c2cbc3df	1417	ParamInfo param[1] = { 0, 8 };
JoeMiller	0:02c0c2cbc3df	1418	for (i = 1; i<sizeof(sensorInformation) / sizeof(sensorInformation[0]); i++)
JoeMiller	0:02c0c2cbc3df	1419	{
JoeMiller	0:02c0c2cbc3df	1420	if (sensorInformation[i].sensorId > 0)
JoeMiller	0:02c0c2cbc3df	1421	{
JoeMiller	0:02c0c2cbc3df	1422	param[0].paramNo = sensorInformation[i].sensorId;
JoeMiller	0:02c0c2cbc3df	1423	em7186_param_read((u8*)&sensorConfiguration[i], PARAM_PAGE_SENSOR_CONF, param, 1);
JoeMiller	0:02c0c2cbc3df	1424	}
JoeMiller	0:02c0c2cbc3df	1425	}
JoeMiller	0:02c0c2cbc3df	1426	}
JoeMiller	0:02c0c2cbc3df	1427
JoeMiller	0:02c0c2cbc3df	1428	void displaySensorConfiguration()
JoeMiller	0:02c0c2cbc3df	1429	{
JoeMiller	0:02c0c2cbc3df	1430	if (!haveSensorInfo) { getSensorInformation(); };
JoeMiller	0:02c0c2cbc3df	1431	getSensorConfiguration();
JoeMiller	0:02c0c2cbc3df	1432	printf("+-------------------------------------------------------------------------+\n\r");
JoeMiller	0:02c0c2cbc3df	1433	printf("\| Sensor Configuration \|\n\r");
JoeMiller	0:02c0c2cbc3df	1434	printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
JoeMiller	0:02c0c2cbc3df	1435	printf("\| Sensor \| Rate \| Delay \| Sensitivity \| Range \|\n\r");
JoeMiller	0:02c0c2cbc3df	1436	printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
JoeMiller	0:02c0c2cbc3df	1437	u8 i;
JoeMiller	0:02c0c2cbc3df	1438	for (i = 0; i < sizeof(sensorInformation) / sizeof(sensorInformation[0]); i++)
JoeMiller	0:02c0c2cbc3df	1439	{
JoeMiller	0:02c0c2cbc3df	1440	if (sensorInformation[i].sensorId > 0)
JoeMiller	0:02c0c2cbc3df	1441	{
JoeMiller	0:02c0c2cbc3df	1442	printf("\| %-32s \| % 5u \| % 5u \| % 11u \| % 6u \|\n\r",
JoeMiller	0:02c0c2cbc3df	1443	em7186_sensor_name[sensorInformation[i].sensorId],
JoeMiller	0:02c0c2cbc3df	1444	sensorConfiguration[i].sampleRate,
JoeMiller	0:02c0c2cbc3df	1445	sensorConfiguration[i].maxReportLatency,
JoeMiller	0:02c0c2cbc3df	1446	sensorConfiguration[i].changeSensitivity,
JoeMiller	0:02c0c2cbc3df	1447	sensorConfiguration[i].dynamicRange);
JoeMiller	0:02c0c2cbc3df	1448	}
JoeMiller	0:02c0c2cbc3df	1449	}
JoeMiller	0:02c0c2cbc3df	1450	printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
JoeMiller	0:02c0c2cbc3df	1451	}
JoeMiller	0:02c0c2cbc3df	1452
JoeMiller	0:02c0c2cbc3df	1453	void displaySensorInformation()
JoeMiller	0:02c0c2cbc3df	1454	{
JoeMiller	0:02c0c2cbc3df	1455	if (!haveSensorInfo) { getSensorInformation(); };
JoeMiller	3:69239f60d620	1456	printf("+------------------------------------------------------------------------------------------+\n\r");
JoeMiller	3:69239f60d620	1457	printf("\| Sensor Information \|\n\r");
JoeMiller	3:69239f60d620	1458	printf("+----------------------------------------+---------+-------+-------+-----+----------+------+\n\r");
JoeMiller	3:69239f60d620	1459	printf("\| ID \|Sensor \| Driver \| Power \| Range \| Res \| Rate \| Size \|\n\r");
JoeMiller	3:69239f60d620	1460	printf("+-----\|----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1461	u8 i;
JoeMiller	0:02c0c2cbc3df	1462	for (i = 0; i < sizeof(sensorInfoParamList) / sizeof(sensorInfoParamList[0]); i++)
JoeMiller	0:02c0c2cbc3df	1463	{
JoeMiller	0:02c0c2cbc3df	1464	if (sensorInformation[i].sensorId > 0)
JoeMiller	0:02c0c2cbc3df	1465	{
JoeMiller	3:69239f60d620	1466	printf("\|%3u \| %-32s \| % 3u.%-3u \| % 4u \| % 5u \| % 3u \| %-3u-% 4u \| % 3u \|\n\r",
JoeMiller	3:69239f60d620	1467	i,
JoeMiller	0:02c0c2cbc3df	1468	em7186_sensor_name[sensorInformation[i].sensorId], sensorInformation[i].driverId,
JoeMiller	0:02c0c2cbc3df	1469	sensorInformation[i].driverVersion, sensorInformation[i].power,
JoeMiller	0:02c0c2cbc3df	1470	sensorInformation[i].maxRange, sensorInformation[i].resolution,
JoeMiller	0:02c0c2cbc3df	1471	sensorInformation[i].minRate, sensorInformation[i].maxRate,
JoeMiller	0:02c0c2cbc3df	1472	sensorInformation[i].eventSize);
JoeMiller	0:02c0c2cbc3df	1473	}
JoeMiller	0:02c0c2cbc3df	1474	}
JoeMiller	3:69239f60d620	1475	printf("+-----\|----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
JoeMiller	0:02c0c2cbc3df	1476	}
JoeMiller	0:02c0c2cbc3df	1477
JoeMiller	0:02c0c2cbc3df	1478
JoeMiller	0:02c0c2cbc3df	1479	void read_BuildVersion(void) {
JoeMiller	0:02c0c2cbc3df	1480
JoeMiller	0:02c0c2cbc3df	1481	ParamInfo param; // elements: {u8 paramNo; u8 size;}
JoeMiller	0:02c0c2cbc3df	1482
JoeMiller	0:02c0c2cbc3df	1483	printf("\n\r\n\rSENtral Firmware build version information from PramIO Page 9\n\r");
JoeMiller	0:02c0c2cbc3df	1484
JoeMiller	0:02c0c2cbc3df	1485	struct pni_version {
JoeMiller	0:02c0c2cbc3df	1486	u8 major;
JoeMiller	0:02c0c2cbc3df	1487	u8 minor;
JoeMiller	0:02c0c2cbc3df	1488	u8 patch;
JoeMiller	0:02c0c2cbc3df	1489	u8 other;
JoeMiller	0:02c0c2cbc3df	1490	u32 build;
JoeMiller	0:02c0c2cbc3df	1491	};
JoeMiller	0:02c0c2cbc3df	1492
JoeMiller	0:02c0c2cbc3df	1493	struct pni_version pni_ver;
JoeMiller	0:02c0c2cbc3df	1494
JoeMiller	0:02c0c2cbc3df	1495
JoeMiller	0:02c0c2cbc3df	1496	param.paramNo = 1;
JoeMiller	0:02c0c2cbc3df	1497	param.size = 8;
JoeMiller	0:02c0c2cbc3df	1498	em7186_param_read((u8 *)&pni_ver, 9, &param, 1);
JoeMiller	0:02c0c2cbc3df	1499
JoeMiller	0:02c0c2cbc3df	1500	printf(" major = %u\n\r", pni_ver.major);
JoeMiller	0:02c0c2cbc3df	1501	printf(" minor = %u\n\r", pni_ver.minor);
JoeMiller	0:02c0c2cbc3df	1502	printf(" patch = %u\n\r", pni_ver.patch);
JoeMiller	0:02c0c2cbc3df	1503	printf(" other = %u\n\r", pni_ver.other);
JoeMiller	0:02c0c2cbc3df	1504	printf(" build = %u\n\r", pni_ver.build);
JoeMiller	0:02c0c2cbc3df	1505
JoeMiller	0:02c0c2cbc3df	1506
JoeMiller	0:02c0c2cbc3df	1507	}

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@9:fd5bf0a4c774, 2019-08-07 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Components

Important Information for this Arm website

Access Warning