/*!
Acquire acceleration data from acceleration sensor.
@return If this function succeeds, the return value is AKRET_PROC_SUCCEED.
Otherwise the return value is AKRET_PROC_FAIL.
@param[out] prms A pointer to #AK8975PRMS structure.
*/
int16 GetAccVec(AK8975PRMS * prms)
{
int i;
int16 acc[3]; /* 将缓存 acc sensor 返回的数据. */
if (AKD_GetAccelerationData(acc) != AKD_SUCCESS) {
return AKRET_PROC_FAIL;
}
// Convert format
for (i = 0; i < 3; i++) {
prms->m_avec.v[i] = acc[i];
}
return AKRET_PROC_SUCCEED;
}
/*!
This is the main routine of measurement.
*/
void AKFS_MeasureLoop(void)
{
BYTE i2cData[SENSOR_DATA_SIZE]; /* ST1 ~ ST2 */
int16 mag[3];
int16 mstat;
int16 acc[3];
struct timespec tsstart= {0, 0};
struct timespec tsend = {0, 0};
struct timespec doze;
int64_t minimum;
uint16 flag;
AKSENSOR_DATA sv_acc;
AKSENSOR_DATA sv_mag;
AKSENSOR_DATA sv_ori;
AKFLOAT tmpx, tmpy, tmpz;
int16 tmp_accuracy;
minimum = -1;
#ifdef WIN32
clock_init_time();
#endif
/* Initialize library functions and device */
if (AKFS_Start(CSPEC_SETTING_FILE) != AKM_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
while (g_stopRequest != AKM_TRUE) {
/* Beginning time */
if (clock_gettime(CLOCK_MONOTONIC, &tsstart) < 0) {
AKMERROR;
goto MEASURE_END;
}
/* Get interval */
if (AKFS_GetInterval(&flag, &minimum) != AKM_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
if ((flag & ACC_DATA_READY) || (flag & ORI_DATA_READY)) {
/* Get accelerometer */
if (AKD_GetAccelerationData(acc) != AKD_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
/* Calculate accelerometer vector */
if (AKFS_Get_ACCELEROMETER(acc, 0, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
sv_acc.x = tmpx;
sv_acc.y = tmpy;
sv_acc.z = tmpz;
sv_acc.status = tmp_accuracy;
} else {
flag &= ~ACC_DATA_READY;
flag &= ~ORI_DATA_READY;
}
}
if ((flag & MAG_DATA_READY) || (flag & ORI_DATA_READY)) {
/* Set to measurement mode */
if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
/* Wait for DRDY and get data from device */
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
/* raw data to x,y,z value */
mag[0] = (int)((int16_t)(i2cData[2]<<8)+((int16_t)i2cData[1]));
mag[1] = (int)((int16_t)(i2cData[4]<<8)+((int16_t)i2cData[3]));
mag[2] = (int)((int16_t)(i2cData[6]<<8)+((int16_t)i2cData[5]));
mstat = i2cData[0] | i2cData[7];
AKMDATA(AKMDATA_BDATA,
"bData=%02X,%02X,%02X,%02X,%02X,%02X,%02X,%02X\n",
i2cData[0], i2cData[1], i2cData[2], i2cData[3],
i2cData[4], i2cData[5], i2cData[6], i2cData[7]);
/* Calculate magnetic field vector */
if (AKFS_Get_MAGNETIC_FIELD(mag, mstat, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
sv_mag.x = tmpx;
sv_mag.y = tmpy;
sv_mag.z = tmpz;
sv_mag.status = tmp_accuracy;
} else {
flag &= ~MAG_DATA_READY;
flag &= ~ORI_DATA_READY;
}
}
if (flag & ORI_DATA_READY) {
if (AKFS_Get_ORIENTATION(&tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
sv_ori.x = tmpx;
sv_ori.y = tmpy;
sv_ori.z = tmpz;
sv_ori.status = tmp_accuracy;
} else {
flag &= ~ORI_DATA_READY;
}
}
/* Output result */
AKFS_OutputResult(flag, &sv_acc, &sv_mag, &sv_ori);
/* Ending time */
if (clock_gettime(CLOCK_MONOTONIC, &tsend) < 0) {
AKMERROR;
goto MEASURE_END;
}
/* Calculate duration */
doze = AKFS_CalcSleep(&tsend, &tsstart, minimum);
AKMDATA(AKMDATA_LOOP, "Sleep: %6.2f msec\n", (doze.tv_nsec/1000000.0f));
nanosleep(&doze, NULL);
#ifdef WIN32
if (_kbhit()) {
_getch();
break;
}
#endif
}
MEASURE_END:
/* Set to PowerDown mode */
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
AKMERROR;
return;
}
/* Save parameters */
if (AKFS_Stop(CSPEC_SETTING_FILE) != AKM_SUCCESS) {
AKMERROR;
}
}
