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; }