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