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 */
AKFS_ReadAK8975FUSEROM(uint8 regs[3])36 int16 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 */
AKFS_SelfTest(void)69 int16 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 */
AKFS_CalcSleep(const struct timespec * end,const struct timespec * start,const int64_t minimum)154 struct 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 */
AKFS_GetInterval(uint16 * flag,int64_t * minimum)195 int16 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 */
AKFS_OutputResult(const uint16 flag,const AKSENSOR_DATA * acc,const AKSENSOR_DATA * mag,const AKSENSOR_DATA * ori)234 void 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 */
AKFS_MeasureLoop(void)269 void 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
398 MEASURE_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