/*
This function is used internaly, so output is RADIAN!
*/
static void AKFS_Angle(
const AKFVEC* avec,
AKFLOAT* pitch, /* radian */
AKFLOAT* roll /* radian */
)
{
AKFLOAT av; /* Size of vector */
av = AKFS_SQRT((avec->u.x)*(avec->u.x) + (avec->u.y)*(avec->u.y) + (avec->u.z)*(avec->u.z));
*pitch = AKFS_ASIN(-(avec->u.y) / av);
*roll = AKFS_ASIN((avec->u.x) / av);
}
static int convert_magnetic(sensors_vec_t *raw, sensors_vec_t *result,
struct sensor_algo_args *args)
{
int16 akret;
int16 aocret;
AKFLOAT radius;
AKMPRMS *prms = &g_prms;
int i;
/* Shift out old data from the buffer for better calibration */
for (i = AKFS_HDATA_SIZE - 1; i >= 1; i--) {
prms->fva_hdata[i] = prms->fva_hdata[i - 1];
}
prms->fva_hdata[0].u.x = raw->x;
prms->fva_hdata[0].u.y = raw->y;
prms->fva_hdata[0].u.z = raw->z;
/* Offset calculation is done in this function */
/* hdata[in] : Android coordinate, sensitivity adjusted. */
/* ho [out]: Android coordinate, sensitivity adjusted. */
aocret = AKFS_AOC(
&prms->s_aocv,
prms->fva_hdata,
&prms->fv_ho
);
/* Subtract offset */
/* hdata[in] : Android coordinate, sensitivity adjusted. */
/* ho [in] : Android coordinate, sensitivity adjusted. */
/* hvbuf[out]: Android coordinate, sensitivity adjusted, */
/* offset subtracted. */
akret = AKFS_VbNorm(
AKFS_HDATA_SIZE,
prms->fva_hdata,
1,
&prms->fv_ho,
&prms->fv_hs,
AKM_MAG_SENSE,
AKFS_HDATA_SIZE,
prms->fva_hvbuf
);
if (akret == AKFS_ERROR) {
ALOGE("error here!");
return -1;
}
/* Averaging */
/* hvbuf[in] : Android coordinate, sensitivity adjusted, */
/* offset subtracted. */
/* hvec [out]: Android coordinate, sensitivity adjusted, */
/* offset subtracted, averaged. */
akret = AKFS_VbAve(
AKFS_HDATA_SIZE,
prms->fva_hvbuf,
CSPEC_HNAVE_V,
&prms->fv_hvec
);
if (akret == AKFS_ERROR) {
ALOGE("error here!");
return -1;
}
/* Check the size of magnetic vector */
radius = AKFS_SQRT(
(prms->fv_hvec.u.x * prms->fv_hvec.u.x) +
(prms->fv_hvec.u.y * prms->fv_hvec.u.y) +
(prms->fv_hvec.u.z * prms->fv_hvec.u.z));
if (radius > AKFS_GEOMAG_MAX) {
prms->i16_hstatus = 0;
} else {
if (aocret == AKFS_SUCCESS) {
prms->i16_hstatus = 3;
}
}
result->x = prms->fv_hvec.u.x;
result->y = prms->fv_hvec.u.y;
result->z = prms->fv_hvec.u.z;
result->status = prms->i16_hstatus;
return 0;
}
/*!
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;
}