/******************************************************************************
* $Id: AKFS_Measure.c 580 2012-03-29 09:56:21Z yamada.rj $
******************************************************************************
*
* Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <inttypes.h>
#ifdef WIN32
#include "AK8975_LinuxDriver.h"
#else
#include "AK8975Driver.h"
#endif
#include "AKFS_Measure.h"
#include "AKFS_Disp.h"
#include "AKFS_APIs.h"
/*!
Read sensitivity adjustment data from fuse ROM.
@return If data are read successfully, the return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_FAIL.
@param[out] regs The read ASA values. When this function succeeds, ASAX value
is saved in regs[0], ASAY is saved in regs[1], ASAZ is saved in regs[2].
*/
int16 AKFS_ReadAK8975FUSEROM(
uint8 regs[3]
)
{
/* Set to FUSE ROM access mode */
if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Read values. ASAX, ASAY, ASAZ */
if (AKD_RxData(AK8975_FUSE_ASAX, regs, 3) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to PowerDown mode */
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
AKMDEBUG(DBG_LEVEL2, "%s: asa(dec)=%d,%d,%d\n",
__FUNCTION__, regs[0], regs[1], regs[2]);
return AKM_SUCCESS;
}
/*!
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;
}
}
/*!
This function calculate the duration of sleep for maintaining
the loop keep the period.
This function calculates "minimum - (end - start)".
@return The result of above equation in nanosecond.
@param end The time of after execution.
@param start The time of before execution.
@param minimum Loop period of each execution.
*/
struct timespec AKFS_CalcSleep(
const struct timespec* end,
const struct timespec* start,
const int64_t minimum
)
{
int64_t endL;
int64_t startL;
int64_t diff;
struct timespec ret;
endL = (end->tv_sec * 1000000000) + end->tv_nsec;
startL = (start->tv_sec * 1000000000) + start->tv_nsec;
diff = minimum;
diff -= (endL - startL);
/* Don't allow negative value */
if (diff < 0) {
diff = 0;
}
/* Convert to timespec */
if (diff > 1000000000) {
ret.tv_sec = diff / 1000000000;
ret.tv_nsec = diff % 1000000000;
} else {
ret.tv_sec = 0;
ret.tv_nsec = diff;
}
return ret;
}
/*!
Get interval of each sensors from device driver.
@return If this function succeeds, the return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_FAIL.
@param flag This variable indicates what sensor frequency is updated.
@param minimum This value show the minimum loop period in all sensors.
*/
int16 AKFS_GetInterval(
uint16* flag,
int64_t* minimum
)
{
/* Accelerometer, Magnetometer, Orientation */
/* Delay is in nano second unit. */
/* Negative value means the sensor is disabled.*/
int64_t delay[AKM_NUM_SENSORS];
int i;
if (AKD_GetDelay(delay) < 0) {
AKMERROR;
return AKM_FAIL;
}
AKMDATA(AKMDATA_GETINTERVAL,
"delay[A,M,O]=%" PRIi64 ",%" PRIi64 ",%" PRIi64 "\n",
delay[0], delay[1], delay[2]);
/* update */
*minimum = 1000000000;
*flag = 0;
for (i=0; i<AKM_NUM_SENSORS; i++) {
/* Set flag */
if (delay[i] >= 0) {
*flag |= 1 << i;
if (*minimum > delay[i]) {
*minimum = delay[i];
}
}
}
return AKM_SUCCESS;
}
/*!
If this program run as console mode, measurement result will be displayed
on console terminal.
@return If this function succeeds, the return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_FAIL.
*/
void AKFS_OutputResult(
const uint16 flag,
const AKSENSOR_DATA* acc,
const AKSENSOR_DATA* mag,
const AKSENSOR_DATA* ori
)
{
int buf[YPR_DATA_SIZE];
/* Store to buffer */
buf[0] = flag; /* Data flag */
buf[1] = CONVERT_ACC(acc->x); /* Ax */
buf[2] = CONVERT_ACC(acc->y); /* Ay */
buf[3] = CONVERT_ACC(acc->z); /* Az */
buf[4] = acc->status; /* Acc status */
buf[5] = CONVERT_MAG(mag->x); /* Mx */
buf[6] = CONVERT_MAG(mag->y); /* My */
buf[7] = CONVERT_MAG(mag->z); /* Mz */
buf[8] = mag->status; /* Mag status */
buf[9] = CONVERT_ORI(ori->x); /* yaw */
buf[10] = CONVERT_ORI(ori->y); /* pitch */
buf[11] = CONVERT_ORI(ori->z); /* roll */
if (g_opmode & OPMODE_CONSOLE) {
/* Console mode */
Disp_Result(buf);
}
/* Set result to driver */
AKD_SetYPR(buf);
}
/*!
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;
}
}