1/******************************************************************************
2 * $Id: AKFS_Measure.c 580 2012-03-29 09:56:21Z yamada.rj $
3 ******************************************************************************
4 *
5 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 *      http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 */
19#ifdef WIN32
20#include "AK8975_LinuxDriver.h"
21#else
22#include "AK8975Driver.h"
23#endif
24
25#include "AKFS_Measure.h"
26#include "AKFS_Disp.h"
27#include "AKFS_APIs.h"
28
29/*!
30  Read sensitivity adjustment data from fuse ROM.
31  @return If data are read successfully, the return value is #AKM_SUCCESS.
32   Otherwise the return value is #AKM_FAIL.
33  @param[out] regs The read ASA values. When this function succeeds, ASAX value
34   is saved in regs[0], ASAY is saved in regs[1], ASAZ is saved in regs[2].
35 */
36int16 AKFS_ReadAK8975FUSEROM(
37		uint8	regs[3]
38)
39{
40	/* Set to FUSE ROM access mode */
41	if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
42		AKMERROR;
43		return AKM_FAIL;
44	}
45
46	/* Read values. ASAX, ASAY, ASAZ */
47	if (AKD_RxData(AK8975_FUSE_ASAX, regs, 3) != AKD_SUCCESS) {
48		AKMERROR;
49		return AKM_FAIL;
50	}
51
52	/* Set to PowerDown mode */
53	if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
54		AKMERROR;
55		return AKM_FAIL;
56	}
57
58	AKMDEBUG(DBG_LEVEL2, "%s: asa(dec)=%d,%d,%d\n",
59			__FUNCTION__, regs[0], regs[1], regs[2]);
60
61	return AKM_SUCCESS;
62}
63
64/*!
65  Carry out self-test.
66  @return If this function succeeds, the return value is #AKM_SUCCESS.
67   Otherwise the return value is #AKM_FAIL.
68 */
69int16 AKFS_SelfTest(void)
70{
71	BYTE	i2cData[SENSOR_DATA_SIZE];
72	BYTE	asa[3];
73	AKFLOAT	hdata[3];
74	int16	ret;
75
76	/* Set to FUSE ROM access mode */
77	if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
78		AKMERROR;
79		return AKM_FAIL;
80	}
81
82	/* Read values from ASAX to ASAZ */
83	if (AKD_RxData(AK8975_FUSE_ASAX, asa, 3) != AKD_SUCCESS) {
84		AKMERROR;
85		return AKM_FAIL;
86	}
87
88	/* Set to PowerDown mode */
89	if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
90		AKMERROR;
91		return AKM_FAIL;
92	}
93
94	/* Set to self-test mode */
95	i2cData[0] = 0x40;
96	if (AKD_TxData(AK8975_REG_ASTC, i2cData, 1) != AKD_SUCCESS) {
97		AKMERROR;
98		return AKM_FAIL;
99	}
100
101	/* Set to Self-test mode */
102	if (AKD_SetMode(AK8975_MODE_SELF_TEST) != AKD_SUCCESS) {
103		AKMERROR;
104		return AKM_FAIL;
105	}
106
107	/*
108	   Wait for DRDY pin changes to HIGH.
109	   Get measurement data from AK8975
110	 */
111	if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
112		AKMERROR;
113		return AKM_FAIL;
114	}
115
116	hdata[0] = AK8975_HDATA_CONVERTER(i2cData[2], i2cData[1], asa[0]);
117	hdata[1] = AK8975_HDATA_CONVERTER(i2cData[4], i2cData[3], asa[1]);
118	hdata[2] = AK8975_HDATA_CONVERTER(i2cData[6], i2cData[5], asa[2]);
119
120	/* Test */
121	ret = 1;
122	if ((hdata[0] < AK8975_SELFTEST_MIN_X) ||
123		(AK8975_SELFTEST_MAX_X < hdata[0])) {
124		ret = 0;
125	}
126	if ((hdata[1] < AK8975_SELFTEST_MIN_Y) ||
127		(AK8975_SELFTEST_MAX_Y < hdata[1])) {
128		ret = 0;
129	}
130	if ((hdata[2] < AK8975_SELFTEST_MIN_Z) ||
131		(AK8975_SELFTEST_MAX_Z < hdata[2])) {
132		ret = 0;
133	}
134
135	AKMDEBUG(DBG_LEVEL2, "Test(%s):%8.2f, %8.2f, %8.2f\n",
136		(ret ? "Success" : "fail"), hdata[0], hdata[1], hdata[2]);
137
138	if (ret) {
139		return AKM_SUCCESS;
140	} else {
141		return AKM_FAIL;
142	}
143}
144
145/*!
146  This function calculate the duration of sleep for maintaining
147   the loop keep the period.
148  This function calculates "minimum - (end - start)".
149  @return The result of above equation in nanosecond.
150  @param end The time of after execution.
151  @param start The time of before execution.
152  @param minimum Loop period of each execution.
153 */
154struct timespec AKFS_CalcSleep(
155	const struct timespec* end,
156	const struct timespec* start,
157	const int64_t minimum
158)
159{
160	int64_t endL;
161	int64_t startL;
162	int64_t diff;
163
164	struct timespec ret;
165
166	endL = (end->tv_sec * 1000000000) + end->tv_nsec;
167	startL = (start->tv_sec * 1000000000) + start->tv_nsec;
168	diff = minimum;
169
170	diff -= (endL - startL);
171
172	/* Don't allow negative value */
173	if (diff < 0) {
174		diff = 0;
175	}
176
177	/* Convert to timespec */
178	if (diff > 1000000000) {
179	ret.tv_sec = diff / 1000000000;
180		ret.tv_nsec = diff % 1000000000;
181	} else {
182		ret.tv_sec = 0;
183		ret.tv_nsec = diff;
184	}
185	return ret;
186}
187
188/*!
189  Get interval of each sensors from device driver.
190  @return If this function succeeds, the return value is #AKM_SUCCESS.
191   Otherwise the return value is #AKM_FAIL.
192  @param flag This variable indicates what sensor frequency is updated.
193  @param minimum This value show the minimum loop period in all sensors.
194 */
195int16 AKFS_GetInterval(
196		uint16*  flag,
197		int64_t* minimum
198)
199{
200	/* Accelerometer, Magnetometer, Orientation */
201	/* Delay is in nano second unit. */
202	/* Negative value means the sensor is disabled.*/
203	int64_t delay[AKM_NUM_SENSORS];
204	int i;
205
206	if (AKD_GetDelay(delay) < 0) {
207		AKMERROR;
208		return AKM_FAIL;
209	}
210	AKMDATA(AKMDATA_GETINTERVAL,"delay[A,M,O]=%lld,%lld,%lld\n",
211		delay[0], delay[1], delay[2]);
212
213	/* update */
214	*minimum = 1000000000;
215	*flag = 0;
216	for (i=0; i<AKM_NUM_SENSORS; i++) {
217		/* Set flag */
218		if (delay[i] >= 0) {
219			*flag |= 1 << i;
220			if (*minimum > delay[i]) {
221				*minimum = delay[i];
222			}
223		}
224	}
225	return AKM_SUCCESS;
226}
227
228/*!
229  If this program run as console mode, measurement result will be displayed
230   on console terminal.
231  @return If this function succeeds, the return value is #AKM_SUCCESS.
232   Otherwise the return value is #AKM_FAIL.
233 */
234void AKFS_OutputResult(
235	const	uint16			flag,
236	const	AKSENSOR_DATA*	acc,
237	const	AKSENSOR_DATA*	mag,
238	const	AKSENSOR_DATA*	ori
239)
240{
241	int buf[YPR_DATA_SIZE];
242
243	/* Store to buffer */
244	buf[0] = flag;					/* Data flag */
245	buf[1] = CONVERT_ACC(acc->x);	/* Ax */
246	buf[2] = CONVERT_ACC(acc->y);	/* Ay */
247	buf[3] = CONVERT_ACC(acc->z);	/* Az */
248	buf[4] = acc->status;			/* Acc status */
249	buf[5] = CONVERT_MAG(mag->x);	/* Mx */
250	buf[6] = CONVERT_MAG(mag->y);	/* My */
251	buf[7] = CONVERT_MAG(mag->z);	/* Mz */
252	buf[8] = mag->status;			/* Mag status */
253	buf[9] = CONVERT_ORI(ori->x);	/* yaw */
254	buf[10] = CONVERT_ORI(ori->y);	/* pitch */
255	buf[11] = CONVERT_ORI(ori->z);	/* roll */
256
257	if (g_opmode & OPMODE_CONSOLE) {
258		/* Console mode */
259		Disp_Result(buf);
260	}
261
262	/* Set result to driver */
263		AKD_SetYPR(buf);
264}
265
266/*!
267 This is the main routine of measurement.
268 */
269void AKFS_MeasureLoop(void)
270{
271	BYTE    i2cData[SENSOR_DATA_SIZE]; /* ST1 ~ ST2 */
272	int16	mag[3];
273	int16	mstat;
274	int16	acc[3];
275	struct	timespec tsstart= {0, 0};
276	struct	timespec tsend = {0, 0};
277	struct	timespec doze;
278	int64_t	minimum;
279	uint16	flag;
280	AKSENSOR_DATA sv_acc;
281	AKSENSOR_DATA sv_mag;
282	AKSENSOR_DATA sv_ori;
283	AKFLOAT tmpx, tmpy, tmpz;
284	int16 tmp_accuracy;
285
286	minimum = -1;
287
288#ifdef WIN32
289	clock_init_time();
290#endif
291
292	/* Initialize library functions and device */
293	if (AKFS_Start(CSPEC_SETTING_FILE) != AKM_SUCCESS) {
294		AKMERROR;
295		goto MEASURE_END;
296	}
297
298	while (g_stopRequest != AKM_TRUE) {
299		/* Beginning time */
300		if (clock_gettime(CLOCK_MONOTONIC, &tsstart) < 0) {
301			AKMERROR;
302			goto MEASURE_END;
303		}
304
305		/* Get interval */
306		if (AKFS_GetInterval(&flag, &minimum) != AKM_SUCCESS) {
307			AKMERROR;
308			goto MEASURE_END;
309		}
310
311		if ((flag & ACC_DATA_READY) || (flag & ORI_DATA_READY)) {
312			/* Get accelerometer */
313			if (AKD_GetAccelerationData(acc) != AKD_SUCCESS) {
314				AKMERROR;
315				goto MEASURE_END;
316			}
317
318			/* Calculate accelerometer vector */
319			if (AKFS_Get_ACCELEROMETER(acc, 0, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
320				sv_acc.x = tmpx;
321				sv_acc.y = tmpy;
322				sv_acc.z = tmpz;
323				sv_acc.status = tmp_accuracy;
324			} else {
325				flag &= ~ACC_DATA_READY;
326				flag &= ~ORI_DATA_READY;
327			}
328		}
329
330		if ((flag & MAG_DATA_READY) || (flag & ORI_DATA_READY)) {
331			/* Set to measurement mode  */
332			if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) {
333				AKMERROR;
334				goto MEASURE_END;
335			}
336
337			/* Wait for DRDY and get data from device */
338			if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
339				AKMERROR;
340				goto MEASURE_END;
341			}
342			/* raw data to x,y,z value */
343			mag[0] = (int)((int16_t)(i2cData[2]<<8)+((int16_t)i2cData[1]));
344			mag[1] = (int)((int16_t)(i2cData[4]<<8)+((int16_t)i2cData[3]));
345			mag[2] = (int)((int16_t)(i2cData[6]<<8)+((int16_t)i2cData[5]));
346			mstat = i2cData[0] | i2cData[7];
347
348			AKMDATA(AKMDATA_BDATA,
349				"bData=%02X,%02X,%02X,%02X,%02X,%02X,%02X,%02X\n",
350				i2cData[0], i2cData[1], i2cData[2], i2cData[3],
351				i2cData[4], i2cData[5], i2cData[6], i2cData[7]);
352
353			/* Calculate magnetic field vector */
354			if (AKFS_Get_MAGNETIC_FIELD(mag, mstat, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
355				sv_mag.x = tmpx;
356				sv_mag.y = tmpy;
357				sv_mag.z = tmpz;
358				sv_mag.status = tmp_accuracy;
359			} else {
360				flag &= ~MAG_DATA_READY;
361				flag &= ~ORI_DATA_READY;
362			}
363		}
364
365		if (flag & ORI_DATA_READY) {
366			if (AKFS_Get_ORIENTATION(&tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
367				sv_ori.x = tmpx;
368				sv_ori.y = tmpy;
369				sv_ori.z = tmpz;
370				sv_ori.status = tmp_accuracy;
371			} else {
372				flag &= ~ORI_DATA_READY;
373			}
374		}
375
376		/* Output result */
377		AKFS_OutputResult(flag, &sv_acc, &sv_mag, &sv_ori);
378
379		/* Ending time */
380		if (clock_gettime(CLOCK_MONOTONIC, &tsend) < 0) {
381			AKMERROR;
382			goto MEASURE_END;
383		}
384
385		/* Calculate duration */
386		doze = AKFS_CalcSleep(&tsend, &tsstart, minimum);
387		AKMDATA(AKMDATA_LOOP, "Sleep: %6.2f msec\n", (doze.tv_nsec/1000000.0f));
388		nanosleep(&doze, NULL);
389
390#ifdef WIN32
391		if (_kbhit()) {
392			_getch();
393			break;
394		}
395#endif
396	}
397
398MEASURE_END:
399	/* Set to PowerDown mode */
400	if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
401		AKMERROR;
402		return;
403	}
404
405	/* Save parameters */
406	if (AKFS_Stop(CSPEC_SETTING_FILE) != AKM_SUCCESS) {
407		AKMERROR;
408	}
409}
410
411
412