/*!
This is the main routine of measurement.
@param[in,out] prms A pointer to a #AK8975PRMS structure.
*/
void MeasureSNGLoop(AK8975PRMS* prms)
{
BYTE i2cData[AKSC_BDATA_SIZE];
int16 i;
int16 bData[AKSC_BDATA_SIZE]; // Measuring block data
int16 ret;
int32 ch;
int32 doze;
int32_t delay;
AKMD_INTERVAL interval;
struct timespec tsstart, tsend;
if (openKey() < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
if (openFormation() < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Get initial interval
GetValidInterval(CSPEC_INTERVAL_SNG, &interval);
// Initialize
if(InitAK8975_Measure(prms) != AKD_SUCCESS){
return;
}
while(TRUE){
// Get start time
if (clock_gettime(CLOCK_REALTIME, &tsstart) < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Set to SNG measurement pattern (Set CNTL register)
if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// .! : 获取 M snesor 的原始数据. 这里可能阻塞.
// Get measurement data from AK8975
// ST1 + (HXL + HXH) + (HYL + HYH) + (HZL + HZH) + ST2
// = 1 + (1 + 1) + (1 + 1) + (1 + 1) + 1 = 8 bytes
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Copy to local variable
// DBGPRINT(DBG_LEVEL3, "%s: bData(Hex)=", __FUNCTION__);
for(i=0; i<AKSC_BDATA_SIZE; i++){
bData[i] = i2cData[i];
// DBGPRINT(DBG_LEVEL3, "%02x,", bData[i]);
}
// DBGPRINT(DBG_LEVEL3, "\n");
D_WHEN_REPEAT(100,
"raw mag x : %d, raw mag y : %d, raw mag z : %d.",
(signed short)(bData[1] + (bData[2] << 8) ),
(signed short)(bData[3] + (bData[4] << 8) ),
(signed short)(bData[5] + (bData[6] << 8) ) );
// .! :
// Get acceelration sensor's measurement data.
if (GetAccVec(prms) != AKRET_PROC_SUCCEED) {
return;
}
/*
DBGPRINT(DBG_LEVEL3,
"%s: acc(Hex)=%02x,%02x,%02x\n", __FUNCTION__,
prms->m_avec.u.x, prms->m_avec.u.y, prms->m_avec.u.z);
*/
ret = MeasuringEventProcess(
bData,
prms,
getFormation(),
interval.decimator,
CSPEC_CNTSUSPEND_SNG
);
// Check the return value
if(ret == AKRET_PROC_SUCCEED){
if(prms->m_cntSuspend > 0){
// Show message
DBGPRINT(DBG_LEVEL2,
"Suspend cycle count = %d\n", prms->m_cntSuspend);
}
else if (prms->m_callcnt <= 1){
// Check interval
if (AKD_GetDelay(&delay) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
} else {
GetValidInterval(delay, &interval);
}
}
// Display(or dispatch) the result.
Disp_MeasurementResultHook(prms);
}
else if(ret == AKRET_FORMATION_CHANGED){
// Switch formation.
SwitchFormation(prms);
}
else if(ret == AKRET_DATA_READERROR){
DBGPRINT(DBG_LEVEL2,
"Data read error occurred.\n\n");
}
else if(ret == AKRET_DATA_OVERFLOW){
DBGPRINT(DBG_LEVEL2,
"Data overflow occurred.\n\n");
}
else if(ret == AKRET_HFLUC_OCCURRED){
DBGPRINT(DBG_LEVEL2,
"AKSC_HFlucCheck did not return 1.\n\n");
}
else{
// Does not reach here
LOGE("MeasuringEventProcess has failed.\n");
break;
}
// Check user order
ch = checkKey();
if (ch == AKKEY_STOP_MEASURE) {
break;
}
else if(ch < 0){
LOGD("Bad key code.\n");
break;
}
// Get end time
if (clock_gettime(CLOCK_REALTIME, &tsend) < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// calculate wait time
doze = interval.interval - ((tsend.tv_sec - tsstart.tv_sec)*1000000 +
(tsend.tv_nsec - tsstart.tv_nsec)/1000);
if (doze < 0) {
doze = 0;
}
// Adjust sampling frequency
// DBGPRINT(DBG_LEVEL3, "Sleep %d usec.\n", doze);
usleep(doze);
}
// Set to PowerDown mode
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
}
closeFormation();
closeKey();
}
/*!
Carry out self-test.
@return If this function succeeds, the return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_FAIL.
*/
int16 AKFS_SelfTest(void)
{
BYTE i2cData[SENSOR_DATA_SIZE];
BYTE asa[3];
AKFLOAT hdata[3];
int16 ret;
/* Set to FUSE ROM access mode */
if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Read values from ASAX to ASAZ */
if (AKD_RxData(AK8975_FUSE_ASAX, asa, 3) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to PowerDown mode */
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to self-test mode */
i2cData[0] = 0x40;
if (AKD_TxData(AK8975_REG_ASTC, i2cData, 1) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to Self-test mode */
if (AKD_SetMode(AK8975_MODE_SELF_TEST) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/*
Wait for DRDY pin changes to HIGH.
Get measurement data from AK8975
*/
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
hdata[0] = AK8975_HDATA_CONVERTER(i2cData[2], i2cData[1], asa[0]);
hdata[1] = AK8975_HDATA_CONVERTER(i2cData[4], i2cData[3], asa[1]);
hdata[2] = AK8975_HDATA_CONVERTER(i2cData[6], i2cData[5], asa[2]);
/* Test */
ret = 1;
if ((hdata[0] < AK8975_SELFTEST_MIN_X) ||
(AK8975_SELFTEST_MAX_X < hdata[0])) {
ret = 0;
}
if ((hdata[1] < AK8975_SELFTEST_MIN_Y) ||
(AK8975_SELFTEST_MAX_Y < hdata[1])) {
ret = 0;
}
if ((hdata[2] < AK8975_SELFTEST_MIN_Z) ||
(AK8975_SELFTEST_MAX_Z < hdata[2])) {
ret = 0;
}
AKMDEBUG(DBG_LEVEL2, "Test(%s):%8.2f, %8.2f, %8.2f\n",
(ret ? "Success" : "fail"), hdata[0], hdata[1], hdata[2]);
if (ret) {
return AKM_SUCCESS;
} else {
return AKM_FAIL;
}
}
/*!
Execute "Onboard Function Test" (NOT includes "START" and "END" command).
@retval 1 The test is passed successfully.
@retval -1 The test is failed.
@retval 0 The test is aborted by kind of system error.
@param[in] prms A pointer to a #AK8975PRMS structure.
*/
int16 FctShipmntTestProcess_Body(AK8975PRMS* prms)
{
int16 pf_total; //p/f flag for this subtest
BYTE i2cData[16];
int16 hdata[3];
int16 asax;
int16 asay;
int16 asaz;
//***********************************************
// Reset Test Result
//***********************************************
pf_total = 1;
//***********************************************
// Step1
//***********************************************
// Set to PowerDown mode
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// When the serial interface is SPI,
// write "00011011" to I2CDIS register(to disable I2C,).
if(CSPEC_SPI_USE == 1){
i2cData[0] = 0x1B;
if (AKD_TxData(AK8975_REG_I2CDIS, i2cData, 1) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
}
// Read values from WIA to ASTC.
if (AKD_RxData(AK8975_REG_WIA, i2cData, 13) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// TEST
TEST_DATA(TLIMIT_NO_RST_WIA, TLIMIT_TN_RST_WIA, (int16)i2cData[0], TLIMIT_LO_RST_WIA, TLIMIT_HI_RST_WIA, &pf_total);
TEST_DATA(TLIMIT_NO_RST_INFO, TLIMIT_TN_RST_INFO, (int16)i2cData[1], TLIMIT_LO_RST_INFO, TLIMIT_HI_RST_INFO, &pf_total);
TEST_DATA(TLIMIT_NO_RST_ST1, TLIMIT_TN_RST_ST1, (int16)i2cData[2], TLIMIT_LO_RST_ST1, TLIMIT_HI_RST_ST1, &pf_total);
TEST_DATA(TLIMIT_NO_RST_HXL, TLIMIT_TN_RST_HXL, (int16)i2cData[3], TLIMIT_LO_RST_HXL, TLIMIT_HI_RST_HXL, &pf_total);
TEST_DATA(TLIMIT_NO_RST_HXH, TLIMIT_TN_RST_HXH, (int16)i2cData[4], TLIMIT_LO_RST_HXH, TLIMIT_HI_RST_HXH, &pf_total);
TEST_DATA(TLIMIT_NO_RST_HYL, TLIMIT_TN_RST_HYL, (int16)i2cData[5], TLIMIT_LO_RST_HYL, TLIMIT_HI_RST_HYL, &pf_total);
TEST_DATA(TLIMIT_NO_RST_HYH, TLIMIT_TN_RST_HYH, (int16)i2cData[6], TLIMIT_LO_RST_HYH, TLIMIT_HI_RST_HYH, &pf_total);
TEST_DATA(TLIMIT_NO_RST_HZL, TLIMIT_TN_RST_HZL, (int16)i2cData[7], TLIMIT_LO_RST_HZL, TLIMIT_HI_RST_HZL, &pf_total);
TEST_DATA(TLIMIT_NO_RST_HZH, TLIMIT_TN_RST_HZH, (int16)i2cData[8], TLIMIT_LO_RST_HZH, TLIMIT_HI_RST_HZH, &pf_total);
TEST_DATA(TLIMIT_NO_RST_ST2, TLIMIT_TN_RST_ST2, (int16)i2cData[9], TLIMIT_LO_RST_ST2, TLIMIT_HI_RST_ST2, &pf_total);
TEST_DATA(TLIMIT_NO_RST_CNTL, TLIMIT_TN_RST_CNTL, (int16)i2cData[10], TLIMIT_LO_RST_CNTL, TLIMIT_HI_RST_CNTL, &pf_total);
// i2cData[11] is BLANK.
TEST_DATA(TLIMIT_NO_RST_ASTC, TLIMIT_TN_RST_ASTC, (int16)i2cData[12], TLIMIT_LO_RST_ASTC, TLIMIT_HI_RST_ASTC, &pf_total);
// Read values from I2CDIS.
if (AKD_RxData(AK8975_REG_I2CDIS, i2cData, 1) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
if(CSPEC_SPI_USE == 1){
TEST_DATA(TLIMIT_NO_RST_I2CDIS, TLIMIT_TN_RST_I2CDIS, (int16)i2cData[0], TLIMIT_LO_RST_I2CDIS_USESPI, TLIMIT_HI_RST_I2CDIS_USESPI, &pf_total);
}else{
TEST_DATA(TLIMIT_NO_RST_I2CDIS, TLIMIT_TN_RST_I2CDIS, (int16)i2cData[0], TLIMIT_LO_RST_I2CDIS_USEI2C, TLIMIT_HI_RST_I2CDIS_USEI2C, &pf_total);
}
// Set to FUSE ROM access mode
if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// Read values from ASAX to ASAZ
if (AKD_RxData(AK8975_FUSE_ASAX, i2cData, 3) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
asax = (int16)i2cData[0];
asay = (int16)i2cData[1];
asaz = (int16)i2cData[2];
#ifdef NOASA
if((asax==0)||(asay==0)||(asaz==0)){
asax = 128;
asay = 128;
asaz = 128;
}
#endif
// TEST
TEST_DATA(TLIMIT_NO_ASAX, TLIMIT_TN_ASAX, asax, TLIMIT_LO_ASAX, TLIMIT_HI_ASAX, &pf_total);
TEST_DATA(TLIMIT_NO_ASAY, TLIMIT_TN_ASAY, asay, TLIMIT_LO_ASAY, TLIMIT_HI_ASAY, &pf_total);
TEST_DATA(TLIMIT_NO_ASAZ, TLIMIT_TN_ASAZ, asaz, TLIMIT_LO_ASAZ, TLIMIT_HI_ASAZ, &pf_total);
// Read values. CNTL
if (AKD_RxData(AK8975_REG_CNTL, i2cData, 1) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// Set to PowerDown mode
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// TEST
TEST_DATA(TLIMIT_NO_WR_CNTL, TLIMIT_TN_WR_CNTL, (int16)i2cData[0], TLIMIT_LO_WR_CNTL, TLIMIT_HI_WR_CNTL, &pf_total);
//***********************************************
// Step2
//***********************************************
// Set to SNG measurement pattern (Set CNTL register)
if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// Wait for DRDY pin changes to HIGH.
// Get measurement data from AK8975
// ST1 + (HXL + HXH) + (HYL + HYH) + (HZL + HZH) + ST2
// = 1 + (1 + 1) + (1 + 1) + (1 + 1) + 1 = 8 bytes
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
hdata[0] = (int16)((((uint16)(i2cData[2]))<<8)+(uint16)(i2cData[1]));
hdata[1] = (int16)((((uint16)(i2cData[4]))<<8)+(uint16)(i2cData[3]));
hdata[2] = (int16)((((uint16)(i2cData[6]))<<8)+(uint16)(i2cData[5]));
// TEST
TEST_DATA(TLIMIT_NO_SNG_ST1, TLIMIT_TN_SNG_ST1, (int16)i2cData[0], TLIMIT_LO_SNG_ST1, TLIMIT_HI_SNG_ST1, &pf_total);
TEST_DATA(TLIMIT_NO_SNG_HX, TLIMIT_TN_SNG_HX, hdata[0], TLIMIT_LO_SNG_HX, TLIMIT_HI_SNG_HX, &pf_total);
TEST_DATA(TLIMIT_NO_SNG_HY, TLIMIT_TN_SNG_HY, hdata[1], TLIMIT_LO_SNG_HY, TLIMIT_HI_SNG_HY, &pf_total);
TEST_DATA(TLIMIT_NO_SNG_HZ, TLIMIT_TN_SNG_HZ, hdata[2], TLIMIT_LO_SNG_HZ, TLIMIT_HI_SNG_HZ, &pf_total);
TEST_DATA(TLIMIT_NO_SNG_ST2, TLIMIT_TN_SNG_ST2, (int16)i2cData[7], TLIMIT_LO_SNG_ST2, TLIMIT_HI_SNG_ST2, &pf_total);
// Generate magnetic field for self-test (Set ASTC register)
i2cData[0] = 0x40;
if (AKD_TxData(AK8975_REG_ASTC, i2cData, 1) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// Set to Self-test mode (Set CNTL register)
if (AKD_SetMode(AK8975_MODE_SELF_TEST) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// Wait for DRDY pin changes to HIGH.
// Get measurement data from AK8975
// ST1 + (HXL + HXH) + (HYL + HYH) + (HZL + HZH) + ST2
// = 1 + (1 + 1) + (1 + 1) + (1 + 1) + 1 = 8Byte
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
// TEST
TEST_DATA(TLIMIT_NO_SLF_ST1, TLIMIT_TN_SLF_ST1, (int16)i2cData[0], TLIMIT_LO_SLF_ST1, TLIMIT_HI_SLF_ST1, &pf_total);
hdata[0] = (int16)((((uint16)(i2cData[2]))<<8)+(uint16)(i2cData[1]));
hdata[1] = (int16)((((uint16)(i2cData[4]))<<8)+(uint16)(i2cData[3]));
hdata[2] = (int16)((((uint16)(i2cData[6]))<<8)+(uint16)(i2cData[5]));
// TEST
TEST_DATA(
TLIMIT_NO_SLF_RVHX,
TLIMIT_TN_SLF_RVHX,
(hdata[0])*((asax - 128)*0.5f/128.0f + 1),
TLIMIT_LO_SLF_RVHX,
TLIMIT_HI_SLF_RVHX,
&pf_total
);
TEST_DATA(
TLIMIT_NO_SLF_RVHY,
TLIMIT_TN_SLF_RVHY,
(hdata[1])*((asay - 128)*0.5f/128.0f + 1),
TLIMIT_LO_SLF_RVHY,
TLIMIT_HI_SLF_RVHY,
&pf_total
);
TEST_DATA(
TLIMIT_NO_SLF_RVHZ,
TLIMIT_TN_SLF_RVHZ,
(hdata[2])*((asaz - 128)*0.5f/128.0f + 1),
TLIMIT_LO_SLF_RVHZ,
TLIMIT_HI_SLF_RVHZ,
&pf_total
);
// TEST
TEST_DATA(TLIMIT_NO_SLF_ST2, TLIMIT_TN_SLF_ST2, (int16)i2cData[7], TLIMIT_LO_SLF_ST2, TLIMIT_HI_SLF_ST2, &pf_total);
// Set to Normal mode for self-test.
i2cData[0] = 0x00;
if (AKD_TxData(AK8975_REG_ASTC, i2cData, 1) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1, "%s:%d Error.\n", __FUNCTION__, __LINE__);
return 0;
}
return pf_total;
}
/*!
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;
}
}