int16 AKFS_Direction(
const int16 nhvec,
const AKFVEC hvec[],
const int16 hnave,
const int16 navec,
const AKFVEC avec[],
const int16 anave,
AKFLOAT* azimuth,
AKFLOAT* pitch,
AKFLOAT* roll
)
{
AKFVEC have, aave;
AKFLOAT azimuthRad;
AKFLOAT pitchRad;
AKFLOAT rollRad;
/* arguments check */
if ((nhvec <= 0) || (navec <= 0) || (hnave <= 0) || (anave <= 0)) {
return AKFS_ERROR;
}
if ((nhvec < hnave) || (navec < anave)) {
return AKFS_ERROR;
}
/* average */
if (AKFS_VbAve(nhvec, hvec, hnave, &have) != AKFS_SUCCESS) {
return AKFS_ERROR;
}
if (AKFS_VbAve(navec, avec, anave, &aave) != AKFS_SUCCESS) {
return AKFS_ERROR;
}
/* calculate pitch and roll */
AKFS_Angle(&aave, &pitchRad, &rollRad);
/* calculate azimuth */
AKFS_Azimuth(&have, pitchRad, rollRad, &azimuthRad);
*azimuth = RAD2DEG(azimuthRad);
*pitch = RAD2DEG(pitchRad);
*roll = RAD2DEG(rollRad);
/* Adjust range of azimuth */
if (*azimuth < 0) {
*azimuth += 360.0f;
}
return AKFS_SUCCESS;
}
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 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;
}
