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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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
