/*!
*/
int16 AKFS_VbNorm(
const int16 ndata, /*!< Size of raw vector buffer */
const AKFVEC vdata[], /*!< Raw vector buffer */
const int16 nbuf, /*!< Size of data to be buffered */
const AKFVEC* o, /*!< Offset */
const AKFVEC* s, /*!< Sensitivity */
const AKFLOAT tgt, /*!< Target sensitivity */
const int16 nvec, /*!< Size of normalized vector buffer */
AKFVEC vvec[] /*!< Normalized vector buffer */
)
{
int i;
/* size check */
if ((ndata <= 0) || (nvec <= 0) || (nbuf <= 0)) {
return AKFS_ERROR;
}
/* dependency check */
if ((nbuf < 1) || (ndata < nbuf) || (nvec < nbuf)) {
return AKFS_ERROR;
}
/* sensitivity check */
if ((s->u.x <= AKFS_EPSILON) ||
(s->u.y <= AKFS_EPSILON) ||
(s->u.z <= AKFS_EPSILON) ||
(tgt <= 0)) {
return AKFS_ERROR;
}
/* calculate and store data to buffer */
if (AKFS_BufShift(nvec, nbuf, vvec) != AKFS_SUCCESS) {
return AKFS_ERROR;
}
for (i=0; i<nbuf; i++) {
vvec[i].u.x = ((vdata[i].u.x - o->u.x) / (s->u.x) * (AKFLOAT)tgt);
vvec[i].u.y = ((vdata[i].u.y - o->u.y) / (s->u.y) * (AKFLOAT)tgt);
vvec[i].u.z = ((vdata[i].u.z - o->u.z) / (s->u.z) * (AKFLOAT)tgt);
}
return AKFS_SUCCESS;
}
/*!
*/
int16 AKFS_DecompAK8975(
const int16 mag[3],
const int16 status,
const uint8vec* asa,
const int16 nhdata,
AKFVEC hdata[]
)
{
/* put st1 and st2 value */
if (AK8975_ST_ERROR(status)) {
return AKFS_ERROR;
}
/* magnetic */
AKFS_BufShift(nhdata, 1, hdata);
hdata[0].u.x = mag[0] * (((asa->u.x)/256.0f) + 0.5f);
hdata[0].u.y = mag[1] * (((asa->u.y)/256.0f) + 0.5f);
hdata[0].u.z = mag[2] * (((asa->u.z)/256.0f) + 0.5f);
return AKFS_SUCCESS;
}
/*!
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;
}
