/*!
Initialize library. At first, 0 is set to all parameters. After that, some
parameters, which should not be 0, are set to specific value. Some of initial
values can be customized by editing the file \c "AKFS_CSpec.h".
@return The return value is #AKM_SUCCESS.
@param[in] hpat Specify a layout pattern number. The number is determined
according to the mount orientation of the magnetometer.
@param[in] regs[3] Specify the ASA values which are read out from
fuse ROM. regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ.
*/
int16 AKFS_Init(
const AKFS_PATNO hpat,
const uint8 regs[]
)
{
AKMDATA(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n",
__FUNCTION__, hpat, regs[0], regs[1], regs[2]);
/* Set 0 to the AK8975 structure. */
memset(&g_prms, 0, sizeof(AK8975PRMS));
/* Sensitivity */
g_prms.mfv_hs.u.x = AK8975_HSENSE_DEFAULT;
g_prms.mfv_hs.u.y = AK8975_HSENSE_DEFAULT;
g_prms.mfv_hs.u.z = AK8975_HSENSE_DEFAULT;
g_prms.mfv_as.u.x = AK8975_ASENSE_DEFAULT;
g_prms.mfv_as.u.y = AK8975_ASENSE_DEFAULT;
g_prms.mfv_as.u.z = AK8975_ASENSE_DEFAULT;
/* Initialize variables that initial value is not 0. */
g_prms.mi_hnaveV = CSPEC_HNAVE_V;
g_prms.mi_hnaveD = CSPEC_HNAVE_D;
g_prms.mi_anaveV = CSPEC_ANAVE_V;
g_prms.mi_anaveD = CSPEC_ANAVE_D;
/* Copy ASA values */
g_prms.mi_asa.u.x = regs[0];
g_prms.mi_asa.u.y = regs[1];
g_prms.mi_asa.u.z = regs[2];
/* Copy layout pattern */
g_prms.m_hpat = hpat;
return AKM_SUCCESS;
}
/*!
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]=%lld,%lld,%lld\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;
}
/*!
This function is called when a measurement sequence is done.
This fucntion writes parameters to file.
@return The return value is #AKM_SUCCESS.
@param[in] path Specify a path to the settings file.
*/
int16 AKFS_Stop(
const char* path
)
{
AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);
/* Write setting files to a file */
if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) {
AKMERROR_STR("AKFS_Save");
}
return AKM_SUCCESS;
}
/*!
Get orientation sensor's elements. The vector format and coordination system
follow the Android definition. Before this function is called, magnetic
field vector and acceleration vector should be stored in the buffer by
calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER.
@return The return value is #AKM_SUCCESS when function succeeds. Otherwise
the return value is #AKM_FAIL.
@param[out] azimuth Azimuthal angle in degree.
@param[out] pitch Pitch angle in degree.
@param[out] roll Roll angle in degree.
@param[out] accuracy Accuracy of orientation sensor.
*/
int16 AKFS_Get_ORIENTATION(
AKFLOAT* azimuth,
AKFLOAT* pitch,
AKFLOAT* roll,
int16* accuracy
)
{
int16 akret;
/* Azimuth calculation */
/* Coordination system follows the Android coordination. */
akret = AKFS_Direction(
AKFS_HDATA_SIZE,
g_prms.mfv_hvbuf,
CSPEC_HNAVE_D,
AKFS_ADATA_SIZE,
g_prms.mfv_avbuf,
CSPEC_ANAVE_D,
&g_prms.mf_azimuth,
&g_prms.mf_pitch,
&g_prms.mf_roll
);
if(akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
*azimuth = g_prms.mf_azimuth;
*pitch = g_prms.mf_pitch;
*roll = g_prms.mf_roll;
*accuracy = g_prms.mi_hstatus;
/* Debug output */
AKMDATA(AKMDATA_ORI, "Ori(%d):%8.2f, %8.2f, %8.2f\n",
*accuracy, *azimuth, *pitch, *roll);
return AKM_SUCCESS;
}
/*
This function is called just before a measurement sequence starts.
This function reads parameters from file, then initializes algorithm
parameters.
@return The return value is #AKM_SUCCESS.
@param[in] path Specify a path to the settings file.
*/
int16 AKFS_Start(
const char* path
)
{
AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);
/* Read setting files from a file */
if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) {
AKMERROR_STR("AKFS_Load");
}
/* Initialize buffer */
AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata);
AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf);
AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata);
AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf);
/* Initialize for AOC */
AKFS_InitAOC(&g_prms.m_aocv);
/* Initialize magnetic status */
g_prms.mi_hstatus = 0;
return AKM_SUCCESS;
}
/*!
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;
}
}
/*!
This function is called when new accelerometer data is available. The output
vector format and coordination system follow the Android definition.
@return The return value is #AKM_SUCCESS when function succeeds. Otherwise
the return value is #AKM_FAIL.
@param[in] acc A set of measurement data from accelerometer. X axis value
should be in acc[0], Y axis value should be in acc[1], Z axis value should be
in acc[2].
@param[in] status A status of accelerometer. This status indicates the result
of acceleration data, i.e. overflow, success or fail, etc.
@param[out] vx X axis value of acceleration vector.
@param[out] vy Y axis value of acceleration vector.
@param[out] vz Z axis value of acceleration vector.
@param[out] accuracy Accuracy of acceleration vector.
This value is always 3.
*/
int16 AKFS_Get_ACCELEROMETER(
const int16 acc[3],
const int16 status,
AKFLOAT* vx,
AKFLOAT* vy,
AKFLOAT* vz,
int16* accuracy
)
{
int16 akret;
AKMDATA(AKMDATA_DUMP, "%s: a[0]=%d, a[1]=%d, a[2]=%d, st=%d\n",
__FUNCTION__, acc[0], acc[1], acc[2], status);
/* Save data to buffer */
AKFS_BufShift(
AKFS_ADATA_SIZE,
1,
g_prms.mfv_adata
);
g_prms.mfv_adata[0].u.x = acc[0];
g_prms.mfv_adata[0].u.y = acc[1];
g_prms.mfv_adata[0].u.z = acc[2];
/* Subtract offset, adjust sensitivity */
/* As a result, a unit of acceleration data in mfv_avbuf is '1G = 9.8' */
akret = AKFS_VbNorm(
AKFS_ADATA_SIZE,
g_prms.mfv_adata,
1,
&g_prms.mfv_ao,
&g_prms.mfv_as,
AK8975_ASENSE_TARGET,
AKFS_ADATA_SIZE,
g_prms.mfv_avbuf
);
if(akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
/* Averaging */
akret = AKFS_VbAve(
AKFS_ADATA_SIZE,
g_prms.mfv_avbuf,
CSPEC_ANAVE_V,
&g_prms.mfv_avec
);
if (akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
/* Adjust coordination */
/* It is not needed. Because, the data from AK8975 driver is already
follows Android coordinate system. */
*vx = g_prms.mfv_avec.u.x;
*vy = g_prms.mfv_avec.u.y;
*vz = g_prms.mfv_avec.u.z;
*accuracy = 3;
/* Debug output */
AKMDATA(AKMDATA_ACC, "Acc(%d):%8.2f, %8.2f, %8.2f\n",
*accuracy, *vx, *vy, *vz);
return AKM_SUCCESS;
}
/*!
This function is called when new magnetometer data is available. The output
vector format and coordination system follow the Android definition.
@return The return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_FAIL.
@param[in] mag A set of measurement data from magnetometer. X axis value
should be in mag[0], Y axis value should be in mag[1], Z axis value should be
in mag[2].
@param[in] status A status of magnetometer. This status indicates the result
of measurement data, i.e. overflow, success or fail, etc.
@param[out] vx X axis value of magnetic field vector.
@param[out] vy Y axis value of magnetic field vector.
@param[out] vz Z axis value of magnetic field vector.
@param[out] accuracy Accuracy of magnetic field vector.
*/
int16 AKFS_Get_MAGNETIC_FIELD(
const int16 mag[3],
const int16 status,
AKFLOAT* vx,
AKFLOAT* vy,
AKFLOAT* vz,
int16* accuracy
)
{
int16 akret;
int16 aocret;
AKFLOAT radius;
AKMDATA(AKMDATA_DUMP, "%s: m[0]=%d, m[1]=%d, m[2]=%d, st=%d\n",
__FUNCTION__, mag[0], mag[1], mag[2], status);
/* Decomposition */
/* Sensitivity adjustment, i.e. multiply ASA, is done in this function. */
akret = AKFS_DecompAK8975(
mag,
status,
&g_prms.mi_asa,
AKFS_HDATA_SIZE,
g_prms.mfv_hdata
);
if(akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
/* Adjust coordination */
akret = AKFS_Rotate(
g_prms.m_hpat,
&g_prms.mfv_hdata[0]
);
if (akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
/* AOC for magnetometer */
/* Offset estimation is done in this function */
aocret = AKFS_AOC(
&g_prms.m_aocv,
g_prms.mfv_hdata,
&g_prms.mfv_ho
);
/* Subtract offset */
/* Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. */
akret = AKFS_VbNorm(
AKFS_HDATA_SIZE,
g_prms.mfv_hdata,
1,
&g_prms.mfv_ho,
&g_prms.mfv_hs,
AK8975_HSENSE_TARGET,
AKFS_HDATA_SIZE,
g_prms.mfv_hvbuf
);
if(akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
/* Averaging */
akret = AKFS_VbAve(
AKFS_HDATA_SIZE,
g_prms.mfv_hvbuf,
CSPEC_HNAVE_V,
&g_prms.mfv_hvec
);
if (akret == AKFS_ERROR) {
AKMERROR;
return AKM_FAIL;
}
/* Check the size of magnetic vector */
radius = AKFS_SQRT(
(g_prms.mfv_hvec.u.x * g_prms.mfv_hvec.u.x) +
(g_prms.mfv_hvec.u.y * g_prms.mfv_hvec.u.y) +
(g_prms.mfv_hvec.u.z * g_prms.mfv_hvec.u.z));
if (radius > AKFS_GEOMAG_MAX) {
g_prms.mi_hstatus = 0;
} else {
if (aocret) {
g_prms.mi_hstatus = 3;
}
}
*vx = g_prms.mfv_hvec.u.x;
*vy = g_prms.mfv_hvec.u.y;
*vz = g_prms.mfv_hvec.u.z;
*accuracy = g_prms.mi_hstatus;
/* Debug output */
AKMDATA(AKMDATA_MAG, "Mag(%d):%8.2f, %8.2f, %8.2f\n",
*accuracy, *vx, *vy, *vz);
return AKM_SUCCESS;
}
