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