diff options
author | Jean-Baptiste Queru <jbq@google.com> | 2013-06-25 22:35:03 +0000 |
---|---|---|
committer | Gerrit Code Review <noreply-gerritcodereview@google.com> | 2013-06-25 22:35:03 +0000 |
commit | 6da5f1825c575f5b4751de0cb83c7d56e1242c66 (patch) | |
tree | b44566d63506ba4704c0bd7cfc7a1c5afbc0a503 | |
parent | 6f701daf0b67ef38f1ba1700984ec78134669cd0 (diff) | |
parent | ab17e987e7f5371df91fe4e137fcf9041489ffdf (diff) | |
download | akm-6da5f1825c575f5b4751de0cb83c7d56e1242c66.tar.gz |
Merge "AKM sensor code for AK8975"tools_r22.2jb-mr1.1-dev-plus-aosp
41 files changed, 5304 insertions, 0 deletions
diff --git a/AK8975_FS/akmdfs/AK8975Driver.c b/AK8975_FS/akmdfs/AK8975Driver.c new file mode 100644 index 0000000..14fe6bd --- /dev/null +++ b/AK8975_FS/akmdfs/AK8975Driver.c @@ -0,0 +1,319 @@ +/****************************************************************************** + * $Id: AK8975Driver.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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 <fcntl.h> +#include "AKFS_Common.h" +#include "AK8975Driver.h" + +#define MSENSOR_NAME "/dev/akm8975_dev" + +static int s_fdDev = -1; + +/*! + Open device driver. + This function opens both device drivers of magnetic sensor and acceleration + sensor. Additionally, some initial hardware settings are done, such as + measurement range, built-in filter function and etc. + @return If this function succeeds, the return value is #AKD_SUCCESS. + Otherwise the return value is #AKD_FAIL. + */ +int16_t AKD_InitDevice(void) +{ + if (s_fdDev < 0) { + /* Open magnetic sensor's device driver. */ + if ((s_fdDev = open(MSENSOR_NAME, O_RDWR)) < 0) { + AKMERROR_STR("open"); + return AKD_FAIL; + } + } + + return AKD_SUCCESS; +} + +/*! + Close device driver. + This function closes both device drivers of magnetic sensor and acceleration + sensor. + */ +void AKD_DeinitDevice(void) +{ + if (s_fdDev >= 0) { + close(s_fdDev); + s_fdDev = -1; + } +} + +/*! + Writes data to a register of the AK8975. When more than one byte of data is + specified, the data is written in contiguous locations starting at an address + specified in \a address. + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + @param[in] address Specify the address of a register in which data is to be + written. + @param[in] data Specify data to write or a pointer to a data array containing + the data. When specifying more than one byte of data, specify the starting + address of the array. + @param[in] numberOfBytesToWrite Specify the number of bytes that make up the + data to write. When a pointer to an array is specified in data, this argument + equals the number of elements of the array. + */ +int16_t AKD_TxData( + const BYTE address, + const BYTE * data, + const uint16_t numberOfBytesToWrite) +{ + int i; + char buf[RWBUF_SIZE]; + + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + if (numberOfBytesToWrite > (RWBUF_SIZE-2)) { + ALOGE("%s: Tx size is too large.", __FUNCTION__); + return AKD_FAIL; + } + + buf[0] = numberOfBytesToWrite + 1; + buf[1] = address; + + for (i = 0; i < numberOfBytesToWrite; i++) { + buf[i + 2] = data[i]; + } + if (ioctl(s_fdDev, ECS_IOCTL_WRITE, buf) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } else { + +#if ENABLE_AKMDEBUG + AKMDATA(AKMDATA_DRV, "addr(HEX)=%02x data(HEX)=", address); + for (i = 0; i < numberOfBytesToWrite; i++) { + AKMDATA(AKMDATA_DRV, " %02x", data[i]); + } + AKMDATA(AKMDATA_DRV, "\n"); +#endif + return AKD_SUCCESS; + } +} + +/*! + Acquires data from a register or the EEPROM of the AK8975. + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + @param[in] address Specify the address of a register from which data is to be + read. + @param[out] data Specify a pointer to a data array which the read data are + stored. + @param[in] numberOfBytesToRead Specify the number of bytes that make up the + data to read. When a pointer to an array is specified in data, this argument + equals the number of elements of the array. + */ +int16_t AKD_RxData( + const BYTE address, + BYTE * data, + const uint16_t numberOfBytesToRead) +{ + int i; + char buf[RWBUF_SIZE]; + + memset(data, 0, numberOfBytesToRead); + + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + if (numberOfBytesToRead > (RWBUF_SIZE-1)) { + ALOGE("%s: Rx size is too large.", __FUNCTION__); + return AKD_FAIL; + } + + buf[0] = numberOfBytesToRead; + buf[1] = address; + + if (ioctl(s_fdDev, ECS_IOCTL_READ, buf) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } else { + for (i = 0; i < numberOfBytesToRead; i++) { + data[i] = buf[i + 1]; + } +#if ENABLE_AKMDEBUG + AKMDATA(AKMDATA_DRV, "addr(HEX)=%02x len=%d data(HEX)=", + address, numberOfBytesToRead); + for (i = 0; i < numberOfBytesToRead; i++) { + AKMDATA(AKMDATA_DRV, " %02x", data[i]); + } + AKMDATA(AKMDATA_DRV, "\n"); +#endif + return AKD_SUCCESS; + } +} + +/*! + Acquire magnetic data from AK8975. If measurement is not done, this function + waits until measurement completion. + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + @param[out] data A magnetic data array. The size should be larger than #SENSOR_DATA_SIZE. + */ +int16_t AKD_GetMagneticData(BYTE data[SENSOR_DATA_SIZE]) +{ + memset(data, 0, SENSOR_DATA_SIZE); + + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + + if (ioctl(s_fdDev, ECS_IOCTL_GETDATA, data) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + + AKMDATA(AKMDATA_DRV, + "bdata(HEX)= %02x %02x %02x %02x %02x %02x %02x %02x\n", + data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]); + + return AKD_SUCCESS; +} + +/*! + Set calculated data to device driver. + @param[in] buf The order of input data depends on driver's specification. + */ +void AKD_SetYPR(const int buf[YPR_DATA_SIZE]) +{ + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + } else { + if (ioctl(s_fdDev, ECS_IOCTL_SET_YPR, buf) < 0) { + AKMERROR_STR("ioctl"); + } + } +} + +/*! + */ +int AKD_GetOpenStatus(int* status) +{ + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + if (ioctl(s_fdDev, ECS_IOCTL_GET_OPEN_STATUS, status) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + return AKD_SUCCESS; +} + +/*! + */ +int AKD_GetCloseStatus(int* status) +{ + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + if (ioctl(s_fdDev, ECS_IOCTL_GET_CLOSE_STATUS, status) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + return AKD_SUCCESS; +} + +/*! + Set AK8975 to the specific mode. + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + @param[in] mode This value should be one of the AK8975_Mode which is defined in + akm8975.h file. + */ +int16_t AKD_SetMode(const BYTE mode) +{ + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + + if (ioctl(s_fdDev, ECS_IOCTL_SET_MODE, &mode) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + + return AKD_SUCCESS; +} + +/*! + Acquire delay + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + @param[out] delay A delay in nanosecond. + */ +int16_t AKD_GetDelay(int64_t delay[AKM_NUM_SENSORS]) +{ + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.\n", __FUNCTION__); + return AKD_FAIL; + } + if (ioctl(s_fdDev, ECS_IOCTL_GET_DELAY, delay) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + return AKD_SUCCESS; +} + +/*! + Get layout information from device driver, i.e. platform data. + */ +int16_t AKD_GetLayout(int16_t* layout) +{ + char tmp; + + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + + if (ioctl(s_fdDev, ECS_IOCTL_GET_LAYOUT, &tmp) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + + *layout = tmp; + return AKD_SUCCESS; +} + +/* Get acceleration data. */ +int16_t AKD_GetAccelerationData(int16_t data[3]) +{ + if (s_fdDev < 0) { + ALOGE("%s: Device file is not opened.", __FUNCTION__); + return AKD_FAIL; + } + if (ioctl(s_fdDev, ECS_IOCTL_GET_ACCEL, data) < 0) { + AKMERROR_STR("ioctl"); + return AKD_FAIL; + } + + AKMDATA(AKMDATA_DRV, "%s: acc=%d, %d, %d\n", + __FUNCTION__, data[0], data[1], data[2]); + + return AKD_SUCCESS; +} diff --git a/AK8975_FS/akmdfs/AK8975Driver.h b/AK8975_FS/akmdfs/AK8975Driver.h new file mode 100644 index 0000000..2f226ac --- /dev/null +++ b/AK8975_FS/akmdfs/AK8975Driver.h @@ -0,0 +1,103 @@ +/****************************************************************************** + * $Id: AK8975Driver.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKMD_INC_AK8975DRIVER_H +#define AKMD_INC_AK8975DRIVER_H + +#include "platform/include/linux/akm8975.h" /* Device driver */ +#include <stdint.h> /* int8_t, int16_t etc. */ + +/*** Constant definition ******************************************************/ +#define AKD_TRUE 1 /*!< Represents true */ +#define AKD_FALSE 0 /*!< Represents false */ +#define AKD_SUCCESS 1 /*!< Represents success.*/ +#define AKD_FAIL 0 /*!< Represents fail. */ +#define AKD_ERROR -1 /*!< Represents error. */ + +/*! 0:Don't Output data, 1:Output data */ +#define AKD_DBG_DATA 0 +/*! Typical interval in ns */ +#define AK8975_MEASUREMENT_TIME_NS ((AK8975_MEASUREMENT_TIME_US) * 1000) +/*! 720 LSG = 1G = 9.8 m/s2 */ +#define LSG 720 + + +/*** Type declaration *********************************************************/ +typedef unsigned char BYTE; + +/*! + Open device driver. + This function opens device driver of acceleration sensor. + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + */ +typedef int16_t(*ACCFNC_INITDEVICE)(void); + +/*! + Close device driver. + This function closes device drivers of acceleration sensor. + */ +typedef void(*ACCFNC_DEINITDEVICE)(void); + +/*! + Acquire acceleration data from acceleration sensor and convert it to Android + coordinate system. + @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise + the return value is #AKD_FAIL. + @param[out] data A acceleration data array. The coordinate system of the + acquired data follows the definition of Android. Unit is SmartCompass. + */ +typedef int16_t(*ACCFNC_GETACCDATA)(short data[3]); + + +/*** Global variables *********************************************************/ + +/*** Prototype of Function ***************************************************/ + +int16_t AKD_InitDevice(void); + +void AKD_DeinitDevice(void); + +int16_t AKD_TxData( + const BYTE address, + const BYTE* data, + const uint16_t numberOfBytesToWrite); + +int16_t AKD_RxData( + const BYTE address, + BYTE* data, + const uint16_t numberOfBytesToRead); + +int16_t AKD_GetMagneticData(BYTE data[SENSOR_DATA_SIZE]); + +void AKD_SetYPR(const int buf[YPR_DATA_SIZE]); + +int AKD_GetOpenStatus(int* status); + +int AKD_GetCloseStatus(int* status); + +int16_t AKD_SetMode(const BYTE mode); + +int16_t AKD_GetDelay(int64_t delay[AKM_NUM_SENSORS]); + +int16_t AKD_GetLayout(int16_t* layout); + +int16_t AKD_GetAccelerationData(int16_t data[3]); + +#endif /* AKMD_INC_AK8975DRIVER_H */ + diff --git a/AK8975_FS/akmdfs/AKFS_APIs.c b/AK8975_FS/akmdfs/AKFS_APIs.c new file mode 100644 index 0000000..e153c98 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs.c @@ -0,0 +1,390 @@ +/****************************************************************************** + * $Id: AKFS_APIs.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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; +} + diff --git a/AK8975_FS/akmdfs/AKFS_APIs.h b/AK8975_FS/akmdfs/AKFS_APIs.h new file mode 100644 index 0000000..32076b8 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs.h @@ -0,0 +1,70 @@ +/****************************************************************************** + * $Id: AKFS_APIs.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_APIS_H +#define AKFS_INC_APIS_H + +/* Include files for AK8975 library. */ +#include "AKFS_Compass.h" + +/*** Constant definition ******************************************************/ +#define AKFS_GEOMAG_MAX 70 + +/*** Type declaration *********************************************************/ + +/*** Global variables *********************************************************/ + +/*** Prototype of function ****************************************************/ +int16 AKFS_Init( + const AKFS_PATNO hpat, + const uint8 regs[] +); + +int16 AKFS_Release(void); + +int16 AKFS_Start(const char* path); + +int16 AKFS_Stop(const char* path); + +int16 AKFS_Get_MAGNETIC_FIELD( + const int16 mag[3], + const int16 status, + AKFLOAT* vx, + AKFLOAT* vy, + AKFLOAT* vz, + int16* accuracy +); + +int16 AKFS_Get_ACCELEROMETER( + const int16 acc[3], + const int16 status, + AKFLOAT* vx, + AKFLOAT* vy, + AKFLOAT* vz, + int16* accuracy +); + +int16 AKFS_Get_ORIENTATION( + AKFLOAT* azimuth, + AKFLOAT* pitch, + AKFLOAT* roll, + int16* accuracy +); + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c new file mode 100644 index 0000000..48c2f23 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c @@ -0,0 +1,45 @@ +/****************************************************************************** + * $Id: AKFS_AK8975.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_AK8975.h" +#include "AKFS_Device.h" + +/*! + */ +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; +} + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h new file mode 100644 index 0000000..65e45fe --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h @@ -0,0 +1,51 @@ +/****************************************************************************** + * $Id: AKFS_AK8975.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_AK8975_H +#define AKFS_INC_AK8975_H + +#include "AKFS_Device.h" + +/***** Constant definition ****************************************************/ +#define AK8975_BDATA_SIZE 8 + +#define AK8975_HSENSE_DEFAULT 1 +#define AK8975_HSENSE_TARGET 0.3f +#define AK8975_ASENSE_DEFAULT 720 +#define AK8975_ASENSE_TARGET 9.80665f + +#define AK8975_HDATA_CONVERTER(hi, low, asa) \ + (AKFLOAT)((int16)((((uint16)(hi))<<8)+(uint16)(low))*(((asa)/256.0f) + 0.5f)) + +#define AK8975_ST_ERROR(st) (((st)&0x09) != 0x01) + +/***** Type declaration *******************************************************/ + +/***** Prototype of function **************************************************/ +AKLIB_C_API_START +int16 AKFS_DecompAK8975( + const int16 mag[3], + const int16 status, + const uint8vec* asa, + const int16 nhdata, + AKFVEC hdata[] +); +AKLIB_C_API_END + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AOC.c b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AOC.c new file mode 100644 index 0000000..f597103 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AOC.c @@ -0,0 +1,334 @@ +/****************************************************************************** + * $Id: AKFS_AOC.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_AOC.h" +#include "AKFS_Math.h" + +/* + * CalcR + */ +static AKFLOAT CalcR( + const AKFVEC* x, + const AKFVEC* y +){ + int16 i; + AKFLOAT r; + + r = 0.0; + for(i = 0; i < 3; i++){ + r += (x->v[i]-y->v[i]) * (x->v[i]-y->v[i]); + } + r = sqrt(r); + + return r; +} + +/* + * From4Points2Sphere() + */ +static int16 From4Points2Sphere( + const AKFVEC points[], /*! (i/o) : input vectors */ + AKFVEC* center, /*! (o) : center of sphere */ + AKFLOAT* r /*! (i) : add/subtract value */ +){ + AKFLOAT dif[3][3]; + AKFLOAT r2[3]; + + AKFLOAT A; + AKFLOAT B; + AKFLOAT C; + AKFLOAT D; + AKFLOAT E; + AKFLOAT F; + AKFLOAT G; + + AKFLOAT OU; + AKFLOAT OD; + + int16 i, j; + + for(i = 0; i < 3; i++){ + r2[i] = 0.0; + for(j = 0; j < 3; j++){ + dif[i][j] = points[i].v[j] - points[3].v[j]; + r2[i] += (points[i].v[j]*points[i].v[j] + - points[3].v[j]*points[3].v[j]); + } + r2[i] *= 0.5; + } + + A = dif[0][0]*dif[2][2] - dif[0][2]*dif[2][0]; + B = dif[0][1]*dif[2][0] - dif[0][0]*dif[2][1]; + C = dif[0][0]*dif[2][1] - dif[0][1]*dif[2][0]; + D = dif[0][0]*r2[2] - dif[2][0]*r2[0]; + E = dif[0][0]*dif[1][1] - dif[0][1]*dif[1][0]; + F = dif[1][0]*dif[0][2] - dif[0][0]*dif[1][2]; + G = dif[0][0]*r2[1] - dif[1][0]*r2[0]; + + OU = D*E + B*G; + OD = C*F + A*E; + + if(fabs(OD) < AKFS_EPSILON){ + return -1; + } + + center->v[2] = OU / OD; + + OU = F*center->v[2] + G; + OD = E; + + if(fabs(OD) < AKFS_EPSILON){ + return -1; + } + + center->v[1] = OU / OD; + + OU = r2[0] - dif[0][1]*center->v[1] - dif[0][2]*center->v[2]; + OD = dif[0][0]; + + if(fabs(OD) < AKFS_EPSILON){ + return -1; + } + + center->v[0] = OU / OD; + + *r = CalcR(&points[0], center); + + return 0; + +} + +/* + * MeanVar + */ +static void MeanVar( + const AKFVEC v[], /*!< (i) : input vectors */ + const int16 n, /*!< (i) : number of vectors */ + AKFVEC* mean, /*!< (o) : (max+min)/2 */ + AKFVEC* var /*!< (o) : variation in vectors */ +){ + int16 i; + int16 j; + AKFVEC max; + AKFVEC min; + + for(j = 0; j < 3; j++){ + min.v[j] = v[0].v[j]; + max.v[j] = v[0].v[j]; + for(i = 1; i < n; i++){ + if(v[i].v[j] < min.v[j]){ + min.v[j] = v[i].v[j]; + } + if(v[i].v[j] > max.v[j]){ + max.v[j] = v[i].v[j]; + } + } + mean->v[j] = (max.v[j] + min.v[j]) / 2.0; /*mean */ + var->v[j] = max.v[j] - min.v[j]; /*var */ + } +} + +/* + * Get4points + */ +static void Get4points( + const AKFVEC v[], /*!< (i) : input vectors */ + const int16 n, /*!< (i) : number of vectors */ + AKFVEC out[] /*!< (o) : */ +){ + int16 i, j; + AKFLOAT temp; + AKFLOAT d; + + AKFVEC dv[AKFS_HBUF_SIZE]; + AKFVEC cross; + AKFVEC tempv; + + /* out 0 */ + out[0] = v[0]; + + /* out 1 */ + d = 0.0; + for(i = 1; i < n; i++){ + temp = CalcR(&v[i], &out[0]); + if(d < temp){ + d = temp; + out[1] = v[i]; + } + } + + /* out 2 */ + d = 0.0; + for(j = 0; j < 3; j++){ + dv[0].v[j] = out[1].v[j] - out[0].v[j]; + } + for(i = 1; i < n; i++){ + for(j = 0; j < 3; j++){ + dv[i].v[j] = v[i].v[j] - out[0].v[j]; + } + tempv.v[0] = dv[0].v[1]*dv[i].v[2] - dv[0].v[2]*dv[i].v[1]; + tempv.v[1] = dv[0].v[2]*dv[i].v[0] - dv[0].v[0]*dv[i].v[2]; + tempv.v[2] = dv[0].v[0]*dv[i].v[1] - dv[0].v[1]*dv[i].v[0]; + temp = tempv.u.x * tempv.u.x + + tempv.u.y * tempv.u.y + + tempv.u.z * tempv.u.z; + if(d < temp){ + d = temp; + out[2] = v[i]; + cross = tempv; + } + } + + /* out 3 */ + d = 0.0; + for(i = 1; i < n; i++){ + temp = dv[i].u.x * cross.u.x + + dv[i].u.y * cross.u.y + + dv[i].u.z * cross.u.z; + temp = fabs(temp); + if(d < temp){ + d = temp; + out[3] = v[i]; + } + } +} + +/* + * CheckInitFvec + */ +static int16 CheckInitFvec( + const AKFVEC *v /*!< [in] vector */ +){ + int16 i; + + for(i = 0; i < 3; i++){ + if(AKFS_FMAX <= v->v[i]){ + return 1; /* initvalue */ + } + } + + return 0; /* not initvalue */ +} + +/* + * AKFS_AOC + */ +int16 AKFS_AOC( /*!< (o) : calibration success(1), failure(0) */ + AKFS_AOC_VAR* haocv, /*!< (i/o) : a set of variables */ + const AKFVEC* hdata, /*!< (i) : vectors of data */ + AKFVEC* ho /*!< (i/o) : offset */ +){ + int16 i, j; + int16 num; + AKFLOAT tempf; + AKFVEC tempho; + + AKFVEC fourpoints[4]; + + AKFVEC var; + AKFVEC mean; + + /* buffer new data */ + for(i = 1; i < AKFS_HBUF_SIZE; i++){ + haocv->hbuf[AKFS_HBUF_SIZE-i] = haocv->hbuf[AKFS_HBUF_SIZE-i-1]; + } + haocv->hbuf[0] = *hdata; + + /* Check Init */ + num = 0; + for(i = AKFS_HBUF_SIZE; 3 < i; i--){ + if(CheckInitFvec(&haocv->hbuf[i-1]) == 0){ + num = i; + break; + } + } + if(num < 4){ + return AKFS_ERROR; + } + + /* get 4 points */ + Get4points(haocv->hbuf, num, fourpoints); + + /* estimate offset */ + if(0 != From4Points2Sphere(fourpoints, &tempho, &haocv->hraoc)){ + return AKFS_ERROR; + } + + /* check distance */ + for(i = 0; i < 4; i++){ + for(j = (i+1); j < 4; j++){ + tempf = CalcR(&fourpoints[i], &fourpoints[j]); + if((tempf < haocv->hraoc)||(tempf < AKFS_HR_TH)){ + return AKFS_ERROR; + } + } + } + + /* update offset buffer */ + for(i = 1; i < AKFS_HOBUF_SIZE; i++){ + haocv->hobuf[AKFS_HOBUF_SIZE-i] = haocv->hobuf[AKFS_HOBUF_SIZE-i-1]; + } + haocv->hobuf[0] = tempho; + + /* clear hbuf */ + for(i = (AKFS_HBUF_SIZE>>1); i < AKFS_HBUF_SIZE; i++) { + for(j = 0; j < 3; j++) { + haocv->hbuf[i].v[j]= AKFS_FMAX; + } + } + + /* Check Init */ + if(CheckInitFvec(&haocv->hobuf[AKFS_HOBUF_SIZE-1]) == 1){ + return AKFS_ERROR; + } + + /* Check ovar */ + tempf = haocv->hraoc * AKFS_HO_TH; + MeanVar(haocv->hobuf, AKFS_HOBUF_SIZE, &mean, &var); + if ((var.u.x >= tempf) || (var.u.y >= tempf) || (var.u.z >= tempf)){ + return AKFS_ERROR; + } + + *ho = mean; + + return AKFS_SUCCESS; +} + +/* + * AKFS_InitAOC + */ +void AKFS_InitAOC( + AKFS_AOC_VAR* haocv +){ + int16 i, j; + + /* Initialize buffer */ + for(i = 0; i < AKFS_HBUF_SIZE; i++) { + for(j = 0; j < 3; j++) { + haocv->hbuf[i].v[j]= AKFS_FMAX; + } + } + for(i = 0; i < AKFS_HOBUF_SIZE; i++) { + for(j = 0; j < 3; j++) { + haocv->hobuf[i].v[j]= AKFS_FMAX; + } + } + + haocv->hraoc = 0.0; +} + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AOC.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AOC.h new file mode 100644 index 0000000..a305eed --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_AOC.h @@ -0,0 +1,54 @@ +/****************************************************************************** + * $Id: AKFS_AOC.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_AOC_H +#define AKFS_INC_AOC_H + +#include "AKFS_Device.h" + +/***** Constant definition ****************************************************/ +#define AKFS_HBUF_SIZE 20 +#define AKFS_HOBUF_SIZE 4 +#define AKFS_HR_TH 10 +#define AKFS_HO_TH 0.15 + +/***** Macro definition *******************************************************/ + +/***** Type declaration *******************************************************/ +typedef struct _AKFS_AOC_VAR{ + AKFVEC hbuf[AKFS_HBUF_SIZE]; + AKFVEC hobuf[AKFS_HOBUF_SIZE]; + AKFLOAT hraoc; +} AKFS_AOC_VAR; + +/***** Prototype of function **************************************************/ +AKLIB_C_API_START +int16 AKFS_AOC( + AKFS_AOC_VAR* haocv, + const AKFVEC* hdata, + AKFVEC* ho +); + +void AKFS_InitAOC( + AKFS_AOC_VAR* haocv +); + +AKLIB_C_API_END + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Configure.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Configure.h new file mode 100644 index 0000000..ecec01c --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Configure.h @@ -0,0 +1,38 @@ +/****************************************************************************** + * $Id: AKFS_Configure.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_CONFIG_H +#define AKFS_INC_CONFIG_H + +/***** Language configuration *************************************************/ +#if defined(__cplusplus) +#define AKLIB_C_API_START extern "C" { +#define AKLIB_C_API_END } +#else +#define AKLIB_C_API_START +#define AKLIB_C_API_END +#endif + +/*! If following line is commented in, double type is used for floating point + calculation */ +/* +#define AKFS_PRECISION_DOUBLE +*/ + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Device.c b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Device.c new file mode 100644 index 0000000..9b91030 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Device.c @@ -0,0 +1,111 @@ +/****************************************************************************** + * $Id: AKFS_Device.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_Device.h" + +int16 AKFS_InitBuffer( + const int16 ndata, /*!< Size of vector buffer */ + AKFVEC vdata[] /*!< Vector buffer */ +) +{ + int i; + + /* size check */ + if (ndata <= 0) { + return AKFS_ERROR; + } + + for (i=0; i<ndata; i++) { + vdata[i].u.x = AKFS_INIT_VALUE_F; + vdata[i].u.y = AKFS_INIT_VALUE_F; + vdata[i].u.z = AKFS_INIT_VALUE_F; + } + + return AKFS_SUCCESS; +} + +int16 AKFS_BufShift( + const int16 len, /*!< size of buffer */ + const int16 shift, /*!< shift size */ + AKFVEC v[] /*!< buffer */ +) +{ + int16 i; + + if((shift < 1) || (len < shift)) { + return AKFS_ERROR; + } + for (i = len-1; i >= shift; i--) { + v[i] = v[i-shift]; + } + return AKFS_SUCCESS; +} + +int16 AKFS_Rotate( + const AKFS_PATNO pat, + AKFVEC* vec +) +{ + AKFLOAT tmp; + switch(pat){ + /* Obverse */ + case PAT1: + /* This is Android default */ + break; + case PAT2: + tmp = vec->u.x; + vec->u.x = vec->u.y; + vec->u.y = -tmp; + break; + case PAT3: + vec->u.x = -(vec->u.x); + vec->u.y = -(vec->u.y); + break; + case PAT4: + tmp = vec->u.x; + vec->u.x = -(vec->u.y); + vec->u.y = tmp; + break; + /* Reverse */ + case PAT5: + vec->u.x = -(vec->u.x); + vec->u.z = -(vec->u.z); + break; + case PAT6: + tmp = vec->u.x; + vec->u.x = vec->u.y; + vec->u.y = tmp; + vec->u.z = -(vec->u.z); + break; + case PAT7: + vec->u.y = -(vec->u.y); + vec->u.z = -(vec->u.z); + break; + case PAT8: + tmp = vec->u.x; + vec->u.x = -(vec->u.y); + vec->u.y = -tmp; + vec->u.z = -(vec->u.z); + break; + default: + return AKFS_ERROR; + } + + return AKFS_SUCCESS; +} + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Device.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Device.h new file mode 100644 index 0000000..149f0c6 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Device.h @@ -0,0 +1,108 @@ +/****************************************************************************** + * $Id: AKFS_Device.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_DEVICE_H +#define AKFS_INC_DEVICE_H + +#include <float.h> +#include "AKFS_Configure.h" + +/***** Constant definition ****************************************************/ +#define AKFS_ERROR 0 +#define AKFS_SUCCESS 1 + +#define AKFS_HDATA_SIZE 32 +#define AKFS_ADATA_SIZE 32 + +/***** Type declaration *******************************************************/ +typedef signed char int8; +typedef signed short int16; +typedef unsigned char uint8; +typedef unsigned short uint16; + + +#ifdef AKFS_PRECISION_DOUBLE +typedef double AKFLOAT; +#define AKFS_EPSILON DBL_EPSILON +#define AKFS_FMAX DBL_MAX +#define AKFS_FMIN DBL_MIN + +#else +typedef float AKFLOAT; +#define AKFS_EPSILON FLT_EPSILON +#define AKFS_FMAX FLT_MAX +#define AKFS_FMIN FLT_MIN + +#endif + +/* Treat maximum value as initial value */ +#define AKFS_INIT_VALUE_F AKFS_FMAX + +/***** Vector *****/ +typedef union _uint8vec{ + struct { + uint8 x; + uint8 y; + uint8 z; + }u; + uint8 v[3]; +} uint8vec; + +typedef union _AKFVEC{ + struct { + AKFLOAT x; + AKFLOAT y; + AKFLOAT z; + }u; + AKFLOAT v[3]; +} AKFVEC; + +/***** Layout pattern *****/ +typedef enum _AKFS_PATNO { + PAT_INVALID = 0, + PAT1, /* obverse: 1st pin is right down */ + PAT2, /* obverse: 1st pin is left down */ + PAT3, /* obverse: 1st pin is left top */ + PAT4, /* obverse: 1st pin is right top */ + PAT5, /* reverse: 1st pin is left down (from top view) */ + PAT6, /* reverse: 1st pin is left top (from top view) */ + PAT7, /* reverse: 1st pin is right top (from top view) */ + PAT8 /* reverse: 1st pin is right down (from top view) */ +} AKFS_PATNO; + +/***** Prototype of function **************************************************/ +AKLIB_C_API_START +int16 AKFS_InitBuffer( + const int16 ndata, /*!< Size of raw vector buffer */ + AKFVEC vdata[] /*!< Raw vector buffer */ +); + +int16 AKFS_BufShift( + const int16 len, + const int16 shift, + AKFVEC v[] +); + +int16 AKFS_Rotate( + const AKFS_PATNO pat, + AKFVEC* vec +); +AKLIB_C_API_END + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Direction.c b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Direction.c new file mode 100644 index 0000000..2c7265e --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Direction.c @@ -0,0 +1,134 @@ +/****************************************************************************** + * $Id: AKFS_Direction.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_Direction.h" +#include "AKFS_VNorm.h" +#include "AKFS_Math.h" + +/* + Coordinate system is right-handed. + X-Axis: from left to right. + Y-Axis: from bottom to top. + Z-Axis: from reverse to obverse. + + azimuth: Rotaion around Z axis, with positive values + when y-axis moves toward the x-axis. + pitch: Rotation around X axis, with positive values + when z-axis moves toward the y-axis. + roll: Rotation around Y axis, with positive values + when x-axis moves toward the z-axis. +*/ + +/* + 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); +} + +/* + This function is used internaly, so output is RADIAN! + */ +static void AKFS_Azimuth( + const AKFVEC* hvec, + const AKFLOAT pitch, /* radian */ + const AKFLOAT roll, /* radian */ + AKFLOAT* azimuth /* radian */ +) +{ + AKFLOAT sinP; /* sin value of pitch angle */ + AKFLOAT cosP; /* cos value of pitch angle */ + AKFLOAT sinR; /* sin value of roll angle */ + AKFLOAT cosR; /* cos value of roll angle */ + AKFLOAT Xh; /* X axis element of vector which is projected to horizontal plane */ + AKFLOAT Yh; /* Y axis element of vector which is projected to horizontal plane */ + + sinP = AKFS_SIN(pitch); + cosP = AKFS_COS(pitch); + sinR = AKFS_SIN(roll); + cosR = AKFS_COS(roll); + + Yh = -(hvec->u.x)*cosR + (hvec->u.z)*sinR; + Xh = (hvec->u.x)*sinP*sinR + (hvec->u.y)*cosP + (hvec->u.z)*sinP*cosR; + + /* atan2(y, x) -> divisor and dividend is opposite from mathematical equation. */ + *azimuth = AKFS_ATAN2(Yh, Xh); +} + +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; +} + + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Direction.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Direction.h new file mode 100644 index 0000000..a7d6fcc --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Direction.h @@ -0,0 +1,40 @@ +/****************************************************************************** + * $Id: AKFS_Direction.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_DIRECTION_H +#define AKFS_INC_DIRECTION_H + +#include "AKFS_Device.h" + +/***** Prototype of function **************************************************/ +AKLIB_C_API_START +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 +); +AKLIB_C_API_END + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Math.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Math.h new file mode 100644 index 0000000..0673c54 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_Math.h @@ -0,0 +1,48 @@ +/****************************************************************************** + * $Id: AKFS_Math.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_MATH_H +#define AKFS_INC_MATH_H + +#include <math.h> +#include "AKFS_Configure.h" + +/***** Constant definition ****************************************************/ +#define AKFS_PI 3.141592654f +#define RAD2DEG(rad) ((rad)*180.0f/AKFS_PI) + +/***** Macro definition *******************************************************/ + +#ifdef AKFS_PRECISION_DOUBLE +#define AKFS_SIN(x) sin(x) +#define AKFS_COS(x) cos(x) +#define AKFS_ASIN(x) asin(x) +#define AKFS_ACOS(x) acos(x) +#define AKFS_ATAN2(y, x) atan2((y), (x)) +#define AKFS_SQRT(x) sqrt(x) +#else +#define AKFS_SIN(x) sinf(x) +#define AKFS_COS(x) cosf(x) +#define AKFS_ASIN(x) asinf(x) +#define AKFS_ACOS(x) acosf(x) +#define AKFS_ATAN2(y, x) atan2f((y), (x)) +#define AKFS_SQRT(x) sqrtf(x) +#endif + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c new file mode 100644 index 0000000..7ae5bfa --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c @@ -0,0 +1,108 @@ +/****************************************************************************** + * $Id: AKFS_VNorm.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_VNorm.h" +#include "AKFS_Device.h" + +/*! + */ +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_VbAve( + const int16 nvec, /*!< Size of normalized vector buffer */ + const AKFVEC vvec[], /*!< Normalized vector buffer */ + const int16 nave, /*!< Number of averaeg */ + AKFVEC* vave /*!< Averaged vector */ +) +{ + int i; + + /* arguments check */ + if ((nave <= 0) || (nvec <= 0) || (nvec < nave)) { + return AKFS_ERROR; + } + + /* calculate average */ + vave->u.x = 0; + vave->u.y = 0; + vave->u.z = 0; + for (i=0; i<nave; i++) { + if ((vvec[i].u.x == AKFS_INIT_VALUE_F) || + (vvec[i].u.y == AKFS_INIT_VALUE_F) || + (vvec[i].u.z == AKFS_INIT_VALUE_F)) { + break; + } + vave->u.x += vvec[i].u.x; + vave->u.y += vvec[i].u.y; + vave->u.z += vvec[i].u.z; + } + if (i == 0) { + vave->u.x = 0; + vave->u.y = 0; + vave->u.z = 0; + } else { + vave->u.x /= i; + vave->u.y /= i; + vave->u.z /= i; + } + return AKFS_SUCCESS; +} + + diff --git a/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h new file mode 100644 index 0000000..1c79ad1 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h @@ -0,0 +1,47 @@ +/****************************************************************************** + * $Id: AKFS_VNorm.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_VNORM_H +#define AKFS_INC_VNORM_H + +#include "AKFS_Device.h" + +/***** Prototype of function **************************************************/ +AKLIB_C_API_START +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 */ +); + +int16 AKFS_VbAve( + const int16 nvec, /*!< Size of normalized vector buffer */ + const AKFVEC vvec[], /*!< Normalized vector buffer */ + const int16 nave, /*!< Number of averaeg */ + AKFVEC* vave /*!< Averaged vector */ +); + +AKLIB_C_API_END + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_CSpec.h b/AK8975_FS/akmdfs/AKFS_CSpec.h new file mode 100644 index 0000000..9acb68d --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_CSpec.h @@ -0,0 +1,39 @@ +/****************************************************************************** + * $Id: AKFS_CSpec.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_CSPEC_H +#define AKFS_INC_CSPEC_H + +/******************************************************************************* + User defines parameters. + ******************************************************************************/ +/* Parameters for Average */ +/* The number of magnetic/acceleration data to be averaged. */ +#define CSPEC_HNAVE_D 4 +#define CSPEC_ANAVE_D 4 +#define CSPEC_HNAVE_V 8 +#define CSPEC_ANAVE_V 8 + +#ifdef WIN32 +#define CSPEC_SETTING_FILE "akmdfs.txt" +#else +#define CSPEC_SETTING_FILE "/data/misc/akmdfs.txt" +#endif + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_Common.h b/AK8975_FS/akmdfs/AKFS_Common.h new file mode 100644 index 0000000..4b6874d --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_Common.h @@ -0,0 +1,153 @@ +/****************************************************************************** + * $Id: AKFS_Common.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_COMMON_H +#define AKFS_INC_COMMON_H + +#ifdef WIN32 +#ifndef _WIN32_WINNT +#define _WIN32_WINNT 0x0501 +#endif + +#include <windows.h> +#include <stdio.h> +#include <stdlib.h> +#include <conio.h> +#include <stdarg.h> +#include <crtdbg.h> +#include "Android.h" + +#define DBG_LEVEL DBG_LEVEL4 +#define ENABLE_AKMDEBUG 1 + +#else +#include <stdio.h> /* frpintf */ +#include <stdlib.h> /* atoi */ +#include <string.h> /* memset */ +#include <unistd.h> +#include <stdarg.h> /* va_list */ +#include <utils/Log.h> /* LOGV */ +#include <errno.h> /* errno */ + +#endif + +/*** Constant definition ******************************************************/ +#define AKM_TRUE 1 /*!< Represents true */ +#define AKM_FALSE 0 /*!< Represents false */ +#define AKM_SUCCESS 1 /*!< Represents success */ +#define AKM_FAIL 0 /*!< Represents fail */ + +#undef LOG_TAG +#define LOG_TAG "AKMD_FS" + +#define DBG_LEVEL0 0 /* Critical */ +#define DBG_LEVEL1 1 /* Notice */ +#define DBG_LEVEL2 2 /* Information */ +#define DBG_LEVEL3 3 /* Debug */ +#define DBG_LEVEL4 4 /* Verbose */ + +#ifndef DBG_LEVEL +#define DBG_LEVEL DBG_LEVEL0 +#endif + +#define DATA_AREA01 0x0001 +#define DATA_AREA02 0x0002 +#define DATA_AREA03 0x0004 +#define DATA_AREA04 0x0008 +#define DATA_AREA05 0x0010 +#define DATA_AREA06 0x0020 +#define DATA_AREA07 0x0040 +#define DATA_AREA08 0x0080 +#define DATA_AREA09 0x0100 +#define DATA_AREA10 0x0200 +#define DATA_AREA11 0x0400 +#define DATA_AREA12 0x0800 +#define DATA_AREA13 0x1000 +#define DATA_AREA14 0x2000 +#define DATA_AREA15 0x4000 +#define DATA_AREA16 0x8000 + + +/* Debug area definition */ +#define AKMDATA_DUMP DATA_AREA01 /*<! Dump data */ +#define AKMDATA_BDATA DATA_AREA02 /*<! BDATA */ +#define AKMDATA_MAG DATA_AREA03 /*<! Magnetic Field */ +#define AKMDATA_ACC DATA_AREA04 /*<! Accelerometer */ +#define AKMDATA_ORI DATA_AREA05 /*<! Orientation */ +#define AKMDATA_GETINTERVAL DATA_AREA06 +#define AKMDATA_LOOP DATA_AREA07 +#define AKMDATA_DRV DATA_AREA08 + +#ifndef ENABLE_AKMDEBUG +#define ENABLE_AKMDEBUG 0 /* Eanble debug output when it is 1. */ +#endif + +#define OPMODE_CONSOLE 0x01 +#define OPMODE_FST 0x02 + +/***** Debug Level Output *************************************/ +#if ENABLE_AKMDEBUG +#define AKMDEBUG(level, format, ...) \ + (((level) <= DBG_LEVEL) \ + ? (fprintf(stdout, (format), ##__VA_ARGS__)) \ + : ((void)0)) +#else +#define AKMDEBUG(level, format, ...) +#endif + +/***** Dbg Zone Output ***************************************/ +#if ENABLE_AKMDEBUG +#define AKMDATA(flag, format, ...) \ + ((((int)flag) & g_dbgzone) \ + ? (fprintf(stdout, (format), ##__VA_ARGS__)) \ + : ((void)0)) +#else +#define AKMDATA(flag, format, ...) +#endif + +/***** Log output ********************************************/ +#ifdef AKM_LOG_ENABLE +#define AKM_LOG(format, ...) ALOGD((format), ##__VA_ARGS__) +#else +#define AKM_LOG(format, ...) +#endif + +/***** Error output *******************************************/ +#define AKMERROR \ + ((g_opmode & OPMODE_CONSOLE) \ + ? (fprintf(stderr, "%s:%d Error.\n", __FUNCTION__, __LINE__)) \ + : (ALOGE("%s:%d Error.", __FUNCTION__, __LINE__))) + +#define AKMERROR_STR(api) \ + ((g_opmode & OPMODE_CONSOLE) \ + ? (fprintf(stderr, "%s:%d %s Error (%s).\n", \ + __FUNCTION__, __LINE__, (api), strerror(errno))) \ + : (ALOGE("%s:%d %s Error (%s).", \ + __FUNCTION__, __LINE__, (api), strerror(errno)))) + +/*** Type declaration *********************************************************/ + +/*** Global variables *********************************************************/ +extern int g_stopRequest; /*!< 0:Not stop, 1:Stop */ +extern int g_opmode; /*!< 0:Daemon mode, 1:Console mode. */ +extern int g_dbgzone; /*!< Debug zone. */ + +/*** Prototype of function ****************************************************/ + +#endif /* AKMD_INC_AKCOMMON_H */ + diff --git a/AK8975_FS/akmdfs/AKFS_Compass.h b/AK8975_FS/akmdfs/AKFS_Compass.h new file mode 100644 index 0000000..98bbf28 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_Compass.h @@ -0,0 +1,91 @@ +/****************************************************************************** + * $Id: AKFS_Compass.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_COMPASS_H +#define AKFS_INC_COMPASS_H + +#include "AKFS_Common.h" +#include "AKFS_CSpec.h" + +#ifdef WIN32 +#include "AK8975_LinuxDriver.h" +#else +#include "AK8975Driver.h" +#endif + +/****************************************/ +/* Include files for AK8975 library. */ +/****************************************/ +#include "AKFS_AK8975.h" +#include "AKFS_Configure.h" +#include "AKFS_AOC.h" +#include "AKFS_Device.h" +#include "AKFS_Direction.h" +#include "AKFS_Math.h" +#include "AKFS_VNorm.h" + +/*** Constant definition ******************************************************/ + +/*** Type declaration *********************************************************/ +typedef struct _AKSENSOR_DATA{ + AKFLOAT x; + AKFLOAT y; + AKFLOAT z; + int8 status; +} AKSENSOR_DATA; + +/*! A parameter structure. */ +typedef struct _AK8975PRMS{ + /* Variables for Decomp8975. */ + AKFVEC mfv_hdata[AKFS_HDATA_SIZE]; + uint8vec mi_asa; + uint8 mi_st; + + /* Variables forAOC. */ + AKFS_AOC_VAR m_aocv; + + /* Variables for Magnetometer buffer. */ + AKFVEC mfv_hvbuf[AKFS_HDATA_SIZE]; + AKFVEC mfv_ho; + AKFVEC mfv_hs; + AKFS_PATNO m_hpat; + + /* Variables for Accelerometer buffer. */ + AKFVEC mfv_adata[AKFS_ADATA_SIZE]; + AKFVEC mfv_avbuf[AKFS_ADATA_SIZE]; + AKFVEC mfv_ao; + AKFVEC mfv_as; + + /* Variables for Direction. */ + int16 mi_hnaveD; + int16 mi_anaveD; + AKFLOAT mf_azimuth; + AKFLOAT mf_pitch; + AKFLOAT mf_roll; + + /* Variables for vector output */ + int16 mi_hnaveV; + int16 mi_anaveV; + AKFVEC mfv_hvec; + AKFVEC mfv_avec; + int16 mi_hstatus; + +} AK8975PRMS; + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_Disp.c b/AK8975_FS/akmdfs/AKFS_Disp.c new file mode 100644 index 0000000..d22cffb --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_Disp.c @@ -0,0 +1,90 @@ +/****************************************************************************** + * $Id: AKFS_Disp.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_Disp.h" +#include "AKFS_Common.h" + +/*! + Print startup message to Android Log daemon. + */ +void Disp_StartMessage(void) +{ + ALOGI("AK8975 Daemon for Open Source v20120329."); + ALOGI("Debug: %s", ((ENABLE_AKMDEBUG)?("ON"):("OFF"))); + ALOGI("Debug level: %d", DBG_LEVEL); +} + +/*! + Print ending message to Android Log daemon. + */ +void Disp_EndMessage(int ret) +{ + ALOGI("AK8975 for Android end (%d).", ret); +} + +/*! + Print result + */ +void Disp_Result(int buf[YPR_DATA_SIZE]) +{ + AKMDEBUG(DBG_LEVEL1, + "Flag=%d\n", buf[0]); + AKMDEBUG(DBG_LEVEL1, + "Acc(%d):%8.2f, %8.2f, %8.2f\n", + buf[4], REVERT_ACC(buf[1]), REVERT_ACC(buf[2]), REVERT_ACC(buf[3])); + AKMDEBUG(DBG_LEVEL1, + "Mag(%d):%8.2f, %8.2f, %8.2f\n", + buf[8], REVERT_MAG(buf[5]), REVERT_MAG(buf[6]), REVERT_MAG(buf[7])); + AKMDEBUG(DBG_LEVEL1, + "Ori(%d)=%8.2f, %8.2f, %8.2f\n", + buf[8], REVERT_ORI(buf[9]), REVERT_ORI(buf[10]), REVERT_ORI(buf[11])); +} + +/*! + Output main menu to stdout and wait for user input from stdin. + @return Selected mode. + */ +MODE Menu_Main(void) +{ + char msg[20]; + memset(msg, 0, sizeof(msg)); + + AKMDEBUG(DBG_LEVEL1, + " -------------------- AK8975 Console Application -------------------- \n" + " 1. Start measurement. \n" + " 2. Self-test. \n" + " Q. Quit application. \n" + " --------------------------------------------------------------------- \n" + " Please select a number.\n" + " ---> "); + fgets(msg, 10, stdin); + AKMDEBUG(DBG_LEVEL1, "\n"); + + /* BUG : If 2-digits number is input, */ + /* only the first character is compared. */ + if (!strncmp(msg, "1", 1)) { + return MODE_Measure; + } else if (!strncmp(msg, "2", 1)) { + return MODE_SelfTest; + } else if (strncmp(msg, "Q", 1) == 0 || strncmp(msg, "q", 1) == 0) { + return MODE_Quit; + } else { + return MODE_ERROR; + } +} + diff --git a/AK8975_FS/akmdfs/AKFS_Disp.h b/AK8975_FS/akmdfs/AKFS_Disp.h new file mode 100644 index 0000000..f33fd8d --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_Disp.h @@ -0,0 +1,53 @@ +/****************************************************************************** + * $Id: AKFS_Disp.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_DISP_H +#define AKFS_INC_DISP_H + +/* Include file for AK8975 library. */ +#include "AKFS_Compass.h" + +/*** Constant definition ******************************************************/ +#define REVERT_ACC(a) ((float)((a) * 9.8f / 720.0f)) +#define REVERT_MAG(m) ((float)((m) * 0.06f)) +#define REVERT_ORI(o) ((float)((o) / 64.0f)) + +/*** Type declaration *********************************************************/ + +/*! These defined types represents the current mode. */ +typedef enum _MODE { + MODE_ERROR, /*!< Error */ + MODE_Measure, /*!< Measurement */ + MODE_SelfTest, /*!< Self-test */ + MODE_Quit /*!< Quit */ +} MODE; + +/*** Prototype of function ****************************************************/ +/* + Disp_ : Display messages. + Menu_ : Display menu (two or more selection) and wait for user input. + */ + +void Disp_StartMessage(void); +void Disp_EndMessage(int ret); +void Disp_Result(int buf[YPR_DATA_SIZE]); + +MODE Menu_Main(void); + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_FileIO.c b/AK8975_FS/akmdfs/AKFS_FileIO.c new file mode 100644 index 0000000..cb69628 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_FileIO.c @@ -0,0 +1,131 @@ +/****************************************************************************** + * $Id: AKFS_FileIO.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_FileIO.h" + +/*** Constant definition ******************************************************/ +#ifdef AKFS_PRECISION_DOUBLE +#define AKFS_SCANF_FORMAT "%63s = %lf" +#else +#define AKFS_SCANF_FORMAT "%63s = %f" +#endif +#define AKFS_PRINTF_FORMAT "%s = %f\n" +#define LOAD_BUF_SIZE 64 + +/*! + Load parameters from file which is specified with #path. This function reads + data from a beginning of the file line by line, and check parameter name + sequentially. In otherword, this function depends on the order of eache + parameter described in the file. + @return If function fails, the return value is #AKM_FAIL. When function fails, + the output is undefined. Therefore, parameters which are possibly overwritten + by this function should be initialized again. If function succeeds, the + return value is #AKM_SUCCESS. + @param[out] prms A pointer to #AK8975PRMS structure. Loaded parameter is + stored to the member of this structure. + @param[in] path A path to the setting file. + */ +int16 AKFS_LoadParameters(AK8975PRMS * prms, const char* path) +{ + int16 ret; + char buf[LOAD_BUF_SIZE]; + FILE *fp = NULL; + + /* Open setting file for read. */ + if ((fp = fopen(path, "r")) == NULL) { + AKMERROR_STR("fopen"); + return AKM_FAIL; + } + + ret = 1; + + /* Load data to HO */ + if (fscanf(fp, AKFS_SCANF_FORMAT, buf, &prms->mfv_ho.u.x) != 2) { + ret = 0; + } else { + if (strncmp(buf, "HO.x", sizeof(buf)) != 0) { + ret = 0; + } + } + if (fscanf(fp, AKFS_SCANF_FORMAT, buf, &prms->mfv_ho.u.y) != 2) { + ret = 0; + } else { + if (strncmp(buf, "HO.y", sizeof(buf)) != 0) { + ret = 0; + } + } + if (fscanf(fp, AKFS_SCANF_FORMAT, buf, &prms->mfv_ho.u.z) != 2) { + ret = 0; + } else { + if (strncmp(buf, "HO.z", sizeof(buf)) != 0) { + ret = 0; + } + } + + if (fclose(fp) != 0) { + AKMERROR_STR("fclose"); + ret = 0; + } + + if (ret == 0) { + AKMERROR; + return AKM_FAIL; + } + + return AKM_SUCCESS; +} + +/*! + Save parameters to file which is specified with #path. This function saves + variables when the offsets of magnetic sensor estimated successfully. + @return If function fails, the return value is #AKM_FAIL. When function fails, + the parameter file may collapsed. Therefore, the parameters file should be + discarded. If function succeeds, the return value is #AKM_SUCCESS. + @param[out] prms A pointer to #AK8975PRMS structure. Member variables are + saved to the parameter file. + @param[in] path A path to the setting file. + */ +int16 AKFS_SaveParameters(AK8975PRMS *prms, const char* path) +{ + int16 ret = 1; + FILE *fp; + + /*Open setting file for write. */ + if ((fp = fopen(path, "w")) == NULL) { + AKMERROR_STR("fopen"); + return AKM_FAIL; + } + + /* Save data to HO */ + if (fprintf(fp, AKFS_PRINTF_FORMAT, "HO.x", prms->mfv_ho.u.x) < 0) { ret = 0; } + if (fprintf(fp, AKFS_PRINTF_FORMAT, "HO.y", prms->mfv_ho.u.y) < 0) { ret = 0; } + if (fprintf(fp, AKFS_PRINTF_FORMAT, "HO.z", prms->mfv_ho.u.z) < 0) { ret = 0; } + + if (fclose(fp) != 0) { + AKMERROR_STR("fclose"); + ret = 0; + } + + if (ret == 0) { + AKMERROR; + return AKM_FAIL; + } + + return AKM_SUCCESS; +} + diff --git a/AK8975_FS/akmdfs/AKFS_FileIO.h b/AK8975_FS/akmdfs/AKFS_FileIO.h new file mode 100644 index 0000000..fff5533 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_FileIO.h @@ -0,0 +1,40 @@ +/****************************************************************************** + * $Id: AKFS_FileIO.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_FILEIO_H +#define AKFS_INC_FILEIO_H + +/* Common include files. */ +#include "AKFS_Common.h" + +/* Include file for AK8975 library. */ +#include "AKFS_Compass.h" + +/*** Constant definition ******************************************************/ + +/*** Type declaration *********************************************************/ + +/*** Global variables *********************************************************/ + +/*** Prototype of function ****************************************************/ +int16 AKFS_LoadParameters(AK8975PRMS *prms, const char* path); + +int16 AKFS_SaveParameters(AK8975PRMS* prms, const char* path); + +#endif + diff --git a/AK8975_FS/akmdfs/AKFS_Measure.c b/AK8975_FS/akmdfs/AKFS_Measure.c new file mode 100644 index 0000000..849c921 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_Measure.c @@ -0,0 +1,411 @@ +/****************************************************************************** + * $Id: AKFS_Measure.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifdef WIN32 +#include "AK8975_LinuxDriver.h" +#else +#include "AK8975Driver.h" +#endif + +#include "AKFS_Measure.h" +#include "AKFS_Disp.h" +#include "AKFS_APIs.h" + +/*! + Read sensitivity adjustment data from fuse ROM. + @return If data are read successfully, the return value is #AKM_SUCCESS. + Otherwise the return value is #AKM_FAIL. + @param[out] regs The read ASA values. When this function succeeds, ASAX value + is saved in regs[0], ASAY is saved in regs[1], ASAZ is saved in regs[2]. + */ +int16 AKFS_ReadAK8975FUSEROM( + uint8 regs[3] +) +{ + /* Set to FUSE ROM access mode */ + if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* Read values. ASAX, ASAY, ASAZ */ + if (AKD_RxData(AK8975_FUSE_ASAX, regs, 3) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* Set to PowerDown mode */ + if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + AKMDEBUG(DBG_LEVEL2, "%s: asa(dec)=%d,%d,%d\n", + __FUNCTION__, regs[0], regs[1], regs[2]); + + return AKM_SUCCESS; +} + +/*! + Carry out self-test. + @return If this function succeeds, the return value is #AKM_SUCCESS. + Otherwise the return value is #AKM_FAIL. + */ +int16 AKFS_SelfTest(void) +{ + BYTE i2cData[SENSOR_DATA_SIZE]; + BYTE asa[3]; + AKFLOAT hdata[3]; + int16 ret; + + /* Set to FUSE ROM access mode */ + if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* Read values from ASAX to ASAZ */ + if (AKD_RxData(AK8975_FUSE_ASAX, asa, 3) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* Set to PowerDown mode */ + if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* Set to self-test mode */ + i2cData[0] = 0x40; + if (AKD_TxData(AK8975_REG_ASTC, i2cData, 1) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* Set to Self-test mode */ + if (AKD_SetMode(AK8975_MODE_SELF_TEST) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + /* + Wait for DRDY pin changes to HIGH. + Get measurement data from AK8975 + */ + if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) { + AKMERROR; + return AKM_FAIL; + } + + hdata[0] = AK8975_HDATA_CONVERTER(i2cData[2], i2cData[1], asa[0]); + hdata[1] = AK8975_HDATA_CONVERTER(i2cData[4], i2cData[3], asa[1]); + hdata[2] = AK8975_HDATA_CONVERTER(i2cData[6], i2cData[5], asa[2]); + + /* Test */ + ret = 1; + if ((hdata[0] < AK8975_SELFTEST_MIN_X) || + (AK8975_SELFTEST_MAX_X < hdata[0])) { + ret = 0; + } + if ((hdata[1] < AK8975_SELFTEST_MIN_Y) || + (AK8975_SELFTEST_MAX_Y < hdata[1])) { + ret = 0; + } + if ((hdata[2] < AK8975_SELFTEST_MIN_Z) || + (AK8975_SELFTEST_MAX_Z < hdata[2])) { + ret = 0; + } + + AKMDEBUG(DBG_LEVEL2, "Test(%s):%8.2f, %8.2f, %8.2f\n", + (ret ? "Success" : "fail"), hdata[0], hdata[1], hdata[2]); + + if (ret) { + return AKM_SUCCESS; + } else { + return AKM_FAIL; + } +} + +/*! + This function calculate the duration of sleep for maintaining + the loop keep the period. + This function calculates "minimum - (end - start)". + @return The result of above equation in nanosecond. + @param end The time of after execution. + @param start The time of before execution. + @param minimum Loop period of each execution. + */ +struct timespec AKFS_CalcSleep( + const struct timespec* end, + const struct timespec* start, + const int64_t minimum +) +{ + int64_t endL; + int64_t startL; + int64_t diff; + + struct timespec ret; + + endL = (end->tv_sec * 1000000000) + end->tv_nsec; + startL = (start->tv_sec * 1000000000) + start->tv_nsec; + diff = minimum; + + diff -= (endL - startL); + + /* Don't allow negative value */ + if (diff < 0) { + diff = 0; + } + + /* Convert to timespec */ + if (diff > 1000000000) { + ret.tv_sec = diff / 1000000000; + ret.tv_nsec = diff % 1000000000; + } else { + ret.tv_sec = 0; + ret.tv_nsec = diff; + } + return ret; +} + +/*! + Get interval of each sensors from device driver. + @return If this function succeeds, the return value is #AKM_SUCCESS. + Otherwise the return value is #AKM_FAIL. + @param flag This variable indicates what sensor frequency is updated. + @param minimum This value show the minimum loop period in all sensors. + */ +int16 AKFS_GetInterval( + uint16* flag, + int64_t* minimum +) +{ + /* Accelerometer, Magnetometer, Orientation */ + /* Delay is in nano second unit. */ + /* Negative value means the sensor is disabled.*/ + int64_t delay[AKM_NUM_SENSORS]; + int i; + + if (AKD_GetDelay(delay) < 0) { + AKMERROR; + return AKM_FAIL; + } + AKMDATA(AKMDATA_GETINTERVAL,"delay[A,M,O]=%lld,%lld,%lld\n", + delay[0], delay[1], delay[2]); + + /* update */ + *minimum = 1000000000; + *flag = 0; + for (i=0; i<AKM_NUM_SENSORS; i++) { + /* Set flag */ + if (delay[i] >= 0) { + *flag |= 1 << i; + if (*minimum > delay[i]) { + *minimum = delay[i]; + } + } + } + return AKM_SUCCESS; +} + +/*! + If this program run as console mode, measurement result will be displayed + on console terminal. + @return If this function succeeds, the return value is #AKM_SUCCESS. + Otherwise the return value is #AKM_FAIL. + */ +void AKFS_OutputResult( + const uint16 flag, + const AKSENSOR_DATA* acc, + const AKSENSOR_DATA* mag, + const AKSENSOR_DATA* ori +) +{ + int buf[YPR_DATA_SIZE]; + + /* Store to buffer */ + buf[0] = flag; /* Data flag */ + buf[1] = CONVERT_ACC(acc->x); /* Ax */ + buf[2] = CONVERT_ACC(acc->y); /* Ay */ + buf[3] = CONVERT_ACC(acc->z); /* Az */ + buf[4] = acc->status; /* Acc status */ + buf[5] = CONVERT_MAG(mag->x); /* Mx */ + buf[6] = CONVERT_MAG(mag->y); /* My */ + buf[7] = CONVERT_MAG(mag->z); /* Mz */ + buf[8] = mag->status; /* Mag status */ + buf[9] = CONVERT_ORI(ori->x); /* yaw */ + buf[10] = CONVERT_ORI(ori->y); /* pitch */ + buf[11] = CONVERT_ORI(ori->z); /* roll */ + + if (g_opmode & OPMODE_CONSOLE) { + /* Console mode */ + Disp_Result(buf); + } + + /* Set result to driver */ + AKD_SetYPR(buf); +} + +/*! + This is the main routine of measurement. + */ +void AKFS_MeasureLoop(void) +{ + BYTE i2cData[SENSOR_DATA_SIZE]; /* ST1 ~ ST2 */ + int16 mag[3]; + int16 mstat; + int16 acc[3]; + struct timespec tsstart= {0, 0}; + struct timespec tsend = {0, 0}; + struct timespec doze; + int64_t minimum; + uint16 flag; + AKSENSOR_DATA sv_acc; + AKSENSOR_DATA sv_mag; + AKSENSOR_DATA sv_ori; + AKFLOAT tmpx, tmpy, tmpz; + int16 tmp_accuracy; + + minimum = -1; + +#ifdef WIN32 + clock_init_time(); +#endif + + /* Initialize library functions and device */ + if (AKFS_Start(CSPEC_SETTING_FILE) != AKM_SUCCESS) { + AKMERROR; + goto MEASURE_END; + } + + while (g_stopRequest != AKM_TRUE) { + /* Beginning time */ + if (clock_gettime(CLOCK_MONOTONIC, &tsstart) < 0) { + AKMERROR; + goto MEASURE_END; + } + + /* Get interval */ + if (AKFS_GetInterval(&flag, &minimum) != AKM_SUCCESS) { + AKMERROR; + goto MEASURE_END; + } + + if ((flag & ACC_DATA_READY) || (flag & ORI_DATA_READY)) { + /* Get accelerometer */ + if (AKD_GetAccelerationData(acc) != AKD_SUCCESS) { + AKMERROR; + goto MEASURE_END; + } + + /* Calculate accelerometer vector */ + if (AKFS_Get_ACCELEROMETER(acc, 0, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) { + sv_acc.x = tmpx; + sv_acc.y = tmpy; + sv_acc.z = tmpz; + sv_acc.status = tmp_accuracy; + } else { + flag &= ~ACC_DATA_READY; + flag &= ~ORI_DATA_READY; + } + } + + if ((flag & MAG_DATA_READY) || (flag & ORI_DATA_READY)) { + /* Set to measurement mode */ + if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) { + AKMERROR; + goto MEASURE_END; + } + + /* Wait for DRDY and get data from device */ + if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) { + AKMERROR; + goto MEASURE_END; + } + /* raw data to x,y,z value */ + mag[0] = (int)((int16_t)(i2cData[2]<<8)+((int16_t)i2cData[1])); + mag[1] = (int)((int16_t)(i2cData[4]<<8)+((int16_t)i2cData[3])); + mag[2] = (int)((int16_t)(i2cData[6]<<8)+((int16_t)i2cData[5])); + mstat = i2cData[0] | i2cData[7]; + + AKMDATA(AKMDATA_BDATA, + "bData=%02X,%02X,%02X,%02X,%02X,%02X,%02X,%02X\n", + i2cData[0], i2cData[1], i2cData[2], i2cData[3], + i2cData[4], i2cData[5], i2cData[6], i2cData[7]); + + /* Calculate magnetic field vector */ + if (AKFS_Get_MAGNETIC_FIELD(mag, mstat, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) { + sv_mag.x = tmpx; + sv_mag.y = tmpy; + sv_mag.z = tmpz; + sv_mag.status = tmp_accuracy; + } else { + flag &= ~MAG_DATA_READY; + flag &= ~ORI_DATA_READY; + } + } + + if (flag & ORI_DATA_READY) { + if (AKFS_Get_ORIENTATION(&tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) { + sv_ori.x = tmpx; + sv_ori.y = tmpy; + sv_ori.z = tmpz; + sv_ori.status = tmp_accuracy; + } else { + flag &= ~ORI_DATA_READY; + } + } + + /* Output result */ + AKFS_OutputResult(flag, &sv_acc, &sv_mag, &sv_ori); + + /* Ending time */ + if (clock_gettime(CLOCK_MONOTONIC, &tsend) < 0) { + AKMERROR; + goto MEASURE_END; + } + + /* Calculate duration */ + doze = AKFS_CalcSleep(&tsend, &tsstart, minimum); + AKMDATA(AKMDATA_LOOP, "Sleep: %6.2f msec\n", (doze.tv_nsec/1000000.0f)); + nanosleep(&doze, NULL); + +#ifdef WIN32 + if (_kbhit()) { + _getch(); + break; + } +#endif + } + +MEASURE_END: + /* Set to PowerDown mode */ + if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) { + AKMERROR; + return; + } + + /* Save parameters */ + if (AKFS_Stop(CSPEC_SETTING_FILE) != AKM_SUCCESS) { + AKMERROR; + } +} + + diff --git a/AK8975_FS/akmdfs/AKFS_Measure.h b/AK8975_FS/akmdfs/AKFS_Measure.h new file mode 100644 index 0000000..c41e363 --- /dev/null +++ b/AK8975_FS/akmdfs/AKFS_Measure.h @@ -0,0 +1,71 @@ +/****************************************************************************** + * $Id: AKFS_Measure.h 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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. + */ +#ifndef AKFS_INC_MEASURE_H +#define AKFS_INC_MEASURE_H + +/* Include files for AK8975 library. */ +#include "AKFS_Compass.h" + +/*** Constant definition ******************************************************/ +#define AK8975_SELFTEST_MIN_X -100 +#define AK8975_SELFTEST_MAX_X 100 + +#define AK8975_SELFTEST_MIN_Y -100 +#define AK8975_SELFTEST_MAX_Y 100 + +#define AK8975_SELFTEST_MIN_Z -1000 +#define AK8975_SELFTEST_MAX_Z -300 + +#define CONVERT_ACC(a) ((int)((a) * 720 / 9.8f)) +#define CONVERT_MAG(m) ((int)((m) / 0.06f)) +#define CONVERT_ORI(o) ((int)((o) * 64)) + +/*** Type declaration *********************************************************/ + +/*** Global variables *********************************************************/ + +/*** Prototype of function ****************************************************/ +int16 AKFS_ReadAK8975FUSEROM( + uint8 regs[3] +); + +int16 AKFS_SelfTest(void); + +struct timespec AKFS_CalcSleep( + const struct timespec* end, + const struct timespec* start, + const int64_t minimum +); + +int16 AKFS_GetInterval( + uint16* flag, + int64_t* minimum +); + +void AKFS_OutputResult( + const uint16 flag, + const AKSENSOR_DATA* acc, + const AKSENSOR_DATA* mag, + const AKSENSOR_DATA* ori +); + +void AKFS_MeasureLoop(void); + +#endif + diff --git a/AK8975_FS/akmdfs/Android.mk b/AK8975_FS/akmdfs/Android.mk new file mode 100644 index 0000000..8c39be6 --- /dev/null +++ b/AK8975_FS/akmdfs/Android.mk @@ -0,0 +1,42 @@ +ifneq ($(TARGET_SIMULATOR),true) + +LOCAL_PATH:= $(call my-dir) + +# dmtd +AKM_FS_LIB=AKFS_APIs_8975 + +##### AKM daemon ############################################################### +include $(CLEAR_VARS) + +LOCAL_C_INCLUDES := \ + $(KERNEL_HEADERS) \ + $(LOCAL_PATH)/$(AKM_FS_LIB) + +LOCAL_SRC_FILES:= \ + $(AKM_FS_LIB)/AKFS_AK8975.c \ + $(AKM_FS_LIB)/AKFS_AOC.c \ + $(AKM_FS_LIB)/AKFS_Device.c \ + $(AKM_FS_LIB)/AKFS_Direction.c \ + $(AKM_FS_LIB)/AKFS_VNorm.c \ + AK8975Driver.c \ + AKFS_APIs.c \ + AKFS_Disp.c \ + AKFS_FileIO.c \ + AKFS_Measure.c \ + main.c + +LOCAL_CFLAGS += \ + -Wall \ + -DENABLE_AKMDEBUG=1 \ + -DOUTPUT_STDOUT=1 \ + -DDBG_LEVEL=2 \ + +LOCAL_MODULE := akmdfs +LOCAL_MODULE_TAGS := optional +LOCAL_FORCE_STATIC_EXECUTABLE := false +LOCAL_SHARED_LIBRARIES := libc libm libutils libcutils +include $(BUILD_EXECUTABLE) + + +endif # TARGET_SIMULATOR != true + diff --git a/AK8975_FS/akmdfs/main.c b/AK8975_FS/akmdfs/main.c new file mode 100644 index 0000000..6db52e4 --- /dev/null +++ b/AK8975_FS/akmdfs/main.c @@ -0,0 +1,294 @@ +/****************************************************************************** + * $Id: main.c 580 2012-03-29 09:56:21Z yamada.rj $ + ****************************************************************************** + * + * 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_Compass.h" +#include "AKFS_Disp.h" +#include "AKFS_FileIO.h" +#include "AKFS_Measure.h" +#include "AKFS_APIs.h" + +#ifndef WIN32 +#include <sched.h> +#include <pthread.h> +#include <linux/input.h> +#endif + +#define ERROR_INITDEVICE (-1) +#define ERROR_OPTPARSE (-2) +#define ERROR_SELF_TEST (-3) +#define ERROR_READ_FUSE (-4) +#define ERROR_INIT (-5) +#define ERROR_GETOPEN_STAT (-6) +#define ERROR_STARTCLONE (-7) +#define ERROR_GETCLOSE_STAT (-8) + +/* Global variable. See AKFS_Common.h file. */ +int g_stopRequest = 0; +int g_opmode = 0; +int g_dbgzone = 0; +int g_mainQuit = AKD_FALSE; + +/* Static variable. */ +static pthread_t s_thread; /*!< Thread handle */ + +/*! + A thread function which is raised when measurement is started. + @param[in] args This parameter is not used currently. + */ +static void* thread_main(void* args) +{ + AKFS_MeasureLoop(); + return ((void*)0); +} + +/*! + Signal handler. This should be used only in DEBUG mode. + @param[in] sig Event + */ +static void signal_handler(int sig) +{ + if (sig == SIGINT) { + ALOGE("SIGINT signal"); + g_stopRequest = 1; + g_mainQuit = AKD_TRUE; + } +} + +/*! + Starts new thread. + @return If this function succeeds, the return value is 1. Otherwise, + the return value is 0. + */ +static int startClone(void) +{ + pthread_attr_t attr; + + pthread_attr_init(&attr); + g_stopRequest = 0; + if (pthread_create(&s_thread, &attr, thread_main, NULL) == 0) { + return 1; + } else { + return 0; + } +} + +/*! + This function parse the option. + @retval 1 Parse succeeds. + @retval 0 Parse failed. + @param[in] argc Argument count + @param[in] argv Argument vector + @param[out] layout_patno + */ +int OptParse( + int argc, + char* argv[], + AKFS_PATNO* layout_patno) +{ +#ifdef WIN32 + /* Static */ +#if defined(AKFS_WIN32_PAT1) + *layout_patno = PAT1; +#elif defined(AKFS_WIN32_PAT2) + *layout_patno = PAT2; +#elif defined(AKFS_WIN32_PAT3) + *layout_patno = PAT3; +#elif defined(AKFS_WIN32_PAT4) + *layout_patno = PAT4; +#elif defined(AKFS_WIN32_PAT5) + *layout_patno = PAT5; +#else + *layout_patno = PAT1; +#endif + g_opmode = OPMODE_CONSOLE; + /*g_opmode = 0;*/ + g_dbgzone = AKMDATA_LOOP | AKMDATA_TEST; +#else + int opt; + char optVal; + + *layout_patno = PAT_INVALID; + + while ((opt = getopt(argc, argv, "sm:z:")) != -1) { + switch(opt){ + case 'm': + optVal = (char)(optarg[0] - '0'); + if ((PAT1 <= optVal) && (optVal <= PAT8)) { + *layout_patno = (AKFS_PATNO)optVal; + AKMDEBUG(DBG_LEVEL2, "%s: Layout=%d\n", __FUNCTION__, optVal); + } + break; + case 's': + g_opmode |= OPMODE_CONSOLE; + break; + case 'z': + /* If error detected, hopefully 0 is returned. */ + errno = 0; + g_dbgzone = (int)strtol(optarg, (char**)NULL, 0); + AKMDEBUG(DBG_LEVEL2, "%s: Dbg Zone=%d\n", __FUNCTION__, g_dbgzone); + break; + default: + ALOGE("%s: Invalid argument", argv[0]); + return 0; + } + } + + /* If layout is not specified with argument, get parameter from driver */ + if (*layout_patno == PAT_INVALID) { + int16_t n; + if (AKD_GetLayout(&n) == AKM_SUCCESS) { + if ((PAT1 <= n) && (n <= PAT8)) { + *layout_patno = (AKFS_PATNO)n; + } + } + } + /* Error */ + if (*layout_patno == PAT_INVALID) { + ALOGE("No layout is specified."); + return 0; + } +#endif + + return 1; +} + +void ConsoleMode(void) +{ + /*** Console Mode *********************************************/ + while (AKD_TRUE) { + /* Select operation */ + switch (Menu_Main()) { + case MODE_SelfTest: + AKFS_SelfTest(); + break; + case MODE_Measure: + /* Reset flag */ + g_stopRequest = 0; + /* Measurement routine */ + AKFS_MeasureLoop(); + break; + + case MODE_Quit: + return; + + default: + AKMDEBUG(DBG_LEVEL0, "Unknown operation mode.\n"); + break; + } + } +} + +int main(int argc, char **argv) +{ + int retValue = 0; + AKFS_PATNO pat; + uint8 regs[3]; + + /* Show the version info of this software. */ + Disp_StartMessage(); + +#if ENABLE_AKMDEBUG + /* Register signal handler */ + signal(SIGINT, signal_handler); +#endif + + /* Open device driver */ + if(AKD_InitDevice() != AKD_SUCCESS) { + retValue = ERROR_INITDEVICE; + goto MAIN_QUIT; + } + + /* Parse command-line options */ + /* This function calls device driver function to get layout */ + if (OptParse(argc, argv, &pat) == 0) { + retValue = ERROR_OPTPARSE; + goto MAIN_QUIT; + } + + /* Self Test */ + if (g_opmode & OPMODE_FST){ + if (AKFS_SelfTest() != AKD_SUCCESS) { + retValue = ERROR_SELF_TEST; + goto MAIN_QUIT; + } + } + + /* OK, then start */ + if (AKFS_ReadAK8975FUSEROM(regs) != AKM_SUCCESS) { + retValue = ERROR_READ_FUSE; + goto MAIN_QUIT; + } + + /* Initialize library. */ + if (AKFS_Init(pat, regs) != AKM_SUCCESS) { + retValue = ERROR_INIT; + goto MAIN_QUIT; + } + + /* Start console mode */ + if (g_opmode & OPMODE_CONSOLE) { + ConsoleMode(); + goto MAIN_QUIT; + } + + /*** Start Daemon ********************************************/ + while (g_mainQuit == AKD_FALSE) { + int st = 0; + /* Wait until device driver is opened. */ + if (AKD_GetOpenStatus(&st) != AKD_SUCCESS) { + retValue = ERROR_GETOPEN_STAT; + goto MAIN_QUIT; + } + if (st == 0) { + ALOGI("Suspended."); + } else { + ALOGI("Compass Opened."); + /* Reset flag */ + g_stopRequest = 0; + /* Start measurement thread. */ + if (startClone() == 0) { + retValue = ERROR_STARTCLONE; + goto MAIN_QUIT; + } + + /* Wait until device driver is closed. */ + if (AKD_GetCloseStatus(&st) != AKD_SUCCESS) { + retValue = ERROR_GETCLOSE_STAT; + g_mainQuit = AKD_TRUE; + } + /* Wait thread completion. */ + g_stopRequest = 1; + pthread_join(s_thread, NULL); + ALOGI("Compass Closed."); + } + } + +MAIN_QUIT: + + /* Release library */ + AKFS_Release(); + /* Close device driver. */ + AKD_DeinitDevice(); + /* Show the last message. */ + Disp_EndMessage(retValue); + + return retValue; +} + + diff --git a/AK8975_FS/akmdfs/platform/include/linux/akm8975.h b/AK8975_FS/akmdfs/platform/include/linux/akm8975.h new file mode 100644 index 0000000..27cf22e --- /dev/null +++ b/AK8975_FS/akmdfs/platform/include/linux/akm8975.h @@ -0,0 +1,90 @@ +/* + * Definitions for akm8975 compass chip. + */ +#ifndef AKM8975_H +#define AKM8975_H + +#include <linux/ioctl.h> + +#define AKM8975_I2C_NAME "akm8975" + +#define SENSOR_DATA_SIZE 8 +#define YPR_DATA_SIZE 12 +#define RWBUF_SIZE 16 + +#define ACC_DATA_FLAG 0 +#define MAG_DATA_FLAG 1 +#define ORI_DATA_FLAG 2 +#define AKM_NUM_SENSORS 3 + +#define ACC_DATA_READY (1<<(ACC_DATA_FLAG)) +#define MAG_DATA_READY (1<<(MAG_DATA_FLAG)) +#define ORI_DATA_READY (1<<(ORI_DATA_FLAG)) + +/*! \name AK8975 constant definition + \anchor AK8975_Def + Constant definitions of the AK8975.*/ +#define AK8975_MEASUREMENT_TIME_US 10000 + +/*! \name AK8975 operation mode + \anchor AK8975_Mode + Defines an operation mode of the AK8975.*/ +/*! @{*/ +#define AK8975_MODE_SNG_MEASURE 0x01 +#define AK8975_MODE_SELF_TEST 0x08 +#define AK8975_MODE_FUSE_ACCESS 0x0F +#define AK8975_MODE_POWERDOWN 0x00 +/*! @}*/ + +/*! \name AK8975 register address +\anchor AK8975_REG +Defines a register address of the AK8975.*/ +/*! @{*/ +#define AK8975_REG_WIA 0x00 +#define AK8975_REG_INFO 0x01 +#define AK8975_REG_ST1 0x02 +#define AK8975_REG_HXL 0x03 +#define AK8975_REG_HXH 0x04 +#define AK8975_REG_HYL 0x05 +#define AK8975_REG_HYH 0x06 +#define AK8975_REG_HZL 0x07 +#define AK8975_REG_HZH 0x08 +#define AK8975_REG_ST2 0x09 +#define AK8975_REG_CNTL 0x0A +#define AK8975_REG_RSV 0x0B +#define AK8975_REG_ASTC 0x0C +#define AK8975_REG_TS1 0x0D +#define AK8975_REG_TS2 0x0E +#define AK8975_REG_I2CDIS 0x0F +/*! @}*/ + +/*! \name AK8975 fuse-rom address +\anchor AK8975_FUSE +Defines a read-only address of the fuse ROM of the AK8975.*/ +/*! @{*/ +#define AK8975_FUSE_ASAX 0x10 +#define AK8975_FUSE_ASAY 0x11 +#define AK8975_FUSE_ASAZ 0x12 +/*! @}*/ + +#define AKMIO 0xA1 + +/* IOCTLs for AKM library */ +#define ECS_IOCTL_READ _IOWR(AKMIO, 0x01, char*) +#define ECS_IOCTL_WRITE _IOW(AKMIO, 0x02, char*) +#define ECS_IOCTL_SET_MODE _IOW(AKMIO, 0x03, short) +#define ECS_IOCTL_GETDATA _IOR(AKMIO, 0x04, char[SENSOR_DATA_SIZE]) +#define ECS_IOCTL_SET_YPR _IOW(AKMIO, 0x05, int[YPR_DATA_SIZE]) +#define ECS_IOCTL_GET_OPEN_STATUS _IOR(AKMIO, 0x06, int) +#define ECS_IOCTL_GET_CLOSE_STATUS _IOR(AKMIO, 0x07, int) +#define ECS_IOCTL_GET_DELAY _IOR(AKMIO, 0x08, long long int[AKM_NUM_SENSORS]) +#define ECS_IOCTL_GET_LAYOUT _IOR(AKMIO, 0x09, char) +#define ECS_IOCTL_GET_ACCEL _IOR(AKMIO, 0x30, short[3]) + +struct akm8975_platform_data { + char layout; + int gpio_DRDY; +}; + +#endif + diff --git a/AK8975_FS/libsensors/AdxlSensor.cpp b/AK8975_FS/libsensors/AdxlSensor.cpp new file mode 100644 index 0000000..7b905bf --- /dev/null +++ b/AK8975_FS/libsensors/AdxlSensor.cpp @@ -0,0 +1,224 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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 <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <sys/select.h> +#include <cutils/log.h> + +#include "AdxlSensor.h" + +#define ADXL_DATA_NAME "ADXL34x accelerometer" +#define ADXL_MAX_SAMPLE_RATE_VAL 11 /* 200 Hz */ + +#define ADXL_UNIT_CONVERSION(value) ((value) * GRAVITY_EARTH / (256.0f)) + +/*****************************************************************************/ + +AdxlSensor::AdxlSensor() + : SensorBase(NULL, ADXL_DATA_NAME), + mEnabled(0), + mDelay(-1), + mInputReader(4), + mHasPendingEvent(false) +{ + mPendingEvent.version = sizeof(sensors_event_t); + mPendingEvent.sensor = ID_A; + mPendingEvent.type = SENSOR_TYPE_ACCELEROMETER; + memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data)); + + if (data_fd >= 0) { + strcpy(input_sysfs_path, "/sys/class/input/"); + strcat(input_sysfs_path, input_name); + strcat(input_sysfs_path, "/device/device/"); + input_sysfs_path_len = strlen(input_sysfs_path); + ALOGD("AdxlSensor: sysfs_path=%s", input_sysfs_path); + } else { + input_sysfs_path[0] = '\0'; + input_sysfs_path_len = 0; + } +} + +AdxlSensor::~AdxlSensor() { + if (mEnabled) { + setEnable(0, 0); + } +} + +int AdxlSensor::setInitialState() { + struct input_absinfo absinfo; + + if (mEnabled) { + if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo)) { + mPendingEvent.acceleration.x = ADXL_UNIT_CONVERSION(absinfo.value); + } + if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo)) { + mPendingEvent.acceleration.y = ADXL_UNIT_CONVERSION(absinfo.value); + } + if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo)) { + mPendingEvent.acceleration.z = ADXL_UNIT_CONVERSION(absinfo.value); + } + } + return 0; +} + +bool AdxlSensor::hasPendingEvents() const { + return mHasPendingEvent; +} + +int AdxlSensor::setEnable(int32_t handle, int enabled) { + int err = 0; + char buffer[2]; + + /* handle check */ + if (handle != ID_A) { + ALOGE("AdxlSensor: Invalid handle (%d)", handle); + return -EINVAL; + } + + buffer[0] = '\0'; + buffer[1] = '\0'; + + if (mEnabled <= 0) { + if(enabled) buffer[0] = '0'; + } else if (mEnabled == 1) { + if(!enabled) buffer[0] = '1'; + } + if (buffer[0] != '\0') { + strcpy(&input_sysfs_path[input_sysfs_path_len], "disable"); + err = write_sys_attribute(input_sysfs_path, buffer, 1); + if (err != 0) { + return err; + } + ALOGD("AdxlSensor: Control set %s", buffer); + setInitialState(); + } + + if (enabled) { + mEnabled++; + if (mEnabled > 32767) mEnabled = 32767; + } else { + mEnabled--; + if (mEnabled < 0) mEnabled = 0; + } + ALOGD("AdxlSensor: mEnabled = %d", mEnabled); + + return err; +} + +int AdxlSensor::setDelay(int32_t handle, int64_t delay_ns) +{ + int err = 0; + int rate_val; + int32_t us; + char buffer[16]; + int bytes; + + /* handle check */ + if (handle != ID_A) { + ALOGE("AdxlSensor: Invalid handle (%d)", handle); + return -EINVAL; + } + + if (mDelay != delay_ns) { + /* + * The ADXL34x Supports 16 sample rates ranging from 3200Hz-0.098Hz + * Calculate best fit and limit to max 200Hz (rate_val 11) + */ + + us = (int32_t)(delay_ns / 1000); + for (rate_val = 0; rate_val < 16; rate_val++) + if (us >= ((10000000) >> rate_val)) + break; + + if (rate_val > ADXL_MAX_SAMPLE_RATE_VAL) { + rate_val = ADXL_MAX_SAMPLE_RATE_VAL; + } + + strcpy(&input_sysfs_path[input_sysfs_path_len], "rate"); + bytes = sprintf(buffer, "%d", rate_val); + err = write_sys_attribute(input_sysfs_path, buffer, bytes); + if (err == 0) { + mDelay = delay_ns; + ALOGD("AdxlSensor: Control set delay %f ms requetsed, using %f ms", + delay_ns/1000000.0f, 1e6 / (3200000 >> (15 - rate_val))); + } + } + + return err; +} + +int64_t AdxlSensor::getDelay(int32_t handle) +{ + return (handle == ID_A) ? mDelay : 0; +} + +int AdxlSensor::getEnable(int32_t handle) +{ + return (handle == ID_A) ? mEnabled : 0; +} + +int AdxlSensor::readEvents(sensors_event_t* data, int count) +{ + if (count < 1) + return -EINVAL; + + if (mHasPendingEvent) { + mHasPendingEvent = false; + mPendingEvent.timestamp = getTimestamp(); + *data = mPendingEvent; + return mEnabled ? 1 : 0; + } + + ssize_t n = mInputReader.fill(data_fd); + if (n < 0) + return n; + + int numEventReceived = 0; + input_event const* event; + + while (count && mInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_ABS) { + float value = event->value; + if (event->code == EVENT_TYPE_ACCEL_X) { + mPendingEvent.acceleration.x = ADXL_UNIT_CONVERSION(value); + } else if (event->code == EVENT_TYPE_ACCEL_Y) { + mPendingEvent.acceleration.y = ADXL_UNIT_CONVERSION(value); + } else if (event->code == EVENT_TYPE_ACCEL_Z) { + mPendingEvent.acceleration.z = ADXL_UNIT_CONVERSION(value); + } + } else if (type == EV_SYN) { + mPendingEvent.timestamp = timevalToNano(event->time); + if (mEnabled) { + *data++ = mPendingEvent; + count--; + numEventReceived++; + } + } else { + ALOGE("AdxlSensor: unknown event (type=%d, code=%d)", + type, event->code); + } + mInputReader.next(); + } + + return numEventReceived; +} + diff --git a/AK8975_FS/libsensors/AdxlSensor.h b/AK8975_FS/libsensors/AdxlSensor.h new file mode 100644 index 0000000..c419efa --- /dev/null +++ b/AK8975_FS/libsensors/AdxlSensor.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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. + */ + +#ifndef ANDROID_ADXL_SENSOR_H +#define ANDROID_ADXL_SENSOR_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + +#include "sensors.h" +#include "SensorBase.h" +#include "InputEventReader.h" + +/*****************************************************************************/ + +struct input_event; + +class AdxlSensor : public SensorBase { + int mEnabled; + int64_t mDelay; + InputEventCircularReader mInputReader; + sensors_event_t mPendingEvent; + bool mHasPendingEvent; + char input_sysfs_path[PATH_MAX]; + int input_sysfs_path_len; + + int setInitialState(); + +public: + AdxlSensor(); + virtual ~AdxlSensor(); + virtual int readEvents(sensors_event_t* data, int count); + virtual bool hasPendingEvents() const; + virtual int setDelay(int32_t handle, int64_t ns); + virtual int setEnable(int32_t handle, int enabled); + virtual int64_t getDelay(int32_t handle); + virtual int getEnable(int32_t handle); +}; + +/*****************************************************************************/ + +#endif // ANDROID_ADXL_SENSOR_H diff --git a/AK8975_FS/libsensors/AkmSensor.cpp b/AK8975_FS/libsensors/AkmSensor.cpp new file mode 100644 index 0000000..6c8da8b --- /dev/null +++ b/AK8975_FS/libsensors/AkmSensor.cpp @@ -0,0 +1,323 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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 <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <sys/select.h> +#include <dlfcn.h> + +#include <cutils/log.h> + +#include "AkmSensor.h" + +#define AKMD_DEFAULT_INTERVAL 200000000 + +/*****************************************************************************/ + +AkmSensor::AkmSensor() +: SensorBase(NULL, "compass"), + mPendingMask(0), + mInputReader(32) +{ + for (int i=0; i<numSensors; i++) { + mEnabled[i] = 0; + mDelay[i] = -1; + } + memset(mPendingEvents, 0, sizeof(mPendingEvents)); + + mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); + mPendingEvents[Accelerometer].sensor = ID_A; + mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; + mPendingEvents[Accelerometer].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[MagneticField].version = sizeof(sensors_event_t); + mPendingEvents[MagneticField].sensor = ID_M; + mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; + mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Orientation ].version = sizeof(sensors_event_t); + mPendingEvents[Orientation ].sensor = ID_O; + mPendingEvents[Orientation ].type = SENSOR_TYPE_ORIENTATION; + mPendingEvents[Orientation ].orientation.status = SENSOR_STATUS_ACCURACY_HIGH; + + if (data_fd) { + strcpy(input_sysfs_path, "/sys/class/compass/akm8975/"); + input_sysfs_path_len = strlen(input_sysfs_path); + } else { + input_sysfs_path[0] = '\0'; + input_sysfs_path_len = 0; + } +} + +AkmSensor::~AkmSensor() +{ + for (int i=0; i<numSensors; i++) { + setEnable(i, 0); + } +} + +int AkmSensor::setEnable(int32_t handle, int enabled) +{ + int id = handle2id(handle); + int err = 0; + char buffer[2]; + + switch (id) { + case Accelerometer: + strcpy(&input_sysfs_path[input_sysfs_path_len], "enable_acc"); + break; + case MagneticField: + strcpy(&input_sysfs_path[input_sysfs_path_len], "enable_mag"); + break; + case Orientation: + strcpy(&input_sysfs_path[input_sysfs_path_len], "enable_ori"); + break; + default: + ALOGE("AkmSensor: unknown handle (%d)", handle); + return -EINVAL; + } + + buffer[0] = '\0'; + buffer[1] = '\0'; + + if (mEnabled[id] <= 0) { + if(enabled) buffer[0] = '1'; + } else if (mEnabled[id] == 1) { + if(!enabled) buffer[0] = '0'; + } + + if (buffer[0] != '\0') { + err = write_sys_attribute(input_sysfs_path, buffer, 1); + if (err != 0) { + return err; + } + ALOGD("AkmSensor: set %s to %s", + &input_sysfs_path[input_sysfs_path_len], buffer); + + /* for AKMD specification */ + if (buffer[0] == '1') { + setDelay(handle, AKMD_DEFAULT_INTERVAL); + } else { + setDelay(handle, -1); + } + } + + if (enabled) { + (mEnabled[id])++; + if (mEnabled[id] > 32767) mEnabled[id] = 32767; + } else { + (mEnabled[id])--; + if (mEnabled[id] < 0) mEnabled[id] = 0; + } + ALOGD("AkmSensor: mEnabled[%d] = %d", id, mEnabled[id]); + + return err; +} + +int AkmSensor::setDelay(int32_t handle, int64_t ns) +{ + int id = handle2id(handle); + int err = 0; + char buffer[32]; + int bytes; + + if (ns < -1 || 2147483647 < ns) { + ALOGE("AkmSensor: invalid delay (%lld)", ns); + return -EINVAL; + } + + switch (id) { + case Accelerometer: + strcpy(&input_sysfs_path[input_sysfs_path_len], "delay_acc"); + break; + case MagneticField: + strcpy(&input_sysfs_path[input_sysfs_path_len], "delay_mag"); + break; + case Orientation: + strcpy(&input_sysfs_path[input_sysfs_path_len], "delay_ori"); + break; + default: + ALOGE("AkmSensor: unknown handle (%d)", handle); + return -EINVAL; + } + + if (ns != mDelay[id]) { + bytes = sprintf(buffer, "%lld", ns); + err = write_sys_attribute(input_sysfs_path, buffer, bytes); + if (err == 0) { + mDelay[id] = ns; + ALOGD("AkmSensor: set %s to %f ms.", + &input_sysfs_path[input_sysfs_path_len], ns/1000000.0f); + } + } + + return err; +} + +int64_t AkmSensor::getDelay(int32_t handle) +{ + int id = handle2id(handle); + if (id > 0) { + return mDelay[id]; + } else { + return 0; + } +} + +int AkmSensor::getEnable(int32_t handle) +{ + int id = handle2id(handle); + if (id >= 0) { + return mEnabled[id]; + } else { + return 0; + } +} + +int AkmSensor::readEvents(sensors_event_t* data, int count) +{ + if (count < 1) + return -EINVAL; + + ssize_t n = mInputReader.fill(data_fd); + if (n < 0) + return n; + + int numEventReceived = 0; + input_event const* event; + + while (count && mInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_ABS) { + processEvent(event->code, event->value); + mInputReader.next(); + } else if (type == EV_SYN) { + int64_t time = timevalToNano(event->time); + for (int j=0 ; count && mPendingMask && j<numSensors ; j++) { + if (mPendingMask & (1<<j)) { + mPendingMask &= ~(1<<j); + mPendingEvents[j].timestamp = time; + //ALOGD("data=%8.5f,%8.5f,%8.5f", + //mPendingEvents[j].data[0], + //mPendingEvents[j].data[1], + //mPendingEvents[j].data[2]); + if (mEnabled[j]) { + *data++ = mPendingEvents[j]; + count--; + numEventReceived++; + } + } + } + if (!mPendingMask) { + mInputReader.next(); + } + } else { + ALOGE("AkmSensor: unknown event (type=%d, code=%d)", + type, event->code); + mInputReader.next(); + } + } + return numEventReceived; +} + +int AkmSensor::setAccel(sensors_event_t* data) +{ + int err; + int16_t acc[3]; + + acc[0] = (int16_t)(data->acceleration.x / GRAVITY_EARTH * AKSC_LSG); + acc[1] = (int16_t)(data->acceleration.y / GRAVITY_EARTH * AKSC_LSG); + acc[2] = (int16_t)(data->acceleration.z / GRAVITY_EARTH * AKSC_LSG); + + strcpy(&input_sysfs_path[input_sysfs_path_len], "accel"); + err = write_sys_attribute(input_sysfs_path, (char*)acc, 6); + if (err < 0) { + ALOGD("AkmSensor: %s write failed.", + &input_sysfs_path[input_sysfs_path_len]); + } + return err; +} + +int AkmSensor::handle2id(int32_t handle) +{ + switch (handle) { + case ID_A: + return Accelerometer; + case ID_M: + return MagneticField; + case ID_O: + return Orientation; + default: + ALOGE("AkmSensor: unknown handle (%d)", handle); + return -EINVAL; + } +} + +void AkmSensor::processEvent(int code, int value) +{ + switch (code) { + case EVENT_TYPE_ACCEL_X: + mPendingMask |= 1<<Accelerometer; + mPendingEvents[Accelerometer].acceleration.x = value * CONVERT_A; + break; + case EVENT_TYPE_ACCEL_Y: + mPendingMask |= 1<<Accelerometer; + mPendingEvents[Accelerometer].acceleration.y = value * CONVERT_A; + break; + case EVENT_TYPE_ACCEL_Z: + mPendingMask |= 1<<Accelerometer; + mPendingEvents[Accelerometer].acceleration.z = value * CONVERT_A; + break; + + case EVENT_TYPE_MAGV_X: + mPendingMask |= 1<<MagneticField; + mPendingEvents[MagneticField].magnetic.x = value * CONVERT_M; + break; + case EVENT_TYPE_MAGV_Y: + mPendingMask |= 1<<MagneticField; + mPendingEvents[MagneticField].magnetic.y = value * CONVERT_M; + break; + case EVENT_TYPE_MAGV_Z: + mPendingMask |= 1<<MagneticField; + mPendingEvents[MagneticField].magnetic.z = value * CONVERT_M; + break; + case EVENT_TYPE_MAGV_STATUS: + mPendingMask |= 1<<MagneticField; + mPendingEvents[MagneticField].magnetic.status = value; + break; + + case EVENT_TYPE_YAW: + mPendingMask |= 1<<Orientation; + mPendingEvents[Orientation].orientation.azimuth = value * CONVERT_O; + break; + case EVENT_TYPE_PITCH: + mPendingMask |= 1<<Orientation; + mPendingEvents[Orientation].orientation.pitch = value * CONVERT_O; + break; + case EVENT_TYPE_ROLL: + mPendingMask |= 1<<Orientation; + mPendingEvents[Orientation].orientation.roll = value * CONVERT_O; + break; + case EVENT_TYPE_ORIENT_STATUS: + mPendingMask |= 1<<Orientation; + mPendingEvents[Orientation].orientation.status = value; + break; + } +} diff --git a/AK8975_FS/libsensors/AkmSensor.h b/AK8975_FS/libsensors/AkmSensor.h new file mode 100644 index 0000000..65d6715 --- /dev/null +++ b/AK8975_FS/libsensors/AkmSensor.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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. + */ + +#ifndef ANDROID_AKM_SENSOR_H +#define ANDROID_AKM_SENSOR_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + + +#include "sensors.h" +#include "SensorBase.h" +#include "InputEventReader.h" + +/*****************************************************************************/ + +struct input_event; + +class AkmSensor : public SensorBase { +public: + AkmSensor(); + virtual ~AkmSensor(); + + enum { + Accelerometer = 0, + MagneticField, + Orientation, + numSensors + }; + + virtual int readEvents(sensors_event_t* data, int count); + virtual int setDelay(int32_t handle, int64_t ns); + virtual int setEnable(int32_t handle, int enabled); + virtual int64_t getDelay(int32_t handle); + virtual int getEnable(int32_t handle); + int setAccel(sensors_event_t* data); + +private: + int mEnabled[numSensors]; + int64_t mDelay[numSensors]; + uint32_t mPendingMask; + InputEventCircularReader mInputReader; + sensors_event_t mPendingEvents[numSensors]; + char input_sysfs_path[PATH_MAX]; + int input_sysfs_path_len; + + int handle2id(int32_t handle); + void processEvent(int code, int value); +}; + +/*****************************************************************************/ + +#endif // ANDROID_AKM_SENSOR_H diff --git a/AK8975_FS/libsensors/Android.mk b/AK8975_FS/libsensors/Android.mk new file mode 100644 index 0000000..49dfd81 --- /dev/null +++ b/AK8975_FS/libsensors/Android.mk @@ -0,0 +1,48 @@ +# Copyright (C) 2008 The Android Open Source Project +# +# 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. + + +LOCAL_PATH := $(call my-dir) + +ifneq ($(TARGET_SIMULATOR),true) + +# HAL module implemenation, not prelinked, and stored in +# hw/<SENSORS_HARDWARE_MODULE_ID>.<ro.product.board>.so +include $(CLEAR_VARS) + +LOCAL_MODULE := sensors.default + +LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw + +LOCAL_MODULE_TAGS := optional + +LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" \ + -Wall \ + -DSENSORHAL_ACC_ADXL346 +# -DSENSORHAL_ACC_KXTF9 + +LOCAL_SRC_FILES := \ + SensorBase.cpp \ + InputEventReader.cpp \ + AkmSensor.cpp \ + sensors.cpp \ + AdxlSensor.cpp +# KionixSensor.cpp + +LOCAL_SHARED_LIBRARIES := liblog libcutils libdl +LOCAL_PRELINK_MODULE := false + +include $(BUILD_SHARED_LIBRARY) + +endif # !TARGET_SIMULATOR diff --git a/AK8975_FS/libsensors/InputEventReader.cpp b/AK8975_FS/libsensors/InputEventReader.cpp new file mode 100644 index 0000000..1014f29 --- /dev/null +++ b/AK8975_FS/libsensors/InputEventReader.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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 <stdint.h> +#include <errno.h> +#include <unistd.h> +#include <poll.h> + +#include <sys/cdefs.h> +#include <sys/types.h> + +#include <linux/input.h> + +#include <cutils/log.h> + +#include "InputEventReader.h" + +/*****************************************************************************/ + +struct input_event; + +InputEventCircularReader::InputEventCircularReader(size_t numEvents) + : mBuffer(new input_event[numEvents * 2]), + mBufferEnd(mBuffer + numEvents), + mHead(mBuffer), + mCurr(mBuffer), + mFreeSpace(numEvents) +{ +} + +InputEventCircularReader::~InputEventCircularReader() +{ + delete [] mBuffer; +} + +ssize_t InputEventCircularReader::fill(int fd) +{ + size_t numEventsRead = 0; + if (mFreeSpace) { + const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event)); + if (nread<0 || nread % sizeof(input_event)) { + // we got a partial event!! + return nread<0 ? -errno : -EINVAL; + } + + numEventsRead = nread / sizeof(input_event); + if (numEventsRead) { + mHead += numEventsRead; + mFreeSpace -= numEventsRead; + if (mHead > mBufferEnd) { + size_t s = mHead - mBufferEnd; + memcpy(mBuffer, mBufferEnd, s * sizeof(input_event)); + mHead = mBuffer + s; + } + } + } + + return numEventsRead; +} + +ssize_t InputEventCircularReader::readEvent(input_event const** events) +{ + *events = mCurr; + ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace; + return available ? 1 : 0; +} + +void InputEventCircularReader::next() +{ + mCurr++; + mFreeSpace++; + if (mCurr >= mBufferEnd) { + mCurr = mBuffer; + } +} diff --git a/AK8975_FS/libsensors/InputEventReader.h b/AK8975_FS/libsensors/InputEventReader.h new file mode 100644 index 0000000..180aade --- /dev/null +++ b/AK8975_FS/libsensors/InputEventReader.h @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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. + */ + +#ifndef ANDROID_INPUT_EVENT_READER_H +#define ANDROID_INPUT_EVENT_READER_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + +/*****************************************************************************/ + +struct input_event; + +class InputEventCircularReader +{ + struct input_event* const mBuffer; + struct input_event* const mBufferEnd; + struct input_event* mHead; + struct input_event* mCurr; + ssize_t mFreeSpace; + +public: + InputEventCircularReader(size_t numEvents); + ~InputEventCircularReader(); + ssize_t fill(int fd); + ssize_t readEvent(input_event const** events); + void next(); +}; + +/*****************************************************************************/ + +#endif // ANDROID_INPUT_EVENT_READER_H diff --git a/AK8975_FS/libsensors/KionixSensor.cpp b/AK8975_FS/libsensors/KionixSensor.cpp new file mode 100644 index 0000000..a2e3229 --- /dev/null +++ b/AK8975_FS/libsensors/KionixSensor.cpp @@ -0,0 +1,202 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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 <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <sys/select.h> +#include <cutils/log.h> +#include <linux/kxtf9.h> + +#include "KionixSensor.h" + +#define KIONIX_IOCTL_ENABLE_OUTPUT KXTF9_IOCTL_ENABLE_OUTPUT +#define KIONIX_IOCTL_DISABLE_OUTPUT KXTF9_IOCTL_DISABLE_OUTPUT +#define KIONIX_IOCTL_GET_ENABLE KXTF9_IOCTL_GET_ENABLE +#define KIONIX_IOCTL_UPDATE_ODR KXTF9_IOCTL_UPDATE_ODR + +#define KIONIX_UNIT_CONVERSION(value) ((value) * GRAVITY_EARTH / (1024.0f)) + +/*****************************************************************************/ + +KionixSensor::KionixSensor() + : SensorBase(DIR_DEV, INPUT_NAME_ACC), + mEnabled(0), + mDelay(-1), + mInputReader(32), + mHasPendingEvent(false) +{ + mPendingEvent.version = sizeof(sensors_event_t); + mPendingEvent.sensor = ID_A; + mPendingEvent.type = SENSOR_TYPE_ACCELEROMETER; + memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data)); + + open_device(); +} + +KionixSensor::~KionixSensor() { + if (mEnabled) { + setEnable(0, 0); + } + + close_device(); +} + +int KionixSensor::setInitialState() { + struct input_absinfo absinfo; + + if (mEnabled) { + if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo)) { + mPendingEvent.acceleration.x = KIONIX_UNIT_CONVERSION(absinfo.value); + } + if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo)) { + mPendingEvent.acceleration.y = KIONIX_UNIT_CONVERSION(absinfo.value); + } + if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo)) { + mPendingEvent.acceleration.z = KIONIX_UNIT_CONVERSION(absinfo.value); + } + } + return 0; +} + +bool KionixSensor::hasPendingEvents() const { + return mHasPendingEvent; +} + +int KionixSensor::setEnable(int32_t handle, int enabled) { + int err = 0; + int opDone = 0; + + /* handle check */ + if (handle != ID_A) { + ALOGE("KionixSensor: Invalid handle (%d)", handle); + return -EINVAL; + } + + if (mEnabled <= 0) { + if (enabled) { + err = ioctl(dev_fd, KIONIX_IOCTL_ENABLE_OUTPUT); + opDone = 1; + } + } else if (mEnabled == 1) { + if (!enabled) { + err = ioctl(dev_fd, KIONIX_IOCTL_DISABLE_OUTPUT); + opDone = 1; + } + } + if (err != 0) { + ALOGE("KionixSensor: IOCTL failed (%s)", strerror(errno)); + return err; + } + if (opDone) { + ALOGD("KionixSensor: Control set %d", enabled); + setInitialState(); + } + + if (enabled) { + mEnabled++; + if (mEnabled > 32767) mEnabled = 32767; + } else { + mEnabled--; + if (mEnabled < 0) mEnabled = 0; + } + ALOGD("KionixSensor: mEnabled = %d", mEnabled); + + return err; +} + +int KionixSensor::setDelay(int32_t handle, int64_t delay_ns) +{ + int err = 0; + int ms; + + /* handle check */ + if (handle != ID_A) { + ALOGE("KionixSensor: Invalid handle (%d)", handle); + return -EINVAL; + } + + if (mDelay != delay_ns) { + ms = delay_ns / 1000000; + if (ioctl(dev_fd, KIONIX_IOCTL_UPDATE_ODR, &ms)) { + return -errno; + } + mDelay = delay_ns; + } + + return err; +} + +int64_t KionixSensor::getDelay(int32_t handle) +{ + return (handle == ID_A) ? mDelay : 0; +} + +int KionixSensor::getEnable(int32_t handle) +{ + return (handle == ID_A) ? mEnabled : 0; +} + +int KionixSensor::readEvents(sensors_event_t* data, int count) +{ + if (count < 1) + return -EINVAL; + + if (mHasPendingEvent) { + mHasPendingEvent = false; + mPendingEvent.timestamp = getTimestamp(); + *data = mPendingEvent; + return mEnabled ? 1 : 0; + } + + ssize_t n = mInputReader.fill(data_fd); + if (n < 0) + return n; + + int numEventReceived = 0; + input_event const* event; + + while (count && mInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_ABS) { + float value = event->value; + if (event->code == EVENT_TYPE_ACCEL_X) { + mPendingEvent.acceleration.x = KIONIX_UNIT_CONVERSION(value); + } else if (event->code == EVENT_TYPE_ACCEL_Y) { + mPendingEvent.acceleration.y = KIONIX_UNIT_CONVERSION(value); + } else if (event->code == EVENT_TYPE_ACCEL_Z) { + mPendingEvent.acceleration.z = KIONIX_UNIT_CONVERSION(value); + } + } else if (type == EV_SYN) { + mPendingEvent.timestamp = timevalToNano(event->time); + if (mEnabled) { + *data++ = mPendingEvent; + count--; + numEventReceived++; + } + } else { + ALOGE("KionixSensor: unknown event (type=%d, code=%d)", + type, event->code); + } + mInputReader.next(); + } + + return numEventReceived; +} + diff --git a/AK8975_FS/libsensors/KionixSensor.h b/AK8975_FS/libsensors/KionixSensor.h new file mode 100644 index 0000000..6f29de4 --- /dev/null +++ b/AK8975_FS/libsensors/KionixSensor.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * 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. + */ + +#ifndef ANDROID_KIONIX_SENSOR_H +#define ANDROID_KIONIX_SENSOR_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + +#include "sensors.h" +#include "SensorBase.h" +#include "InputEventReader.h" + +/*****************************************************************************/ + +struct input_event; + +class KionixSensor : public SensorBase { + int mEnabled; + int64_t mDelay; + InputEventCircularReader mInputReader; + sensors_event_t mPendingEvent; + bool mHasPendingEvent; + char input_sysfs_path[PATH_MAX]; + int input_sysfs_path_len; + + int setInitialState(); + +public: + KionixSensor(); + virtual ~KionixSensor(); + virtual int readEvents(sensors_event_t* data, int count); + virtual bool hasPendingEvents() const; + virtual int setDelay(int32_t handle, int64_t ns); + virtual int setEnable(int32_t handle, int enabled); + virtual int64_t getDelay(int32_t handle); + virtual int getEnable(int32_t handle); +}; + +/*****************************************************************************/ + +#endif // ANDROID_KIONIX_SENSOR_H diff --git a/AK8975_FS/libsensors/SensorBase.cpp b/AK8975_FS/libsensors/SensorBase.cpp new file mode 100644 index 0000000..3f24fae --- /dev/null +++ b/AK8975_FS/libsensors/SensorBase.cpp @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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 <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <sys/select.h> + +#include <cutils/log.h> + +#include <linux/input.h> + +#include "SensorBase.h" + +/*****************************************************************************/ + +SensorBase::SensorBase( + const char* dev_name, + const char* data_name) + : dev_name(dev_name), data_name(data_name), + dev_fd(-1), data_fd(-1) +{ + if (data_name) { + data_fd = openInput(data_name); + } +} + +SensorBase::~SensorBase() { + if (data_fd >= 0) { + close(data_fd); + } + if (dev_fd >= 0) { + close(dev_fd); + } +} + +int SensorBase::open_device() { + if (dev_fd<0 && dev_name) { + dev_fd = open(dev_name, O_RDONLY); + ALOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno)); + } + return 0; +} + +int SensorBase::close_device() { + if (dev_fd >= 0) { + close(dev_fd); + dev_fd = -1; + } + return 0; +} + +int SensorBase::write_sys_attribute( + const char *path, const char *value, int bytes) +{ + int fd, amt; + + fd = open(path, O_WRONLY); + if (fd < 0) { + ALOGE("SensorBase: write_attr failed to open %s (%s)", + path, strerror(errno)); + return -1; + } + + amt = write(fd, value, bytes); + amt = ((amt == -1) ? -errno : 0); + ALOGE_IF(amt < 0, "SensorBase: write_int failed to write %s (%s)", + path, strerror(errno)); + close(fd); + return amt; +} + +int SensorBase::getFd() const { + if (!data_name) { + return dev_fd; + } + return data_fd; +} + +int SensorBase::setDelay(int32_t handle, int64_t ns) { + return 0; +} + +int64_t SensorBase::getDelay(int32_t handle) { + return 0; +} + +bool SensorBase::hasPendingEvents() const { + return false; +} + +int64_t SensorBase::getTimestamp() { + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return int64_t(t.tv_sec)*1000000000LL + t.tv_nsec; +} + +int SensorBase::openInput(const char* inputName) { + int fd = -1; + const char *dirname = "/dev/input"; + char devname[PATH_MAX]; + char *filename; + DIR *dir; + struct dirent *de; + dir = opendir(dirname); + if(dir == NULL) + return -1; + strcpy(devname, dirname); + filename = devname + strlen(devname); + *filename++ = '/'; + while((de = readdir(dir))) { + if(de->d_name[0] == '.' && + (de->d_name[1] == '\0' || + (de->d_name[1] == '.' && de->d_name[2] == '\0'))) + continue; + strcpy(filename, de->d_name); + fd = open(devname, O_RDONLY); + if (fd>=0) { + char name[80]; + if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) { + name[0] = '\0'; + } + if (!strcmp(name, inputName)) { + strcpy(input_name, filename); + break; + } else { + close(fd); + fd = -1; + } + } + } + closedir(dir); + ALOGE_IF(fd<0, "couldn't find '%s' input device", inputName); + return fd; +} diff --git a/AK8975_FS/libsensors/SensorBase.h b/AK8975_FS/libsensors/SensorBase.h new file mode 100644 index 0000000..4d16784 --- /dev/null +++ b/AK8975_FS/libsensors/SensorBase.h @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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. + */ + +#ifndef ANDROID_SENSOR_BASE_H +#define ANDROID_SENSOR_BASE_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + + +/*****************************************************************************/ + +struct sensors_event_t; + +class SensorBase { +protected: + const char* dev_name; + const char* data_name; + char input_name[PATH_MAX]; + int dev_fd; + int data_fd; + + int openInput(const char* inputName); + static int64_t getTimestamp(); + + + static int64_t timevalToNano(timeval const& t) { + return t.tv_sec*1000000000LL + t.tv_usec*1000; + } + + int open_device(); + int close_device(); + int write_int(char const *path, int value); + int write_sys_attribute( + char const *path, char const *value, int bytes); + +public: + SensorBase( + const char* dev_name, + const char* data_name); + + virtual ~SensorBase(); + + virtual int readEvents(sensors_event_t* data, int count) = 0; + virtual bool hasPendingEvents() const; + virtual int getFd() const; + + virtual int setDelay(int32_t handle, int64_t ns); + virtual int64_t getDelay(int32_t handle); + + /* When this function is called, increments the reference counter. */ + virtual int setEnable(int32_t handle, int enabled) = 0; + /* It returns the number of reference. */ + virtual int getEnable(int32_t handle) = 0; +}; + +/*****************************************************************************/ + +#endif // ANDROID_SENSOR_BASE_H diff --git a/AK8975_FS/libsensors/sensors.cpp b/AK8975_FS/libsensors/sensors.cpp new file mode 100644 index 0000000..68be4fb --- /dev/null +++ b/AK8975_FS/libsensors/sensors.cpp @@ -0,0 +1,374 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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. + */ + +#define LOG_TAG "Sensors" + +#include <hardware/sensors.h> +#include <fcntl.h> +#include <errno.h> +#include <dirent.h> +#include <math.h> +#include <poll.h> +#include <pthread.h> +#include <stdlib.h> + +#include <linux/input.h> + +#include <utils/Atomic.h> +#include <utils/Log.h> + +#include "sensors.h" + +#if defined SENSORHAL_ACC_ADXL346 +#include "AdxlSensor.h" +#elif defined SENSORHAL_ACC_KXTF9 +#include "KionixSensor.h" +#else +#error "Sensor configuration ERROR: No sensor is defined." +#endif + +#include "AkmSensor.h" + +/*****************************************************************************/ + +#define DELAY_OUT_TIME 0x7FFFFFFF + +#define LIGHT_SENSOR_POLLTIME 2000000000 + + +#define SENSORS_ACCELERATION (1<<ID_A) +#define SENSORS_MAGNETIC_FIELD (1<<ID_M) +#define SENSORS_ORIENTATION (1<<ID_O) + +#define SENSORS_ACCELERATION_HANDLE 0 +#define SENSORS_MAGNETIC_FIELD_HANDLE 1 +#define SENSORS_ORIENTATION_HANDLE 2 + +/*****************************************************************************/ + +/* The SENSORS Module */ +static const struct sensor_t sSensorList[] = { + { "AK8975 3-axis Magnetic field sensor", + "Asahi Kasei Microdevices", + 1, + SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 1228.8f, + CONVERT_M, 0.35f, 10000, { } }, +#ifdef SENSORHAL_ACC_ADXL346 + { "Analog Devices ADXL345/6 3-axis Accelerometer", + "ADI", + 1, SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, (GRAVITY_EARTH * 16.0f), + (GRAVITY_EARTH * 16.0f) / 4096.0f, 0.145f, 10000, { } }, + { "AK8975 Orientation sensor", + "Asahi Kasei Microdevices", + 1, SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, + CONVERT_O, 0.495f, 10000, { } } +#endif +#ifdef SENSORHAL_ACC_KXTF9 + { "Kionix KXTF9 3-axis Accelerometer", + "Kionix", + 1, SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, (GRAVITY_EARTH * 2.0f), + (GRAVITY_EARTH) / 1024.0f, 0.7f, 10000, { } }, + { "AK8975 Orientation sensor", + "Asahi Kasei Microdevices", + 1, SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, + CONVERT_O, 1.05f, 10000, { } } +#endif +}; + + +static int open_sensors(const struct hw_module_t* module, const char* id, + struct hw_device_t** device); + +static int sensors__get_sensors_list(struct sensors_module_t* module, + struct sensor_t const** list) +{ + *list = sSensorList; + return ARRAY_SIZE(sSensorList); +} + +static struct hw_module_methods_t sensors_module_methods = { + open: open_sensors +}; + +struct sensors_module_t HAL_MODULE_INFO_SYM = { + common: { + tag: HARDWARE_MODULE_TAG, + version_major: 1, + version_minor: 0, + id: SENSORS_HARDWARE_MODULE_ID, + name: "AKM Sensor module", + author: "Asahi Kasei Microdevices", + methods: &sensors_module_methods, + }, + get_sensors_list: sensors__get_sensors_list, +}; + +struct sensors_poll_context_t { + struct sensors_poll_device_t device; // must be first + + sensors_poll_context_t(); + ~sensors_poll_context_t(); + int activate(int handle, int enabled); + int setDelay(int handle, int64_t ns); + int setDelay_sub(int handle, int64_t ns); + int pollEvents(sensors_event_t* data, int count); + +private: + enum { + acc = 0, + akm = 1, + numSensorDrivers, + numFds, + }; + + static const size_t wake = numFds - 1; + static const char WAKE_MESSAGE = 'W'; + struct pollfd mPollFds[numFds]; + int mWritePipeFd; + SensorBase* mSensors[numSensorDrivers]; + + /* These function will be different depends on + * which sensor is implemented in AKMD program. + */ + int handleToDriver(int handle); + int proxy_enable(int handle, int enabled); + int proxy_setDelay(int handle, int64_t ns); +}; + +/*****************************************************************************/ + +sensors_poll_context_t::sensors_poll_context_t() +{ +#ifdef SENSORHAL_ACC_ADXL346 + mSensors[acc] = new AdxlSensor(); +#endif +#ifdef SENSORHAL_ACC_KXTF9 + mSensors[acc] = new KionixSensor(); +#endif + mPollFds[acc].fd = mSensors[acc]->getFd(); + mPollFds[acc].events = POLLIN; + mPollFds[acc].revents = 0; + + mSensors[akm] = new AkmSensor(); + mPollFds[akm].fd = mSensors[akm]->getFd(); + mPollFds[akm].events = POLLIN; + mPollFds[akm].revents = 0; + + int wakeFds[2]; + int result = pipe(wakeFds); + ALOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno)); + fcntl(wakeFds[0], F_SETFL, O_NONBLOCK); + fcntl(wakeFds[1], F_SETFL, O_NONBLOCK); + mWritePipeFd = wakeFds[1]; + + mPollFds[wake].fd = wakeFds[0]; + mPollFds[wake].events = POLLIN; + mPollFds[wake].revents = 0; +} + +sensors_poll_context_t::~sensors_poll_context_t() { + for (int i=0 ; i<numSensorDrivers ; i++) { + delete mSensors[i]; + } + close(mPollFds[wake].fd); + close(mWritePipeFd); +} + +int sensors_poll_context_t::handleToDriver(int handle) { + switch (handle) { + case ID_A: + return acc; + case ID_M: + case ID_O: + return akm; + } + return -EINVAL; +} + +int sensors_poll_context_t::activate(int handle, int enabled) { + int drv = handleToDriver(handle); + int err; + + switch (handle) { + case ID_A: + case ID_M: + /* No dependencies */ + break; + + case ID_O: + /* These sensors depend on ID_A and ID_M */ + mSensors[handleToDriver(ID_A)]->setEnable(ID_A, enabled); + mSensors[handleToDriver(ID_M)]->setEnable(ID_M, enabled); + break; + + default: + return -EINVAL; + } + err = mSensors[drv]->setEnable(handle, enabled); + + if (enabled && !err) { + const char wakeMessage(WAKE_MESSAGE); + int result = write(mWritePipeFd, &wakeMessage, 1); + ALOGE_IF(result<0, "error sending wake message (%s)", strerror(errno)); + } + return err; +} + +int sensors_poll_context_t::setDelay(int handle, int64_t ns) { + switch (handle) { + case ID_A: + case ID_M: + /* No dependencies */ + break; + + case ID_O: + /* These sensors depend on ID_A and ID_M */ + setDelay_sub(ID_A, ns); + setDelay_sub(ID_M, ns); + break; + + default: + return -EINVAL; + } + return setDelay_sub(handle, ns); +} + +int sensors_poll_context_t::setDelay_sub(int handle, int64_t ns) { + int drv = handleToDriver(handle); + int en = mSensors[drv]->getEnable(handle); + int64_t cur = mSensors[drv]->getDelay(handle); + int err = 0; + + if (en <= 1) { + /* no dependencies */ + if (cur != ns) { + err = mSensors[drv]->setDelay(handle, ns); + } + } else { + /* has dependencies, choose shorter interval */ + if (cur > ns) { + err = mSensors[drv]->setDelay(handle, ns); + } + } + return err; +} + +int sensors_poll_context_t::pollEvents(sensors_event_t* data, int count) +{ + int nbEvents = 0; + int n = 0; + + do { + // see if we have some leftover from the last poll() + for (int i=0 ; count && i<numSensorDrivers ; i++) { + SensorBase* const sensor(mSensors[i]); + if ((mPollFds[i].revents & POLLIN) || (sensor->hasPendingEvents())) { + int nb = sensor->readEvents(data, count); + if (nb < count) { + // no more data for this sensor + mPollFds[i].revents = 0; + } + if ((0 != nb) && (acc == i)) { + ((AkmSensor*)(mSensors[akm]))->setAccel(&data[nb-1]); + } + count -= nb; + nbEvents += nb; + data += nb; + } + } + + if (count) { + // we still have some room, so try to see if we can get + // some events immediately or just wait if we don't have + // anything to return + n = poll(mPollFds, numFds, nbEvents ? 0 : -1); + if (n<0) { + ALOGE("poll() failed (%s)", strerror(errno)); + return -errno; + } + if (mPollFds[wake].revents & POLLIN) { + char msg; + int result = read(mPollFds[wake].fd, &msg, 1); + ALOGE_IF(result<0, "error reading from wake pipe (%s)", strerror(errno)); + ALOGE_IF(msg != WAKE_MESSAGE, "unknown message on wake queue (0x%02x)", int(msg)); + mPollFds[wake].revents = 0; + } + } + // if we have events and space, go read them + } while (n && count); + + return nbEvents; +} + +/*****************************************************************************/ + +static int poll__close(struct hw_device_t *dev) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + if (ctx) { + delete ctx; + } + return 0; +} + +static int poll__activate(struct sensors_poll_device_t *dev, + int handle, int enabled) { + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->activate(handle, enabled); +} + +static int poll__setDelay(struct sensors_poll_device_t *dev, + int handle, int64_t ns) { + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->setDelay(handle, ns); +} + +static int poll__poll(struct sensors_poll_device_t *dev, + sensors_event_t* data, int count) { + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->pollEvents(data, count); +} + +/*****************************************************************************/ + +/** Open a new instance of a sensor device using name */ +static int open_sensors(const struct hw_module_t* module, const char* id, + struct hw_device_t** device) +{ + int status = -EINVAL; + sensors_poll_context_t *dev = new sensors_poll_context_t(); + + memset(&dev->device, 0, sizeof(sensors_poll_device_t)); + + dev->device.common.tag = HARDWARE_DEVICE_TAG; + dev->device.common.version = 0; + dev->device.common.module = const_cast<hw_module_t*>(module); + dev->device.common.close = poll__close; + dev->device.activate = poll__activate; + dev->device.setDelay = poll__setDelay; + dev->device.poll = poll__poll; + + *device = &dev->device.common; + status = 0; + + return status; +} + diff --git a/AK8975_FS/libsensors/sensors.h b/AK8975_FS/libsensors/sensors.h new file mode 100644 index 0000000..6060e6a --- /dev/null +++ b/AK8975_FS/libsensors/sensors.h @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * 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. + */ + +#ifndef ANDROID_SENSORS_H +#define ANDROID_SENSORS_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + +#include <linux/input.h> + +#include <hardware/hardware.h> +#include <hardware/sensors.h> + +__BEGIN_DECLS + +/*****************************************************************************/ + +#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) + +#define ID_A (0) +#define ID_M (1) +#define ID_O (2) + +/*****************************************************************************/ + +/* + * The SENSORS Module + */ + +/*****************************************************************************/ + +/* For ADXL346 */ +#define EVENT_TYPE_ACCEL_X ABS_X +#define EVENT_TYPE_ACCEL_Y ABS_Y +#define EVENT_TYPE_ACCEL_Z ABS_Z +#define EVENT_TYPE_ACCEL_STATUS ABS_THROTTLE + +/* For AK8975 */ +#define EVENT_TYPE_MAGV_X ABS_RX +#define EVENT_TYPE_MAGV_Y ABS_RY +#define EVENT_TYPE_MAGV_Z ABS_RZ +#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER + +/* Fro AKM Algorithm */ +#define EVENT_TYPE_YAW ABS_HAT0X +#define EVENT_TYPE_PITCH ABS_HAT0Y +#define EVENT_TYPE_ROLL ABS_HAT1X +#define EVENT_TYPE_ORIENT_STATUS ABS_HAT1Y + + +/* conversion of acceleration data to SI units (m/s^2) */ +/* 720 LSB = 1G */ +#define LSG (256.0f) +#define AKSC_LSG (720.0f) +#define CONVERT_A (GRAVITY_EARTH / LSG) + +/* conversion of magnetic data to uT units */ +#define CONVERT_M (0.06f) + +/* conversion of orientation data to degree units */ +#define CONVERT_O (0.015625f) + +#define SENSOR_STATE_MASK (0x7FFF) + +/*****************************************************************************/ + +__END_DECLS + +#endif // ANDROID_SENSORS_H |