summaryrefslogtreecommitdiffstats
path: root/libsensors/akmdfs/AKFS_APIs.c
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors/akmdfs/AKFS_APIs.c')
-rw-r--r--libsensors/akmdfs/AKFS_APIs.c389
1 files changed, 389 insertions, 0 deletions
diff --git a/libsensors/akmdfs/AKFS_APIs.c b/libsensors/akmdfs/AKFS_APIs.c
new file mode 100644
index 0000000..ace9bc1
--- /dev/null
+++ b/libsensors/akmdfs/AKFS_APIs.c
@@ -0,0 +1,389 @@
+/******************************************************************************
+ *
+ * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ ******************************************************************************/
+#include "AKFS_Common.h"
+#include "AKFS_Disp.h"
+#include "AKFS_FileIO.h"
+#include "AKFS_APIs.h"
+
+#ifdef WIN32
+#include "AK8975_LinuxDriver.h"
+#endif
+
+static AK8975PRMS g_prms;
+
+/*!
+ 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;
+}
+
+/*!
+ Release resources. This function is for future expansion.
+ @return The return value is #AKM_SUCCESS.
+ */
+int16 AKFS_Release(void)
+{
+ 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 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;
+}
+
+/*!
+ 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;
+}
+
+/*!
+ 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;
+}
+
+/*!
+ 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;
+}
+