/*!
 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;
	}
}