diff options
62 files changed, 6895 insertions, 2132 deletions
diff --git a/libsensors/AkmSensor.cpp b/libsensors/AkmSensor.cpp deleted file mode 100644 index 0c6bd1e..0000000 --- a/libsensors/AkmSensor.cpp +++ /dev/null @@ -1,331 +0,0 @@ -/* - * 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 "ak8973b.h" - -#include <cutils/log.h> -#include "AkmSensor.h" - -//#define LOG_NDEBUG 0 - -/*****************************************************************************/ - -int (*akm_is_sensor_enabled)(uint32_t sensor_type); -int (*akm_enable_sensor)(uint32_t sensor_type); -int (*akm_disable_sensor)(uint32_t sensor_type); -int (*akm_set_delay)(uint32_t sensor_type, uint64_t delay); - -int stub_is_sensor_enabled(uint32_t sensor_type) { - return 0; -} - -int stub_enable_disable_sensor(uint32_t sensor_type) { - return -ENODEV; -} - -int stub_set_delay(uint32_t sensor_type, uint64_t delay) { - return -ENODEV; -} - -AkmSensor::AkmSensor() -: SensorBase(NULL, NULL), - mEnabled(0), - mPendingMask(0), - mInputReader(32) -{ - /* Open the library before opening the input device. The library - * creates a uinput device. - */ - if (loadAKMLibrary() == 0) { - data_name = "compass_sensor"; - data_fd = openInput("compass_sensor"); - } - - 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; - - // read the actual value of all sensors if they're enabled already - struct input_absinfo absinfo; - short flags = 0; - - if (akm_is_sensor_enabled(SENSOR_TYPE_ACCELEROMETER)) { - mEnabled |= 1<<Accelerometer; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo)) { - mPendingEvents[Accelerometer].acceleration.x = absinfo.value * CONVERT_A_X; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo)) { - mPendingEvents[Accelerometer].acceleration.y = absinfo.value * CONVERT_A_Y; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo)) { - mPendingEvents[Accelerometer].acceleration.z = absinfo.value * CONVERT_A_Z; - } - } - if (akm_is_sensor_enabled(SENSOR_TYPE_MAGNETIC_FIELD)) { - mEnabled |= 1<<MagneticField; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_MAGV_X), &absinfo)) { - mPendingEvents[MagneticField].magnetic.x = absinfo.value * CONVERT_M_X; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_MAGV_Y), &absinfo)) { - mPendingEvents[MagneticField].magnetic.y = absinfo.value * CONVERT_M_Y; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_MAGV_Z), &absinfo)) { - mPendingEvents[MagneticField].magnetic.z = absinfo.value * CONVERT_M_Z; - } - } - if (akm_is_sensor_enabled(SENSOR_TYPE_ORIENTATION)) { - mEnabled |= 1<<Orientation; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_YAW), &absinfo)) { - mPendingEvents[Orientation].orientation.azimuth = absinfo.value; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_PITCH), &absinfo)) { - mPendingEvents[Orientation].orientation.pitch = absinfo.value; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ROLL), &absinfo)) { - mPendingEvents[Orientation].orientation.roll = -absinfo.value; - } - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_ORIENT_STATUS), &absinfo)) { - mPendingEvents[Orientation].orientation.status = uint8_t(absinfo.value & SENSOR_STATE_MASK); - } - } - - // disable temperature sensor, since it is not supported - akm_disable_sensor(SENSOR_TYPE_TEMPERATURE); -} - -AkmSensor::~AkmSensor() -{ - if (mLibAKM) { - unsigned ref = ::dlclose(mLibAKM); - } -} - -int AkmSensor::enable(int32_t handle, int en) -{ - int what = -1; - - switch (handle) { - case ID_A: what = Accelerometer; break; - case ID_M: what = MagneticField; break; - case ID_O: what = Orientation; break; - } - - if (uint32_t(what) >= numSensors) - return -EINVAL; - - int newState = en ? 1 : 0; - int err = 0; - - if ((uint32_t(newState)<<what) != (mEnabled & (1<<what))) { - uint32_t sensor_type; - switch (what) { - case Accelerometer: sensor_type = SENSOR_TYPE_ACCELEROMETER; break; - case MagneticField: sensor_type = SENSOR_TYPE_MAGNETIC_FIELD; break; - case Orientation: sensor_type = SENSOR_TYPE_ORIENTATION; break; - } - short flags = newState; - if (en) - err = akm_enable_sensor(sensor_type); - else - err = akm_disable_sensor(sensor_type); - - ALOGE_IF(err, "Could not change sensor state (%s)", strerror(-err)); - if (!err) { - mEnabled &= ~(1<<what); - mEnabled |= (uint32_t(flags)<<what); - } - } - return err; -} - -int AkmSensor::setDelay(int32_t handle, int64_t ns) -{ - int what = -1; - uint32_t sensor_type = 0; - - if (ns < 0) - return -EINVAL; - - switch (handle) { - case ID_A: sensor_type = SENSOR_TYPE_ACCELEROMETER; break; - case ID_M: sensor_type = SENSOR_TYPE_MAGNETIC_FIELD; break; - case ID_O: sensor_type = SENSOR_TYPE_ORIENTATION; break; - } - - if (sensor_type == 0) - return -EINVAL; - - mDelays[what] = ns; - return update_delay(); -} - -int AkmSensor::update_delay() -{ - if (mEnabled) { - uint64_t wanted = -1LLU; - for (int i=0 ; i<numSensors ; i++) { - if (mEnabled & (1<<i)) { - uint64_t ns = mDelays[i]; - wanted = wanted < ns ? wanted : ns; - } - } - short delay = int64_t(wanted) / 1000000; - if (ioctl(dev_fd, ECS_IOCTL_APP_SET_DELAY, &delay)) { - return -errno; - } - } - return 0; -} - - -int AkmSensor::loadAKMLibrary() -{ - mLibAKM = dlopen("libakm.so", RTLD_NOW); - - if (!mLibAKM) { - akm_is_sensor_enabled = stub_is_sensor_enabled; - akm_enable_sensor = stub_enable_disable_sensor; - akm_disable_sensor = stub_enable_disable_sensor; - akm_set_delay = stub_set_delay; - ALOGE("AkmSensor: unable to load AKM Library, %s", dlerror()); - return -ENOENT; - } - - *(void **)&akm_is_sensor_enabled = dlsym(mLibAKM, "akm_is_sensor_enabled"); - *(void **)&akm_enable_sensor = dlsym(mLibAKM, "akm_enable_sensor"); - *(void **)&akm_disable_sensor = dlsym(mLibAKM, "akm_disable_sensor"); - *(void **)&akm_set_delay = dlsym(mLibAKM, "akm_set_delay"); - - 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_REL) { - 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; - if (mEnabled & (1<<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; -} - -void AkmSensor::processEvent(int code, int value) -{ - switch (code) { - case EVENT_TYPE_ACCEL_X: - mPendingMask |= 1<<Accelerometer; - mPendingEvents[Accelerometer].acceleration.x = value * CONVERT_A_X; - break; - case EVENT_TYPE_ACCEL_Y: - mPendingMask |= 1<<Accelerometer; - mPendingEvents[Accelerometer].acceleration.y = value * CONVERT_A_Y; - break; - case EVENT_TYPE_ACCEL_Z: - mPendingMask |= 1<<Accelerometer; - mPendingEvents[Accelerometer].acceleration.z = value * CONVERT_A_Z; - break; - - case EVENT_TYPE_MAGV_X: - ALOGV("AkmSensor: EVENT_TYPE_MAGV_X value =%d", value); - mPendingMask |= 1<<MagneticField; - mPendingEvents[MagneticField].magnetic.x = value * CONVERT_M_X; - break; - case EVENT_TYPE_MAGV_Y: - ALOGV("AkmSensor: EVENT_TYPE_MAGV_Y value =%d", value); - mPendingMask |= 1<<MagneticField; - mPendingEvents[MagneticField].magnetic.y = value * CONVERT_M_Y; - break; - case EVENT_TYPE_MAGV_Z: - ALOGV("AkmSensor: EVENT_TYPE_MAGV_Z value =%d", value); - mPendingMask |= 1<<MagneticField; - mPendingEvents[MagneticField].magnetic.z = value * CONVERT_M_Z; - break; - - case EVENT_TYPE_YAW: - mPendingMask |= 1<<Orientation; - mPendingEvents[Orientation].orientation.azimuth = value * CONVERT_O_A; - break; - case EVENT_TYPE_PITCH: - mPendingMask |= 1<<Orientation; - mPendingEvents[Orientation].orientation.pitch = value * CONVERT_O_P; - break; - case EVENT_TYPE_ROLL: - mPendingMask |= 1<<Orientation; - mPendingEvents[Orientation].orientation.roll = value * CONVERT_O_R; - break; - case EVENT_TYPE_ORIENT_STATUS: - uint8_t status = uint8_t(value & SENSOR_STATE_MASK); - if (status == 4) - status = 0; - mPendingMask |= 1<<Orientation; - mPendingEvents[Orientation].orientation.status = status; - break; - } -} diff --git a/libsensors/AkmSensor.h b/libsensors/AkmSensor.h deleted file mode 100644 index c155ec2..0000000 --- a/libsensors/AkmSensor.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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 = 1, - Orientation = 2, - numSensors - }; - - virtual int setDelay(int32_t handle, int64_t ns); - virtual int enable(int32_t handle, int enabled); - virtual int readEvents(sensors_event_t* data, int count); - void processEvent(int code, int value); - -private: - int loadAKMLibrary(); - int update_delay(); - void *mLibAKM; - uint32_t mEnabled; - uint32_t mPendingMask; - InputEventCircularReader mInputReader; - sensors_event_t mPendingEvents[numSensors]; - uint64_t mDelays[numSensors]; -}; - -/*****************************************************************************/ - -#endif // ANDROID_AKM_SENSOR_H diff --git a/libsensors/Android.mk b/libsensors/Android.mk deleted file mode 100644 index 9f0d998..0000000 --- a/libsensors/Android.mk +++ /dev/null @@ -1,46 +0,0 @@ -# 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.$(TARGET_BOARD_PLATFORM) - -LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw - -LOCAL_MODULE_TAGS := optional - -LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -LOCAL_SRC_FILES := \ - sensors.cpp \ - SensorBase.cpp \ - LightSensor.cpp \ - ProximitySensor.cpp \ - AkmSensor.cpp \ - GyroSensor.cpp \ - PressureSensor.cpp \ - InputEventReader.cpp - -LOCAL_SHARED_LIBRARIES := liblog libcutils libdl -LOCAL_PRELINK_MODULE := false - -include $(BUILD_SHARED_LIBRARY) - -endif # !TARGET_SIMULATOR diff --git a/libsensors/GyroSensor.cpp b/libsensors/GyroSensor.cpp deleted file mode 100644 index 0b0a438..0000000 --- a/libsensors/GyroSensor.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* - * 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 "GyroSensor.h" - -#define FETCH_FULL_EVENT_BEFORE_RETURN 1 -#define IGNORE_EVENT_TIME 350000000 -/*****************************************************************************/ - -GyroSensor::GyroSensor() - : SensorBase(NULL, "gyro_sensor"), - mEnabled(0), - mInputReader(4), - mHasPendingEvent(false), - mEnabledTime(0) -{ - mPendingEvent.version = sizeof(sensors_event_t); - mPendingEvent.sensor = ID_GY; - mPendingEvent.type = SENSOR_TYPE_GYROSCOPE; - memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data)); - - if (data_fd) { - strcpy(input_sysfs_path, "/sys/class/input/"); - strcat(input_sysfs_path, input_name); - strcat(input_sysfs_path, "/device/"); - input_sysfs_path_len = strlen(input_sysfs_path); - enable(0, 1); - } -} - -GyroSensor::~GyroSensor() { - if (mEnabled) { - enable(0, 0); - } -} - -int GyroSensor::setInitialState() { - struct input_absinfo absinfo_x; - struct input_absinfo absinfo_y; - struct input_absinfo absinfo_z; - float value; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_GYRO_X), &absinfo_x) && - !ioctl(data_fd, EVIOCGABS(EVENT_TYPE_GYRO_X), &absinfo_y) && - !ioctl(data_fd, EVIOCGABS(EVENT_TYPE_GYRO_X), &absinfo_z)) { - value = absinfo_x.value; - mPendingEvent.data[0] = value * CONVERT_GYRO_X; - value = absinfo_x.value; - mPendingEvent.data[1] = value * CONVERT_GYRO_Y; - value = absinfo_x.value; - mPendingEvent.data[2] = value * CONVERT_GYRO_Z; - mHasPendingEvent = true; - } - return 0; -} - -int GyroSensor::enable(int32_t, int en) { - int flags = en ? 1 : 0; - if (flags != mEnabled) { - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "enable"); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - char buf[2]; - int err; - buf[1] = 0; - if (flags) { - buf[0] = '1'; - mEnabledTime = getTimestamp() + IGNORE_EVENT_TIME; - } else { - buf[0] = '0'; - } - err = write(fd, buf, sizeof(buf)); - close(fd); - mEnabled = flags; - setInitialState(); - return 0; - } - return -1; - } - return 0; -} - -bool GyroSensor::hasPendingEvents() const { - return mHasPendingEvent; -} - -int GyroSensor::setDelay(int32_t handle, int64_t delay_ns) -{ - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "poll_delay"); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - char buf[80]; - sprintf(buf, "%lld", delay_ns); - write(fd, buf, strlen(buf)+1); - close(fd); - return 0; - } - return -1; -} - -int GyroSensor::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; - -#if FETCH_FULL_EVENT_BEFORE_RETURN -again: -#endif - while (count && mInputReader.readEvent(&event)) { - int type = event->type; - if (type == EV_REL) { - float value = event->value; - if (event->code == EVENT_TYPE_GYRO_X) { - mPendingEvent.data[0] = value * CONVERT_GYRO_X; - } else if (event->code == EVENT_TYPE_GYRO_Y) { - mPendingEvent.data[1] = value * CONVERT_GYRO_Y; - } else if (event->code == EVENT_TYPE_GYRO_Z) { - mPendingEvent.data[2] = value * CONVERT_GYRO_Z; - } - } else if (type == EV_SYN) { - mPendingEvent.timestamp = timevalToNano(event->time); - if (mEnabled) { - if (mPendingEvent.timestamp >= mEnabledTime) { - *data++ = mPendingEvent; - numEventReceived++; - } - count--; - } - } else { - ALOGE("GyroSensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mInputReader.next(); - } - -#if FETCH_FULL_EVENT_BEFORE_RETURN - /* if we didn't read a complete event, see if we can fill and - try again instead of returning with nothing and redoing poll. */ - if (numEventReceived == 0 && mEnabled == 1) { - n = mInputReader.fill(data_fd); - if (n) - goto again; - } -#endif - - return numEventReceived; -} - diff --git a/libsensors/GyroSensor.h b/libsensors/GyroSensor.h deleted file mode 100644 index e8997de..0000000 --- a/libsensors/GyroSensor.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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_GYRO_SENSOR_H -#define ANDROID_GYRO_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 GyroSensor : public SensorBase { - int mEnabled; - InputEventCircularReader mInputReader; - sensors_event_t mPendingEvent; - bool mHasPendingEvent; - char input_sysfs_path[PATH_MAX]; - int input_sysfs_path_len; - int64_t mEnabledTime; - - int setInitialState(); - -public: - GyroSensor(); - virtual ~GyroSensor(); - virtual int readEvents(sensors_event_t* data, int count); - virtual bool hasPendingEvents() const; - virtual int setDelay(int32_t handle, int64_t ns); - virtual int enable(int32_t handle, int enabled); -}; - -/*****************************************************************************/ - -#endif // ANDROID_GYRO_SENSOR_H diff --git a/libsensors/InputEventReader.cpp b/libsensors/InputEventReader.cpp deleted file mode 100644 index 1014f29..0000000 --- a/libsensors/InputEventReader.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - * 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/libsensors/InputEventReader.h b/libsensors/InputEventReader.h deleted file mode 100644 index 180aade..0000000 --- a/libsensors/InputEventReader.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * 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/libsensors/LightSensor.cpp b/libsensors/LightSensor.cpp deleted file mode 100644 index f33d156..0000000 --- a/libsensors/LightSensor.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/* - * 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 <linux/lightsensor.h> - -#include <cutils/log.h> - -#include "LightSensor.h" - -// #define LOG_NDEBUG 0 - -/*****************************************************************************/ - -LightSensor::LightSensor() - : SensorBase(NULL, "light_sensor"), - mEnabled(0), - mInputReader(4), - mHasPendingEvent(false) -{ - mPendingEvent.version = sizeof(sensors_event_t); - mPendingEvent.sensor = ID_L; - mPendingEvent.type = SENSOR_TYPE_LIGHT; - memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data)); - - if (data_fd) { - strcpy(input_sysfs_path, "/sys/class/input/"); - strcat(input_sysfs_path, input_name); - strcat(input_sysfs_path, "/device/"); - input_sysfs_path_len = strlen(input_sysfs_path); - enable(0, 1); - } -} - -LightSensor::~LightSensor() { - if (mEnabled) { - enable(0, 0); - } -} - -int LightSensor::setInitialState() { - struct input_absinfo absinfo; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_LIGHT), &absinfo)) { - // make sure to report an event immediately - mHasPendingEvent = true; - mPendingEvent.light = absinfo.value; - } - return 0; -} - -int LightSensor::setDelay(int32_t handle, int64_t ns) -{ - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "poll_delay"); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - char buf[80]; - sprintf(buf, "%lld", ns); - write(fd, buf, strlen(buf)+1); - close(fd); - return 0; - } - return -1; -} - -int LightSensor::enable(int32_t handle, int en) -{ - int flags = en ? 1 : 0; - if (flags != mEnabled) { - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "enable"); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - char buf[2]; - int err; - buf[1] = 0; - if (flags) { - buf[0] = '1'; - } else { - buf[0] = '0'; - } - err = write(fd, buf, sizeof(buf)); - close(fd); - mEnabled = flags; - return 0; - } - return -1; - } - return 0; -} - -bool LightSensor::hasPendingEvents() const { - return mHasPendingEvent; -} - -int LightSensor::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) { - if (event->code == EVENT_TYPE_LIGHT) { - // Convert adc value to lux assuming: - // I = 10 * log(Ev) uA - // R = 47kOhm - // Max adc value 4095 = 3.3V - // 1/4 of light reaches sensor - mPendingEvent.light = powf(10, event->value * (330.0f / 4095.0f / 47.0f)) * 4; - } - } else if (type == EV_SYN) { - mPendingEvent.timestamp = timevalToNano(event->time); - if (mEnabled) { - *data++ = mPendingEvent; - count--; - numEventReceived++; - } - } else { - ALOGE("LightSensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mInputReader.next(); - } - - return numEventReceived; -} diff --git a/libsensors/LightSensor.h b/libsensors/LightSensor.h deleted file mode 100644 index 85e65d9..0000000 --- a/libsensors/LightSensor.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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_LIGHT_SENSOR_H -#define ANDROID_LIGHT_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 LightSensor : public SensorBase { - int mEnabled; - InputEventCircularReader mInputReader; - sensors_event_t mPendingEvent; - bool mHasPendingEvent; - char input_sysfs_path[PATH_MAX]; - int input_sysfs_path_len; - - float indexToValue(size_t index) const; - int setInitialState(); - -public: - LightSensor(); - virtual ~LightSensor(); - virtual int readEvents(sensors_event_t* data, int count); - virtual bool hasPendingEvents() const; - virtual int setDelay(int32_t handle, int64_t ns); - virtual int enable(int32_t handle, int enabled); -}; - -/*****************************************************************************/ - -#endif // ANDROID_LIGHT_SENSOR_H diff --git a/libsensors/MODULE_LICENSE_APACHE2 b/libsensors/MODULE_LICENSE_APACHE2 deleted file mode 100644 index e69de29..0000000 --- a/libsensors/MODULE_LICENSE_APACHE2 +++ /dev/null diff --git a/libsensors/PressureSensor.cpp b/libsensors/PressureSensor.cpp deleted file mode 100644 index 9b57654..0000000 --- a/libsensors/PressureSensor.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* - * 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 "PressureSensor.h" - -/* - * The BMP driver gives pascal values. - * It needs to be changed into hectoPascal - */ -#define PRESSURE_HECTO (1.0f/100.0f) - -/*****************************************************************************/ - -PressureSensor::PressureSensor() - : SensorBase(NULL, "barometer_sensor"), - mEnabled(0), - mInputReader(4), - mHasPendingEvent(false) -{ - mPendingEvent.version = sizeof(sensors_event_t); - mPendingEvent.sensor = ID_PR; - mPendingEvent.type = SENSOR_TYPE_PRESSURE; - memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data)); - - if (data_fd) { - strcpy(input_sysfs_path, "/sys/class/input/"); - strcat(input_sysfs_path, input_name); - strcat(input_sysfs_path, "/device/"); - input_sysfs_path_len = strlen(input_sysfs_path); - enable(0, 1); - } -} - -PressureSensor::~PressureSensor() { - if (mEnabled) { - enable(0, 0); - } -} - -int PressureSensor::setInitialState() { - struct input_absinfo absinfo; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_PRESSURE), &absinfo)) { - // make sure to report an event immediately - mHasPendingEvent = true; - mPendingEvent.pressure = absinfo.value * PRESSURE_HECTO; - } - return 0; -} - -int PressureSensor::enable(int32_t, int en) { - int flags = en ? 1 : 0; - if (flags != mEnabled) { - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "enable"); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - char buf[2]; - buf[1] = 0; - if (flags) { - buf[0] = '1'; - } else { - buf[0] = '0'; - } - write(fd, buf, sizeof(buf)); - close(fd); - mEnabled = flags; - setInitialState(); - return 0; - } - return -1; - } - return 0; -} - -bool PressureSensor::hasPendingEvents() const { - return mHasPendingEvent; -} - -int PressureSensor::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) { - if (event->code == EVENT_TYPE_PRESSURE) { - mPendingEvent.pressure = event->value * PRESSURE_HECTO; - } - } else if (type == EV_SYN) { - mPendingEvent.timestamp = timevalToNano(event->time); - if (mEnabled) { - *data++ = mPendingEvent; - count--; - numEventReceived++; - } - } else { - ALOGE("PressureSensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mInputReader.next(); - } - - return numEventReceived; -} diff --git a/libsensors/PressureSensor.h b/libsensors/PressureSensor.h deleted file mode 100644 index f2596e4..0000000 --- a/libsensors/PressureSensor.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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_PRESSURE_SENSOR_H -#define ANDROID_PRESSURE_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 PressureSensor : public SensorBase { - int mEnabled; - InputEventCircularReader mInputReader; - sensors_event_t mPendingEvent; - bool mHasPendingEvent; - char input_sysfs_path[PATH_MAX]; - int input_sysfs_path_len; - - int setInitialState(); - -public: - PressureSensor(); - virtual ~PressureSensor(); - virtual int readEvents(sensors_event_t* data, int count); - virtual bool hasPendingEvents() const; - virtual int enable(int32_t handle, int enabled); -}; - -/*****************************************************************************/ - -#endif // ANDROID_PRESSURE_SENSOR_H diff --git a/libsensors/ProximitySensor.cpp b/libsensors/ProximitySensor.cpp deleted file mode 100644 index 879698d..0000000 --- a/libsensors/ProximitySensor.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* - * 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 <linux/capella_cm3602.h> - -#include <cutils/log.h> - -#include "ProximitySensor.h" - -/*****************************************************************************/ - -ProximitySensor::ProximitySensor() - : SensorBase(NULL, "proximity_sensor"), - mEnabled(0), - mInputReader(4), - mHasPendingEvent(false) -{ - mPendingEvent.version = sizeof(sensors_event_t); - mPendingEvent.sensor = ID_P; - mPendingEvent.type = SENSOR_TYPE_PROXIMITY; - memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data)); - - if (data_fd) { - strcpy(input_sysfs_path, "/sys/class/input/"); - strcat(input_sysfs_path, input_name); - strcat(input_sysfs_path, "/device/"); - input_sysfs_path_len = strlen(input_sysfs_path); - enable(0, 1); - } -} - -ProximitySensor::~ProximitySensor() { - if (mEnabled) { - enable(0, 0); - } -} - -int ProximitySensor::setInitialState() { - struct input_absinfo absinfo; - if (!ioctl(data_fd, EVIOCGABS(EVENT_TYPE_PROXIMITY), &absinfo)) { - // make sure to report an event immediately - mHasPendingEvent = true; - mPendingEvent.distance = indexToValue(absinfo.value); - } - return 0; -} - -int ProximitySensor::enable(int32_t, int en) { - int flags = en ? 1 : 0; - if (flags != mEnabled) { - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "enable"); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - char buf[2]; - buf[1] = 0; - if (flags) { - buf[0] = '1'; - } else { - buf[0] = '0'; - } - write(fd, buf, sizeof(buf)); - close(fd); - mEnabled = flags; - setInitialState(); - return 0; - } - return -1; - } - return 0; -} - -bool ProximitySensor::hasPendingEvents() const { - return mHasPendingEvent; -} - -int ProximitySensor::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) { - if (event->code == EVENT_TYPE_PROXIMITY) { - if (event->value != -1) { - // FIXME: not sure why we're getting -1 sometimes - mPendingEvent.distance = indexToValue(event->value); - } - } - } else if (type == EV_SYN) { - mPendingEvent.timestamp = timevalToNano(event->time); - if (mEnabled) { - *data++ = mPendingEvent; - count--; - numEventReceived++; - } - } else { - ALOGE("ProximitySensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mInputReader.next(); - } - - return numEventReceived; -} - -float ProximitySensor::indexToValue(size_t index) const -{ - return index * PROXIMITY_THRESHOLD_GP2A; -} diff --git a/libsensors/ProximitySensor.h b/libsensors/ProximitySensor.h deleted file mode 100644 index 08ea49c..0000000 --- a/libsensors/ProximitySensor.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * 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_PROXIMITY_SENSOR_H -#define ANDROID_PROXIMITY_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 ProximitySensor : public SensorBase { - int mEnabled; - InputEventCircularReader mInputReader; - sensors_event_t mPendingEvent; - bool mHasPendingEvent; - char input_sysfs_path[PATH_MAX]; - int input_sysfs_path_len; - - int setInitialState(); - float indexToValue(size_t index) const; - -public: - ProximitySensor(); - virtual ~ProximitySensor(); - virtual int readEvents(sensors_event_t* data, int count); - virtual bool hasPendingEvents() const; - virtual int enable(int32_t handle, int enabled); -}; - -/*****************************************************************************/ - -#endif // ANDROID_PROXIMITY_SENSOR_H diff --git a/libsensors/SensorBase.cpp b/libsensors/SensorBase.cpp deleted file mode 100644 index 79b1ee2..0000000 --- a/libsensors/SensorBase.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* - * 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::getFd() const { - if (!data_name) { - return dev_fd; - } - return data_fd; -} - -int SensorBase::setDelay(int32_t handle, int64_t ns) { - 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/libsensors/SensorBase.h b/libsensors/SensorBase.h deleted file mode 100644 index bb4d055..0000000 --- a/libsensors/SensorBase.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * 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(); - -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 int enable(int32_t handle, int enabled) = 0; -}; - -/*****************************************************************************/ - -#endif // ANDROID_SENSOR_BASE_H diff --git a/libsensors/ak8973b.h b/libsensors/ak8973b.h deleted file mode 100644 index 9b7ab60..0000000 --- a/libsensors/ak8973b.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Definitions for akm8973 compass chip. - */ -#ifndef AKM8973_H -#define AKM8973_H - -#include <linux/ioctl.h> - -#define AKM8973_I2C_NAME "ak8973b" - -#define AKMIO 0xA1 - -/* IOCTLs for AKM library */ -#define ECS_IOCTL_WRITE _IOW(AKMIO, 0x01, char*) -#define ECS_IOCTL_READ _IOWR(AKMIO, 0x02, char*) -#define ECS_IOCTL_RESET _IO(AKMIO, 0x03) -#define ECS_IOCTL_SET_MODE _IOW(AKMIO, 0x04, short) -#define ECS_IOCTL_GETDATA _IOR(AKMIO, 0x05, char[SENSOR_DATA_SIZE]) -#define ECS_IOCTL_SET_YPR _IOW(AKMIO, 0x06, short[12]) -#define ECS_IOCTL_GET_OPEN_STATUS _IOR(AKMIO, 0x07, int) -#define ECS_IOCTL_GET_CLOSE_STATUS _IOR(AKMIO, 0x08, int) -#define ECS_IOCTL_GET_DELAY _IOR(AKMIO, 0x30, int64_t) -#define ECS_IOCTL_GET_PROJECT_NAME _IOR(AKMIO, 0x0D, char[64]) -#define ECS_IOCTL_GET_MATRIX _IOR(AKMIO, 0x0E, short [4][3][3]) - -/* IOCTLs for APPs */ -#define ECS_IOCTL_APP_SET_MODE _IOW(AKMIO, 0x10, short) -#define ECS_IOCTL_APP_SET_MFLAG _IOW(AKMIO, 0x11, short) -#define ECS_IOCTL_APP_GET_MFLAG _IOW(AKMIO, 0x12, short) -#define ECS_IOCTL_APP_SET_AFLAG _IOW(AKMIO, 0x13, short) -#define ECS_IOCTL_APP_GET_AFLAG _IOR(AKMIO, 0x14, short) -#define ECS_IOCTL_APP_SET_TFLAG _IOR(AKMIO, 0x15, short) -#define ECS_IOCTL_APP_GET_TFLAG _IOR(AKMIO, 0x16, short) -#define ECS_IOCTL_APP_RESET_PEDOMETER _IO(AKMIO, 0x17) -#define ECS_IOCTL_APP_SET_DELAY _IOW(AKMIO, 0x18, int64_t) -#define ECS_IOCTL_APP_GET_DELAY ECS_IOCTL_GET_DELAY - -/* Set raw magnetic vector flag */ -#define ECS_IOCTL_APP_SET_MVFLAG _IOW(AKMIO, 0x19, short) - -/* Get raw magnetic vector flag */ -#define ECS_IOCTL_APP_GET_MVFLAG _IOR(AKMIO, 0x1A, short) - -struct akm8973_platform_data { - short layouts[4][3][3]; - char project_name[64]; - int gpio_RST; - int gpio_INT; -}; - -#endif diff --git a/libsensors/sensors.cpp b/libsensors/sensors.cpp deleted file mode 100644 index 8f43b08..0000000 --- a/libsensors/sensors.cpp +++ /dev/null @@ -1,341 +0,0 @@ -/* - * 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" - -#include "LightSensor.h" -#include "ProximitySensor.h" -#include "AkmSensor.h" -#include "GyroSensor.h" -#include "PressureSensor.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_LIGHT (1<<ID_L) -#define SENSORS_PROXIMITY (1<<ID_P) -#define SENSORS_GYROSCOPE (1<<ID_GY) -#define SENSORS_PRESSURE (1<<ID_PR) - -#define SENSORS_ACCELERATION_HANDLE 0 -#define SENSORS_MAGNETIC_FIELD_HANDLE 1 -#define SENSORS_ORIENTATION_HANDLE 2 -#define SENSORS_LIGHT_HANDLE 3 -#define SENSORS_PROXIMITY_HANDLE 4 -#define SENSORS_GYROSCOPE_HANDLE 5 -#define SENSORS_PRESSURE_HANDLE 6 - -#define AKM_FTRACE 0 -#define AKM_DEBUG 0 -#define AKM_DATA 0 - -/*****************************************************************************/ - -/* The SENSORS Module */ -static const struct sensor_t sSensorList[] = { - { "KR3DM 3-axis Accelerometer", - "STMicroelectronics", - 1, SENSORS_ACCELERATION_HANDLE, - SENSOR_TYPE_ACCELEROMETER, RANGE_A, CONVERT_A, 0.23f, 20000, 0, 0, { } }, - { "AK8975 3-axis Magnetic field sensor", - "Asahi Kasei Microdevices", - 1, SENSORS_MAGNETIC_FIELD_HANDLE, - SENSOR_TYPE_MAGNETIC_FIELD, 2000.0f, CONVERT_M, 6.8f, 16667, 0, 0, { } }, - { "AK8973 Orientation sensor", - "Asahi Kasei Microdevices", - 1, SENSORS_ORIENTATION_HANDLE, - SENSOR_TYPE_ORIENTATION, 360.0f, CONVERT_O, 7.8f, 16667, 0, 0, { } }, - { "GP2A Light sensor", - "Sharp", - 1, SENSORS_LIGHT_HANDLE, - SENSOR_TYPE_LIGHT, 10240.0f, 1.0f, 0.75f, 0, 0, 0, { } }, - { "GP2A Proximity sensor", - "Sharp", - 1, SENSORS_PROXIMITY_HANDLE, - SENSOR_TYPE_PROXIMITY, 5.0f, 5.0f, 0.75f, 0, 0, 0, { } }, - { "K3G Gyroscope sensor", - "STMicroelectronics", - 1, SENSORS_GYROSCOPE_HANDLE, - SENSOR_TYPE_GYROSCOPE, RANGE_GYRO, CONVERT_GYRO, 6.1f, 1190, 0, 0, { } }, - { "BMP180 Pressure sensor", - "Bosch", - 1, SENSORS_PRESSURE_HANDLE, - SENSOR_TYPE_PRESSURE, 1100.0f, 0.01f, 0.67f, 20000, 0, 0, { } }, -}; - - -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: "Samsung Sensor module", - author: "Samsung Electronic Company", - 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 pollEvents(sensors_event_t* data, int count); - -private: - enum { - light = 0, - proximity = 1, - akm = 2, - gyro = 3, - pressure = 4, - numSensorDrivers, - numFds, - }; - - static const size_t wake = numFds - 1; - static const char WAKE_MESSAGE = 'W'; - struct pollfd mPollFds[numFds]; - int mWritePipeFd; - SensorBase* mSensors[numSensorDrivers]; - - int handleToDriver(int handle) const { - switch (handle) { - case ID_A: - case ID_M: - case ID_O: - return akm; - case ID_P: - return proximity; - case ID_L: - return light; - case ID_GY: - return gyro; - case ID_PR: - return pressure; - } - return -EINVAL; - } -}; - -/*****************************************************************************/ - -sensors_poll_context_t::sensors_poll_context_t() -{ - mSensors[light] = new LightSensor(); - mPollFds[light].fd = mSensors[light]->getFd(); - mPollFds[light].events = POLLIN; - mPollFds[light].revents = 0; - - mSensors[proximity] = new ProximitySensor(); - mPollFds[proximity].fd = mSensors[proximity]->getFd(); - mPollFds[proximity].events = POLLIN; - mPollFds[proximity].revents = 0; - - mSensors[akm] = new AkmSensor(); - mPollFds[akm].fd = mSensors[akm]->getFd(); - mPollFds[akm].events = POLLIN; - mPollFds[akm].revents = 0; - - mSensors[gyro] = new GyroSensor(); - mPollFds[gyro].fd = mSensors[gyro]->getFd(); - mPollFds[gyro].events = POLLIN; - mPollFds[gyro].revents = 0; - - mSensors[pressure] = new PressureSensor(); - mPollFds[pressure].fd = mSensors[pressure]->getFd(); - mPollFds[pressure].events = POLLIN; - mPollFds[pressure].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::activate(int handle, int enabled) { - int index = handleToDriver(handle); - if (index < 0) return index; - int err = mSensors[index]->enable(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) { - - int index = handleToDriver(handle); - if (index < 0) return index; - return mSensors[index]->setDelay(handle, ns); -} - -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; - } - 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/libsensors/sensors.h b/libsensors/sensors.h deleted file mode 100644 index 1cfca89..0000000 --- a/libsensors/sensors.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * 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) -#define ID_L (3) -#define ID_P (4) -#define ID_GY (5) -#define ID_PR (6) - -/*****************************************************************************/ - -/* - * The SENSORS Module - */ - -/* the GP2A is a binary proximity sensor that triggers around 5 cm on - * this hardware */ -#define PROXIMITY_THRESHOLD_GP2A 5.0f - -/*****************************************************************************/ - -#define AKM_DEVICE_NAME "/dev/akm8975" - -// for akm8975 -#define EVENT_TYPE_ACCEL_X ABS_Y //1 -#define EVENT_TYPE_ACCEL_Y ABS_X //0 -#define EVENT_TYPE_ACCEL_Z ABS_Z //2 -//#define EVENT_TYPE_ACCEL_STATUS ABS_WHEEL //8 - -#define EVENT_TYPE_YAW ABS_RX //3 -#define EVENT_TYPE_PITCH ABS_RY //4 -#define EVENT_TYPE_ROLL ABS_RZ //5 -#define EVENT_TYPE_ORIENT_STATUS ABS_WHEEL //8 - -#define EVENT_TYPE_MAGV_X ABS_THROTTLE // 7 -#define EVENT_TYPE_MAGV_Y ABS_RUDDER // 6 -#define EVENT_TYPE_MAGV_Z ABS_GAS // 9 - -#define EVENT_TYPE_TEMPERATURE ABS_THROTTLE -#define EVENT_TYPE_STEP_COUNT ABS_GAS -#define EVENT_TYPE_PROXIMITY ABS_DISTANCE -#define EVENT_TYPE_PRESSURE ABS_PRESSURE -#define EVENT_TYPE_LIGHT ABS_MISC - -#define EVENT_TYPE_GYRO_X REL_RY -#define EVENT_TYPE_GYRO_Y REL_RX -#define EVENT_TYPE_GYRO_Z REL_RZ - -// 90 LSB = 1G for KR3DM -#define LSB (90.0f) -#define NUMOFACCDATA (8.0f) - -// conversion of acceleration data to SI units (m/s^2) -#define RANGE_A (2*GRAVITY_EARTH) -#define CONVERT_A (GRAVITY_EARTH / LSB / NUMOFACCDATA) -#define CONVERT_A_X (CONVERT_A) -#define CONVERT_A_Y (-CONVERT_A) -#define CONVERT_A_Z (-CONVERT_A) - -// conversion of magnetic data to uT units -#define CONVERT_M (1.0f/16.0f) -#define CONVERT_M_X (-CONVERT_M) -#define CONVERT_M_Y (-CONVERT_M) -#define CONVERT_M_Z (CONVERT_M) - -/* conversion of orientation data to degree units */ -#define CONVERT_O (1.0f/1000.0f) -#define CONVERT_O_A (CONVERT_O) -#define CONVERT_O_P (CONVERT_O) -#define CONVERT_O_R (CONVERT_O) - -// conversion of gyro data to SI units (radian/sec) -#define RANGE_GYRO (500.0f*(float)M_PI/180.0f) -#define CONVERT_GYRO ((70.0f / 4000.0f) * ((float)M_PI / 180.0f)) -#define CONVERT_GYRO_X (-CONVERT_GYRO) -#define CONVERT_GYRO_Y (-CONVERT_GYRO) -#define CONVERT_GYRO_Z (-CONVERT_GYRO) - -#define SENSOR_STATE_MASK (0x7FFF) - -/*****************************************************************************/ - -__END_DECLS - -#endif // ANDROID_SENSORS_H @@ -35,7 +35,7 @@ PRODUCT_PROPERTY_OVERRIDES += \ # Sensors PRODUCT_PACKAGES += \ - sensors.exynos4 + sensors.smdk4210 # Keylayout PRODUCT_COPY_FILES += \ diff --git a/sensors/Android.mk b/sensors/Android.mk new file mode 100644 index 0000000..026bfa6 --- /dev/null +++ b/sensors/Android.mk @@ -0,0 +1,49 @@ +# Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see <http://www.gnu.org/licenses/>. + +LOCAL_PATH := $(call my-dir) + +include $(CLEAR_VARS) + +LOCAL_SRC_FILES := \ + smdk4210_sensors.c \ + input.c \ + orientation.c \ + akm8975.c \ + akmdfs/AKFS_APIs_8975/AKFS_AK8975.c \ + akmdfs/AKFS_APIs_8975/AKFS_AOC.c \ + akmdfs/AKFS_APIs_8975/AKFS_Device.c \ + akmdfs/AKFS_APIs_8975/AKFS_Direction.c \ + akmdfs/AKFS_APIs_8975/AKFS_VNorm.c \ + akmdfs/AKFS_FileIO.c \ + k3dh.c \ + gp2a_light.c \ + gp2a_proximity.c \ + k3g.c \ + bmp180.c + +LOCAL_C_INCLUDES := \ + $(LOCAL_PATH)/akmdfs \ + $(LOCAL_PATH)/akmdfs/AKFS_APIs_8975 \ + $(LOCAL_PATH) + +LOCAL_SHARED_LIBRARIES := libutils libcutils liblog libhardware +LOCAL_PRELINK_MODULE := false + +LOCAL_MODULE := sensors.smdk4210 +LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw +LOCAL_MODULE_TAGS := optional + +include $(BUILD_SHARED_LIBRARY) diff --git a/sensors/ak8975-reg.h b/sensors/ak8975-reg.h new file mode 100644 index 0000000..1a78a27 --- /dev/null +++ b/sensors/ak8975-reg.h @@ -0,0 +1,48 @@ +/* linux/drivers/misc/ak8975-reg.h + * + * Copyright (c) 2010 Samsung Electronics Co., Ltd. + * http://www.samsung.com/ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. +*/ +#ifndef __AK8975_REG__ +#define __AK8975_REG__ + +/* Compass device dependent definition */ +#define AK8975_MODE_SNG_MEASURE 0x01 +#define AK8975_MODE_SELF_TEST 0x08 +#define AK8975_MODE_FUSE_ACCESS 0x0F +#define AK8975_MODE_POWER_DOWN 0x00 + +/* Rx buffer size. i.e ST,TMPS,H1X,H1Y,H1Z*/ +#define SENSOR_DATA_SIZE 8 + +/* Read/Write buffer size.*/ +#define RWBUF_SIZE 16 + +/* AK8975 register address */ +#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 + +/* AK8975 fuse-rom address */ +#define AK8975_FUSE_ASAX 0x10 +#define AK8975_FUSE_ASAY 0x11 +#define AK8975_FUSE_ASAZ 0x12 + +#endif /* __AK8975_REG__ */ diff --git a/sensors/ak8975.h b/sensors/ak8975.h new file mode 100644 index 0000000..79a5de4 --- /dev/null +++ b/sensors/ak8975.h @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2010 Samsung Electronics. All rights reserved. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ +#ifndef AKM8975_H +#define AKM8975_H + +#include <linux/ioctl.h> + +#define AKM8975_I2C_NAME "ak8975" + +/* Rx buffer size, i.e from ST1 to ST2 */ +#define SENSOR_DATA_SIZE 8 +#define AKMIO 0xA1 + +/* IOCTLs for AKM library */ +/* WRITE and READ sizes don't include data. On WRITE, the first value is data + * size plus one and the second value is the register address. On READ + * the first value is the data size and second value is the register + * address and the data is written back into the buffer starting at + * the second byte (the length is unchanged). + */ +#define ECS_IOCTL_WRITE _IOW(AKMIO, 0x01, char*) +#define ECS_IOCTL_READ _IOWR(AKMIO, 0x02, char*) +#define ECS_IOCTL_RESET _IO(AKMIO, 0x03) +#define ECS_IOCTL_SET_MODE _IOW(AKMIO, 0x04, short) +#define ECS_IOCTL_GETDATA _IOR(AKMIO, 0x05, \ + char[SENSOR_DATA_SIZE]) +#define ECS_IOCTL_SET_YPR _IOW(AKMIO, 0x06, short[12]) +#define ECS_IOCTL_GET_OPEN_STATUS _IOR(AKMIO, 0x07, int) +#define ECS_IOCTL_GET_CLOSE_STATUS _IOR(AKMIO, 0x08, int) +#define ECS_IOCTL_GET_DELAY _IOR(AKMIO, 0x30, int64_t) +#define ECS_IOCTL_GET_PROJECT_NAME _IOR(AKMIO, 0x0D, char[64]) +#define ECS_IOCTL_GET_MATRIX _IOR(AKMIO, 0x0E, short[4][3][3]) + +/* IOCTLs for APPs */ +#define ECS_IOCTL_APP_SET_MODE _IOW(AKMIO, 0x10, short) +#define ECS_IOCTL_APP_SET_MFLAG _IOW(AKMIO, 0x11, short) +#define ECS_IOCTL_APP_GET_MFLAG _IOR(AKMIO, 0x12, short) +#define ECS_IOCTL_APP_SET_AFLAG _IOW(AKMIO, 0x13, short) +#define ECS_IOCTL_APP_GET_AFLAG _IOR(AKMIO, 0x14, short) +#define ECS_IOCTL_APP_SET_TFLAG _IOW(AKMIO, 0x15, short) +#define ECS_IOCTL_APP_GET_TFLAG _IOR(AKMIO, 0x16, short) +#define ECS_IOCTL_APP_RESET_PEDOMETER _IO(AKMIO, 0x17) +#define ECS_IOCTL_APP_SET_DELAY _IOW(AKMIO, 0x18, int64_t) +#define ECS_IOCTL_APP_GET_DELAY ECS_IOCTL_GET_DELAY + +/* Set raw magnetic vector flag */ +#define ECS_IOCTL_APP_SET_MVFLAG _IOW(AKMIO, 0x19, short) + +/* Get raw magnetic vector flag */ +#define ECS_IOCTL_APP_GET_MVFLAG _IOR(AKMIO, 0x1A, short) + +#ifdef __KERNEL__ +struct akm8975_platform_data { + int gpio_data_ready_int; +}; +#endif + +#endif diff --git a/sensors/akm8975.c b/sensors/akm8975.c new file mode 100644 index 0000000..c46b823 --- /dev/null +++ b/sensors/akm8975.c @@ -0,0 +1,596 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" +#include "ak8975.h" +#include "ak8975-reg.h" + +#include <AKFS_Compass.h> +#include <AKFS_FileIO.h> + +#define AKFS_CONFIG_PATH "/data/misc/akfs.txt" +#define AKFS_PAT PAT3 + +struct akm8975_data { + struct smdk4210_sensors_handlers *orientation_sensor; + + AK8975PRMS akfs_params; + sensors_vec_t magnetic; + + long int delay; + int device_fd; + int uinput_fd; + + pthread_t thread; + pthread_mutex_t mutex; + int thread_continue; +}; + +int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_data) +{ + AK8975PRMS *params; + int rc; + + if (akm8975_data == NULL || magnetic_data == NULL) + return -EINVAL; + + params = &akm8975_data->akfs_params; + + // Decomposition + // Sensitivity adjustment, i.e. multiply ASA, is done in this function. + rc = AKFS_DecompAK8975(magnetic_data, 1, ¶ms->mi_asa, AKFS_HDATA_SIZE, params->mfv_hdata); + if (rc == AKFS_ERROR) { + ALOGE("Failed to decomp!"); + return -1; + } + + // Adjust coordination + rc = AKFS_Rotate(params->m_hpat, ¶ms->mfv_hdata[0]); + if (rc == AKFS_ERROR) { + ALOGE("Failed to rotate!"); + return -1; + } + + // AOC for magnetometer + // Offset estimation is done in this function + AKFS_AOC(¶ms->m_aocv, params->mfv_hdata, ¶ms->mfv_ho); + + // Subtract offset + // Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. + rc = AKFS_VbNorm(AKFS_HDATA_SIZE, params->mfv_hdata, 1, + ¶ms->mfv_ho, ¶ms->mfv_hs, AK8975_HSENSE_TARGET, + AKFS_HDATA_SIZE, params->mfv_hvbuf); + if (rc == AKFS_ERROR) { + ALOGE("Failed to normalize!"); + return -1; + } + + // Averaging + rc = AKFS_VbAve(AKFS_HDATA_SIZE, params->mfv_hvbuf, CSPEC_HNAVE_V, ¶ms->mfv_hvec); + if (rc == AKFS_ERROR) { + ALOGE("Failed to average!"); + return -1; + } + + akm8975_data->magnetic.x = params->mfv_hvec.u.x; + akm8975_data->magnetic.y = params->mfv_hvec.u.y; + akm8975_data->magnetic.z = params->mfv_hvec.u.z; + + return 0; +} + +int akfs_init(struct akm8975_data *akm8975_data, char *asa, AKFS_PATNO pat) +{ + AK8975PRMS *params; + + if (akm8975_data == NULL || asa == NULL) + return -EINVAL; + + params = &akm8975_data->akfs_params; + + memset(params, 0, sizeof(AK8975PRMS)); + + // Sensitivity + params->mfv_hs.u.x = AK8975_HSENSE_DEFAULT; + params->mfv_hs.u.y = AK8975_HSENSE_DEFAULT; + params->mfv_hs.u.z = AK8975_HSENSE_DEFAULT; + params->mfv_as.u.x = AK8975_ASENSE_DEFAULT; + params->mfv_as.u.y = AK8975_ASENSE_DEFAULT; + params->mfv_as.u.z = AK8975_ASENSE_DEFAULT; + + // Initialize variables that initial value is not 0. + params->mi_hnaveV = CSPEC_HNAVE_V; + params->mi_hnaveD = CSPEC_HNAVE_D; + params->mi_anaveV = CSPEC_ANAVE_V; + params->mi_anaveD = CSPEC_ANAVE_D; + + // Copy ASA values + params->mi_asa.u.x = asa[0]; + params->mi_asa.u.y = asa[1]; + params->mi_asa.u.z = asa[2]; + + // Copy layout pattern + params->m_hpat = pat; + + return 0; +} + +void *akm8975_thread(void *thread_data) +{ + struct smdk4210_sensors_handlers *handlers = NULL; + struct akm8975_data *data = NULL; + struct input_event event; + struct timeval time; + char i2c_data[SENSOR_DATA_SIZE] = { 0 }; + short magnetic_data[3]; + short mode; + long int before, after; + int diff; + int device_fd; + int uinput_fd; + int rc; + + if (thread_data == NULL) + return NULL; + + handlers = (struct smdk4210_sensors_handlers *) thread_data; + if (handlers->data == NULL) + return NULL; + + data = (struct akm8975_data *) handlers->data; + + device_fd = data->device_fd; + if (device_fd < 0) + return NULL; + + uinput_fd = data->uinput_fd; + if (uinput_fd < 0) + return NULL; + + while (data->thread_continue) { + pthread_mutex_lock(&data->mutex); + if (!data->thread_continue) + break; + + while (handlers->activated) { + gettimeofday(&time, NULL); + before = timestamp(&time); + + mode = AK8975_MODE_SNG_MEASURE; + rc = ioctl(device_fd, ECS_IOCTL_SET_MODE, &mode); + if (rc < 0) { + ALOGE("%s: Unable to set akm8975 mode", __func__); + return NULL; + } + + memset(&i2c_data, 0, sizeof(i2c_data)); + rc = ioctl(device_fd, ECS_IOCTL_GETDATA, &i2c_data); + if (rc < 0) { + ALOGE("%s: Unable to get akm8975 data", __func__); + return NULL; + } + + if (!(i2c_data[0] & 0x01)) { + ALOGE("%s: akm8975 data is not ready", __func__); + continue; + } + + if (i2c_data[7] & (1 << 2) || i2c_data[7] & (1 << 3)) { + ALOGE("%s: akm8975 data read error or overflow", __func__); + continue; + } + + magnetic_data[0] = (short) (i2c_data[2] << 8) | (i2c_data[1]); + magnetic_data[1] = (short) (i2c_data[4] << 8) | (i2c_data[3]); + magnetic_data[2] = (short) (i2c_data[6] << 8) | (i2c_data[5]); + + rc = akfs_get_magnetic_field(data, (short *) &magnetic_data); + if (rc < 0) { + ALOGE("%s: Unable to get AKFS magnetic field", __func__); + continue; + } + + input_event_set(&event, EV_REL, REL_X, (int) (data->magnetic.x * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_Y, (int) (data->magnetic.y * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_Z, (int) (data->magnetic.z * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_SYN, 0, 0); + write(uinput_fd, &event, sizeof(event)); + + gettimeofday(&time, NULL); + after = timestamp(&time); + + diff = (int) (data->delay - (after - before)) / 1000; + if (diff <= 0) + continue; + + usleep(diff); + } + } + return NULL; +} + +int akm8975_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct akm8975_data *data = NULL; + pthread_attr_t thread_attr; + char i2c_data[RWBUF_SIZE] = { 0 }; + short mode; + int device_fd = -1; + int uinput_fd = -1; + int input_fd = -1; + int rc; + int i; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL || device == NULL) + return -EINVAL; + + data = (struct akm8975_data *) calloc(1, sizeof(struct akm8975_data)); + + for (i = 0; i < device->handlers_count; i++) { + if (device->handlers[i] == NULL) + continue; + + if (device->handlers[i]->handle == SENSOR_TYPE_ORIENTATION) + data->orientation_sensor = device->handlers[i]; + } + + device_fd = open("/dev/akm8975", O_RDONLY); + if (device_fd < 0) { + ALOGE("%s: Unable to open device", __func__); + goto error; + } + + mode = AK8975_MODE_POWER_DOWN; + rc = ioctl(device_fd, ECS_IOCTL_SET_MODE, &mode); + if (rc < 0) { + ALOGE("%s: Unable to set akm8975 mode", __func__); + goto error; + } + + mode = AK8975_MODE_FUSE_ACCESS; + rc = ioctl(device_fd, ECS_IOCTL_SET_MODE, &mode); + if (rc < 0) { + ALOGE("%s: Unable to set akm8975 mode", __func__); + goto error; + } + + i2c_data[0] = 3; + i2c_data[1] = AK8975_FUSE_ASAX; + rc = ioctl(device_fd, ECS_IOCTL_READ, &i2c_data); + if (rc < 0) { + ALOGE("%s: Unable to read akm8975 FUSE data", __func__); + goto error; + } + + ALOGD("AKM8975 ASA (Sensitivity Adjustment) values are: (%d, %d, %d)", + i2c_data[1], i2c_data[2], i2c_data[3]); + + rc = akfs_init(data, &i2c_data[1], AKFS_PAT); + if (rc < 0) { + ALOGE("%s: Unable to init AKFS", __func__); + goto error; + } + + i2c_data[0] = 1; + i2c_data[1] = AK8975_REG_WIA; + rc = ioctl(device_fd, ECS_IOCTL_READ, &i2c_data); + if (rc < 0) { + ALOGE("%s: Unable to read akm8975 FUSE data", __func__); + goto error; + } + + ALOGD("AKM8975 WIA (Device ID) value is: 0x%x", i2c_data[1]); + + mode = AK8975_MODE_POWER_DOWN; + rc = ioctl(device_fd, ECS_IOCTL_SET_MODE, &mode); + if (rc < 0) { + ALOGE("%s: Unable to set akm8975 mode", __func__); + goto error; + } + + uinput_fd = uinput_rel_create("magnetic"); + if (uinput_fd < 0) { + ALOGD("%s: Unable to create uinput", __func__); + goto error; + } + + input_fd = input_open("magnetic"); + if (input_fd < 0) { + ALOGE("%s: Unable to open magnetic input", __func__); + goto error; + } + + data->thread_continue = 1; + + pthread_mutex_init(&data->mutex, NULL); + pthread_mutex_lock(&data->mutex); + + pthread_attr_init(&thread_attr); + pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED); + + rc = pthread_create(&data->thread, &thread_attr, akm8975_thread, (void *) handlers); + if (rc < 0) { + ALOGE("%s: Unable to create akm8975 thread", __func__); + pthread_mutex_destroy(&data->mutex); + goto error; + } + + data->device_fd = device_fd; + data->uinput_fd = uinput_fd; + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (uinput_fd >= 0) + close(uinput_fd); + + if (input_fd >= 0) + close(input_fd); + + if (device_fd >= 0) + close(device_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int akm8975_deinit(struct smdk4210_sensors_handlers *handlers) +{ + struct akm8975_data *data = NULL; + short mode; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct akm8975_data *) handlers->data; + + handlers->activated = 0; + data->thread_continue = 0; + pthread_mutex_unlock(&data->mutex); + + pthread_mutex_destroy(&data->mutex); + + if (data->uinput_fd >= 0) { + uinput_destroy(data->uinput_fd); + close(data->uinput_fd); + } + data->uinput_fd = -1; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + mode = AK8975_MODE_POWER_DOWN; + rc = ioctl(data->device_fd, ECS_IOCTL_SET_MODE, &mode); + if (rc < 0) + ALOGE("%s: Unable to set akm8975 mode", __func__); + + if (data->device_fd >= 0) + close(data->device_fd); + data->device_fd = -1; + + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int akm8975_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct akm8975_data *data; + AK8975PRMS *akfs_params; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct akm8975_data *) handlers->data; + akfs_params = &data->akfs_params; + + // Read settings from a file + rc = AKFS_LoadParameters(akfs_params, AKFS_CONFIG_PATH); + if (rc != AKM_SUCCESS) + ALOGE("%s: Unable to read AKFS parameters", __func__); + + // Initialize buffer + AKFS_InitBuffer(AKFS_HDATA_SIZE, akfs_params->mfv_hdata); + AKFS_InitBuffer(AKFS_HDATA_SIZE, akfs_params->mfv_hvbuf); + AKFS_InitBuffer(AKFS_ADATA_SIZE, akfs_params->mfv_adata); + AKFS_InitBuffer(AKFS_ADATA_SIZE, akfs_params->mfv_avbuf); + + // Initialize for AOC + AKFS_InitAOC(&akfs_params->m_aocv); + // Initialize magnetic status + akfs_params->mi_hstatus = 0; + + handlers->activated = 1; + pthread_mutex_unlock(&data->mutex); + + return 0; +} + +int akm8975_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct akm8975_data *data; + AK8975PRMS *akfs_params; + int device_fd; + short mode; + int empty; + int rc; + int i; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct akm8975_data *) handlers->data; + akfs_params = &data->akfs_params; + + device_fd = data->device_fd; + if (device_fd < 0) + return -1; + + empty = 1; + + for (i = 0; i < 3; i++) { + if (akfs_params->mfv_ho.v[i] != 0) { + empty = 0; + break; + } + } + + if (!empty) { + // Write settings to a file + rc = AKFS_SaveParameters(akfs_params, AKFS_CONFIG_PATH); + if (rc != AKM_SUCCESS) + ALOGE("%s: Unable to write AKFS parameters", __func__); + } + + mode = AK8975_MODE_POWER_DOWN; + rc = ioctl(device_fd, ECS_IOCTL_SET_MODE, &mode); + if (rc < 0) + ALOGE("%s: Unable to set akm8975 mode", __func__); + + handlers->activated = 0; + + return 0; +} + +int akm8975_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +{ + struct akm8975_data *data; + + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct akm8975_data *) handlers->data; + + data->delay = delay; + + return 0; +} + +float akm8975_convert(int value) +{ + return (float) value / 1000.0f; +} + +int akm8975_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct akm8975_data *data; + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || handlers->data == NULL || event == NULL) + return -EINVAL; + + data = (struct akm8975_data *) handlers->data; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -1; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_REL) { + switch (input_event.code) { + case REL_X: + event->magnetic.x = akm8975_convert(input_event.value); + break; + case REL_Y: + event->magnetic.y = akm8975_convert(input_event.value); + break; + case REL_Z: + event->magnetic.z = akm8975_convert(input_event.value); + break; + default: + continue; + } + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT) + event->timestamp = input_timestamp(&input_event); + } + } while (input_event.type != EV_SYN); + + if (data->orientation_sensor != NULL) + orientation_fill(data->orientation_sensor, NULL, &event->magnetic); + + return 0; +} + +struct smdk4210_sensors_handlers akm8975 = { + .name = "AKM8975", + .handle = SENSOR_TYPE_MAGNETIC_FIELD, + .init = akm8975_init, + .deinit = akm8975_deinit, + .activate = akm8975_activate, + .deactivate = akm8975_deactivate, + .set_delay = akm8975_set_delay, + .get_data = akm8975_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/akmdfs/AK8975Driver.c b/sensors/akmdfs/AK8975Driver.c new file mode 100644 index 0000000..003c8d2 --- /dev/null +++ b/sensors/akmdfs/AK8975Driver.c @@ -0,0 +1,318 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AK8975Driver.h b/sensors/akmdfs/AK8975Driver.h new file mode 100644 index 0000000..731210d --- /dev/null +++ b/sensors/akmdfs/AK8975Driver.h @@ -0,0 +1,102 @@ +/****************************************************************************** + * + * 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 <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/sensors/akmdfs/AKFS_APIs.c b/sensors/akmdfs/AKFS_APIs.c new file mode 100644 index 0000000..ace9bc1 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs.c @@ -0,0 +1,389 @@ +/****************************************************************************** + * + * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ******************************************************************************/ +#include "AKFS_Common.h" +#include "AKFS_Disp.h" +#include "AKFS_FileIO.h" +#include "AKFS_APIs.h" + +#ifdef WIN32 +#include "AK8975_LinuxDriver.h" +#endif + +static AK8975PRMS g_prms; + +/*! + Initialize library. At first, 0 is set to all parameters. After that, some + parameters, which should not be 0, are set to specific value. Some of initial + values can be customized by editing the file \c "AKFS_CSpec.h". + @return The return value is #AKM_SUCCESS. + @param[in] hpat Specify a layout pattern number. The number is determined + according to the mount orientation of the magnetometer. + @param[in] regs[3] Specify the ASA values which are read out from + fuse ROM. regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ. + */ +int16 AKFS_Init( + const AKFS_PATNO hpat, + const uint8 regs[] +) +{ + AKMDATA(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n", + __FUNCTION__, hpat, regs[0], regs[1], regs[2]); + + /* Set 0 to the AK8975 structure. */ + memset(&g_prms, 0, sizeof(AK8975PRMS)); + + /* Sensitivity */ + g_prms.mfv_hs.u.x = AK8975_HSENSE_DEFAULT; + g_prms.mfv_hs.u.y = AK8975_HSENSE_DEFAULT; + g_prms.mfv_hs.u.z = AK8975_HSENSE_DEFAULT; + g_prms.mfv_as.u.x = AK8975_ASENSE_DEFAULT; + g_prms.mfv_as.u.y = AK8975_ASENSE_DEFAULT; + g_prms.mfv_as.u.z = AK8975_ASENSE_DEFAULT; + + /* Initialize variables that initial value is not 0. */ + g_prms.mi_hnaveV = CSPEC_HNAVE_V; + g_prms.mi_hnaveD = CSPEC_HNAVE_D; + g_prms.mi_anaveV = CSPEC_ANAVE_V; + g_prms.mi_anaveD = CSPEC_ANAVE_D; + + /* Copy ASA values */ + g_prms.mi_asa.u.x = regs[0]; + g_prms.mi_asa.u.y = regs[1]; + g_prms.mi_asa.u.z = regs[2]; + + /* Copy layout pattern */ + g_prms.m_hpat = hpat; + + return AKM_SUCCESS; +} + +/*! + Release resources. This function is for future expansion. + @return The return value is #AKM_SUCCESS. + */ +int16 AKFS_Release(void) +{ + return AKM_SUCCESS; +} + +/* + This function is called just before a measurement sequence starts. + This function reads parameters from file, then initializes algorithm + parameters. + @return The return value is #AKM_SUCCESS. + @param[in] path Specify a path to the settings file. + */ +int16 AKFS_Start( + const char* path +) +{ + AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); + + /* Read setting files from a file */ + if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) { + AKMERROR_STR("AKFS_Load"); + } + + /* Initialize buffer */ + AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata); + AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf); + AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata); + AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf); + + /* Initialize for AOC */ + AKFS_InitAOC(&g_prms.m_aocv); + /* Initialize magnetic status */ + g_prms.mi_hstatus = 0; + + return AKM_SUCCESS; +} + +/*! + This function is called when a measurement sequence is done. + This fucntion writes parameters to file. + @return The return value is #AKM_SUCCESS. + @param[in] path Specify a path to the settings file. + */ +int16 AKFS_Stop( + const char* path +) +{ + AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); + + /* Write setting files to a file */ + if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) { + AKMERROR_STR("AKFS_Save"); + } + + return AKM_SUCCESS; +} + +/*! + This function is called when new magnetometer data is available. The output + vector format and coordination system follow the Android definition. + @return The return value is #AKM_SUCCESS. + Otherwise the return value is #AKM_FAIL. + @param[in] mag A set of measurement data from magnetometer. X axis value + should be in mag[0], Y axis value should be in mag[1], Z axis value should be + in mag[2]. + @param[in] status A status of magnetometer. This status indicates the result + of measurement data, i.e. overflow, success or fail, etc. + @param[out] vx X axis value of magnetic field vector. + @param[out] vy Y axis value of magnetic field vector. + @param[out] vz Z axis value of magnetic field vector. + @param[out] accuracy Accuracy of magnetic field vector. + */ +int16 AKFS_Get_MAGNETIC_FIELD( + const int16 mag[3], + const int16 status, + AKFLOAT* vx, + AKFLOAT* vy, + AKFLOAT* vz, + int16* accuracy +) +{ + int16 akret; + int16 aocret; + AKFLOAT radius; + + AKMDATA(AKMDATA_DUMP, "%s: m[0]=%d, m[1]=%d, m[2]=%d, st=%d\n", + __FUNCTION__, mag[0], mag[1], mag[2], status); + + /* Decomposition */ + /* Sensitivity adjustment, i.e. multiply ASA, is done in this function. */ + akret = AKFS_DecompAK8975( + mag, + status, + &g_prms.mi_asa, + AKFS_HDATA_SIZE, + g_prms.mfv_hdata + ); + if(akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + + /* Adjust coordination */ + akret = AKFS_Rotate( + g_prms.m_hpat, + &g_prms.mfv_hdata[0] + ); + if (akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + + /* AOC for magnetometer */ + /* Offset estimation is done in this function */ + aocret = AKFS_AOC( + &g_prms.m_aocv, + g_prms.mfv_hdata, + &g_prms.mfv_ho + ); + + /* Subtract offset */ + /* Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. */ + akret = AKFS_VbNorm( + AKFS_HDATA_SIZE, + g_prms.mfv_hdata, + 1, + &g_prms.mfv_ho, + &g_prms.mfv_hs, + AK8975_HSENSE_TARGET, + AKFS_HDATA_SIZE, + g_prms.mfv_hvbuf + ); + if(akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + + /* Averaging */ + akret = AKFS_VbAve( + AKFS_HDATA_SIZE, + g_prms.mfv_hvbuf, + CSPEC_HNAVE_V, + &g_prms.mfv_hvec + ); + if (akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + + /* Check the size of magnetic vector */ + radius = AKFS_SQRT( + (g_prms.mfv_hvec.u.x * g_prms.mfv_hvec.u.x) + + (g_prms.mfv_hvec.u.y * g_prms.mfv_hvec.u.y) + + (g_prms.mfv_hvec.u.z * g_prms.mfv_hvec.u.z)); + + if (radius > AKFS_GEOMAG_MAX) { + g_prms.mi_hstatus = 0; + } else { + if (aocret) { + g_prms.mi_hstatus = 3; + } + } + + *vx = g_prms.mfv_hvec.u.x; + *vy = g_prms.mfv_hvec.u.y; + *vz = g_prms.mfv_hvec.u.z; + *accuracy = g_prms.mi_hstatus; + + /* Debug output */ + AKMDATA(AKMDATA_MAG, "Mag(%d):%8.2f, %8.2f, %8.2f\n", + *accuracy, *vx, *vy, *vz); + + return AKM_SUCCESS; +} + +/*! + This function is called when new accelerometer data is available. The output + vector format and coordination system follow the Android definition. + @return The return value is #AKM_SUCCESS when function succeeds. Otherwise + the return value is #AKM_FAIL. + @param[in] acc A set of measurement data from accelerometer. X axis value + should be in acc[0], Y axis value should be in acc[1], Z axis value should be + in acc[2]. + @param[in] status A status of accelerometer. This status indicates the result + of acceleration data, i.e. overflow, success or fail, etc. + @param[out] vx X axis value of acceleration vector. + @param[out] vy Y axis value of acceleration vector. + @param[out] vz Z axis value of acceleration vector. + @param[out] accuracy Accuracy of acceleration vector. + This value is always 3. + */ +int16 AKFS_Get_ACCELEROMETER( + const int16 acc[3], + const int16 status, + AKFLOAT* vx, + AKFLOAT* vy, + AKFLOAT* vz, + int16* accuracy +) +{ + int16 akret; + + AKMDATA(AKMDATA_DUMP, "%s: a[0]=%d, a[1]=%d, a[2]=%d, st=%d\n", + __FUNCTION__, acc[0], acc[1], acc[2], status); + + /* Save data to buffer */ + AKFS_BufShift( + AKFS_ADATA_SIZE, + 1, + g_prms.mfv_adata + ); + g_prms.mfv_adata[0].u.x = acc[0]; + g_prms.mfv_adata[0].u.y = acc[1]; + g_prms.mfv_adata[0].u.z = acc[2]; + + /* Subtract offset, adjust sensitivity */ + /* As a result, a unit of acceleration data in mfv_avbuf is '1G = 9.8' */ + akret = AKFS_VbNorm( + AKFS_ADATA_SIZE, + g_prms.mfv_adata, + 1, + &g_prms.mfv_ao, + &g_prms.mfv_as, + AK8975_ASENSE_TARGET, + AKFS_ADATA_SIZE, + g_prms.mfv_avbuf + ); + if(akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + + /* Averaging */ + akret = AKFS_VbAve( + AKFS_ADATA_SIZE, + g_prms.mfv_avbuf, + CSPEC_ANAVE_V, + &g_prms.mfv_avec + ); + if (akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + + /* Adjust coordination */ + /* It is not needed. Because, the data from AK8975 driver is already + follows Android coordinate system. */ + + *vx = g_prms.mfv_avec.u.x; + *vy = g_prms.mfv_avec.u.y; + *vz = g_prms.mfv_avec.u.z; + *accuracy = 3; + + /* Debug output */ + AKMDATA(AKMDATA_ACC, "Acc(%d):%8.2f, %8.2f, %8.2f\n", + *accuracy, *vx, *vy, *vz); + + return AKM_SUCCESS; +} + +/*! + Get orientation sensor's elements. The vector format and coordination system + follow the Android definition. Before this function is called, magnetic + field vector and acceleration vector should be stored in the buffer by + calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER. + @return The return value is #AKM_SUCCESS when function succeeds. Otherwise + the return value is #AKM_FAIL. + @param[out] azimuth Azimuthal angle in degree. + @param[out] pitch Pitch angle in degree. + @param[out] roll Roll angle in degree. + @param[out] accuracy Accuracy of orientation sensor. + */ +int16 AKFS_Get_ORIENTATION( + AKFLOAT* azimuth, + AKFLOAT* pitch, + AKFLOAT* roll, + int16* accuracy +) +{ + int16 akret; + + /* Azimuth calculation */ + /* Coordination system follows the Android coordination. */ + akret = AKFS_Direction( + AKFS_HDATA_SIZE, + g_prms.mfv_hvbuf, + CSPEC_HNAVE_D, + AKFS_ADATA_SIZE, + g_prms.mfv_avbuf, + CSPEC_ANAVE_D, + &g_prms.mf_azimuth, + &g_prms.mf_pitch, + &g_prms.mf_roll + ); + + if(akret == AKFS_ERROR) { + AKMERROR; + return AKM_FAIL; + } + *azimuth = g_prms.mf_azimuth; + *pitch = g_prms.mf_pitch; + *roll = g_prms.mf_roll; + *accuracy = g_prms.mi_hstatus; + + /* Debug output */ + AKMDATA(AKMDATA_ORI, "Ori(%d):%8.2f, %8.2f, %8.2f\n", + *accuracy, *azimuth, *pitch, *roll); + + return AKM_SUCCESS; +} + diff --git a/sensors/akmdfs/AKFS_APIs.h b/sensors/akmdfs/AKFS_APIs.h new file mode 100644 index 0000000..e4d1e48 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs.h @@ -0,0 +1,69 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c new file mode 100644 index 0000000..7bac9a1 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c @@ -0,0 +1,44 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h new file mode 100644 index 0000000..25459e3 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h @@ -0,0 +1,50 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c new file mode 100644 index 0000000..62b2361 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c @@ -0,0 +1,333 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h new file mode 100644 index 0000000..ffaaa88 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h @@ -0,0 +1,53 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h new file mode 100644 index 0000000..1f80f48 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h @@ -0,0 +1,37 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c new file mode 100644 index 0000000..3d99ab1 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c @@ -0,0 +1,110 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h new file mode 100644 index 0000000..0292d54 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h @@ -0,0 +1,107 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c new file mode 100644 index 0000000..f47e930 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c @@ -0,0 +1,133 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h new file mode 100644 index 0000000..c08338d --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h @@ -0,0 +1,39 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h new file mode 100644 index 0000000..dfe48b3 --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h @@ -0,0 +1,47 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c new file mode 100644 index 0000000..ffa934a --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c @@ -0,0 +1,107 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h new file mode 100644 index 0000000..c3c9bed --- /dev/null +++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h @@ -0,0 +1,46 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_CSpec.h b/sensors/akmdfs/AKFS_CSpec.h new file mode 100644 index 0000000..380f06f --- /dev/null +++ b/sensors/akmdfs/AKFS_CSpec.h @@ -0,0 +1,38 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_Common.h b/sensors/akmdfs/AKFS_Common.h new file mode 100644 index 0000000..7780801 --- /dev/null +++ b/sensors/akmdfs/AKFS_Common.h @@ -0,0 +1,141 @@ +/****************************************************************************** + * + * 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> /* ALOGV */ +#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 */ + +#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 \ + ALOGE("%s:%d Error.", __FUNCTION__, __LINE__) + +#define AKMERROR_STR(api) \ + ALOGE("%s:%d %s Error (%s).", \ + __FUNCTION__, __LINE__, (api), strerror(errno)) + +/*** Type declaration *********************************************************/ + +/*** Global variables *********************************************************/ + +/*** Prototype of function ****************************************************/ + +#endif /* AKMD_INC_AKCOMMON_H */ + diff --git a/sensors/akmdfs/AKFS_Compass.h b/sensors/akmdfs/AKFS_Compass.h new file mode 100644 index 0000000..aa59285 --- /dev/null +++ b/sensors/akmdfs/AKFS_Compass.h @@ -0,0 +1,84 @@ +/****************************************************************************** + * + * 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" + +/****************************************/ +/* 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/sensors/akmdfs/AKFS_Disp.c b/sensors/akmdfs/AKFS_Disp.c new file mode 100644 index 0000000..4e7c501 --- /dev/null +++ b/sensors/akmdfs/AKFS_Disp.c @@ -0,0 +1,89 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_Disp.h b/sensors/akmdfs/AKFS_Disp.h new file mode 100644 index 0000000..4769e3c --- /dev/null +++ b/sensors/akmdfs/AKFS_Disp.h @@ -0,0 +1,52 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_FileIO.c b/sensors/akmdfs/AKFS_FileIO.c new file mode 100644 index 0000000..92c2ce9 --- /dev/null +++ b/sensors/akmdfs/AKFS_FileIO.c @@ -0,0 +1,130 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_FileIO.h b/sensors/akmdfs/AKFS_FileIO.h new file mode 100644 index 0000000..62f6e77 --- /dev/null +++ b/sensors/akmdfs/AKFS_FileIO.h @@ -0,0 +1,39 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_Measure.c b/sensors/akmdfs/AKFS_Measure.c new file mode 100644 index 0000000..84c0843 --- /dev/null +++ b/sensors/akmdfs/AKFS_Measure.c @@ -0,0 +1,410 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/AKFS_Measure.h b/sensors/akmdfs/AKFS_Measure.h new file mode 100644 index 0000000..d156b95 --- /dev/null +++ b/sensors/akmdfs/AKFS_Measure.h @@ -0,0 +1,70 @@ +/****************************************************************************** + * + * 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/sensors/akmdfs/NOTICE b/sensors/akmdfs/NOTICE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/sensors/akmdfs/NOTICE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/sensors/akmdfs/Version.txt b/sensors/akmdfs/Version.txt new file mode 100644 index 0000000..3d56b30 --- /dev/null +++ b/sensors/akmdfs/Version.txt @@ -0,0 +1,31 @@ +Version + * Console Version 20120329 + 570 AK8975Driver.c + 496 AK8975Driver.h + 579 AKFS_APIs.c + 572 AKFS_APIs.h + 580 AKFS_APIs_8975/AKFS_AK8975.c + 580 AKFS_APIs_8975/AKFS_AK8975.h + 580 AKFS_APIs_8975/AKFS_AOC.c + 580 AKFS_APIs_8975/AKFS_AOC.h + 580 AKFS_APIs_8975/AKFS_Configure.h + 580 AKFS_APIs_8975/AKFS_Device.c + 580 AKFS_APIs_8975/AKFS_Device.h + 580 AKFS_APIs_8975/AKFS_Direction.c + 580 AKFS_APIs_8975/AKFS_Direction.h + 580 AKFS_APIs_8975/AKFS_Math.h + 580 AKFS_APIs_8975/AKFS_VNorm.c + 580 AKFS_APIs_8975/AKFS_VNorm.h + 568 AKFS_CSpec.h + 573 AKFS_Common.h + 572 AKFS_Compass.h + 579 AKFS_Disp.c + 568 AKFS_Disp.h + 568 AKFS_FileIO.c + 549 AKFS_FileIO.h + 573 AKFS_Measure.c + 568 AKFS_Measure.h + 574 Android.mk + 575 NOTICE + 579 main.c + diff --git a/sensors/akmdfs/main.c b/sensors/akmdfs/main.c new file mode 100644 index 0000000..a3df0bc --- /dev/null +++ b/sensors/akmdfs/main.c @@ -0,0 +1,293 @@ +/****************************************************************************** + * + * 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/sensors/bmp180.c b/sensors/bmp180.c new file mode 100644 index 0000000..cae97fb --- /dev/null +++ b/sensors/bmp180.c @@ -0,0 +1,239 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <sys/types.h> +#include <linux/ioctl.h> +#include <linux/input.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +struct bmp180_data { + char path_enable[PATH_MAX]; + char path_delay[PATH_MAX]; +}; + +int bmp180_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct bmp180_data *data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL) + return -EINVAL; + + data = (struct bmp180_data *) calloc(1, sizeof(struct bmp180_data)); + + input_fd = input_open("barometer_sensor"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; + } + + rc = sysfs_path_prefix("barometer_sensor", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(data->path_enable, PATH_MAX, "%s/enable", path); + snprintf(data->path_delay, PATH_MAX, "%s/poll_delay", path); + + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (input_fd >= 0) + close(input_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int bmp180_deinit(struct smdk4210_sensors_handlers *handlers) +{ + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL) + return -EINVAL; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + if (handlers->data != NULL) + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int bmp180_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct bmp180_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct bmp180_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int bmp180_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct bmp180_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct bmp180_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int bmp180_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +{ + struct bmp180_data *data; + int rc; + + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct bmp180_data *) handlers->data; + + rc = sysfs_value_write(data->path_delay, (int) delay); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + return 0; +} + +float bmp180_convert(int value) +{ + return value / 100.0f; +} + +int bmp180_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || event == NULL) + return -EINVAL; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -EINVAL; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + switch (input_event.code) { + case ABS_PRESSURE: + event->pressure = bmp180_convert(input_event.value); + break; + default: + continue; + } + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT && event->pressure != 0) { + event->timestamp = input_timestamp(&input_event); + break; + } else { + return -1; + } + } + } while (1); + + return 0; +} + +struct smdk4210_sensors_handlers bmp180 = { + .name = "BMP180", + .handle = SENSOR_TYPE_PRESSURE, + .init = bmp180_init, + .deinit = bmp180_deinit, + .activate = bmp180_activate, + .deactivate = bmp180_deactivate, + .set_delay = bmp180_set_delay, + .get_data = bmp180_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/gp2a_light.c b/sensors/gp2a_light.c new file mode 100644 index 0000000..7c4040c --- /dev/null +++ b/sensors/gp2a_light.c @@ -0,0 +1,239 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <sys/types.h> +#include <linux/ioctl.h> +#include <linux/input.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +struct gp2a_light_data { + char path_enable[PATH_MAX]; + char path_delay[PATH_MAX]; +}; + +int gp2a_light_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct gp2a_light_data *data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL) + return -EINVAL; + + data = (struct gp2a_light_data *) calloc(1, sizeof(struct gp2a_light_data)); + + input_fd = input_open("light_sensor"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; + } + + rc = sysfs_path_prefix("light_sensor", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(data->path_enable, PATH_MAX, "%s/enable", path); + snprintf(data->path_delay, PATH_MAX, "%s/poll_delay", path); + + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (input_fd >= 0) + close(input_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int gp2a_light_deinit(struct smdk4210_sensors_handlers *handlers) +{ + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL) + return -EINVAL; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + if (handlers->data != NULL) + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int gp2a_light_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct gp2a_light_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct gp2a_light_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int gp2a_light_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct gp2a_light_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct gp2a_light_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int gp2a_light_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +{ + struct gp2a_light_data *data; + int rc; + + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct gp2a_light_data *) handlers->data; + + rc = sysfs_value_write(data->path_delay, (int) delay); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + return 0; +} + +float gp2a_light_convert(int value) +{ + // Converting the raw value to lux units is done with: + // I = 10 * log(light) uA + // U = raw * 3300 / 4095 (max ADC value is 3.3V for 4095 LSB) + // R = 47kOhm + // => light = 10 ^ (I / 10) = 10 ^ (U / R / 10) + // => light = 10 ^ (raw * 330 / 4095 / 47) + // Only 1/4 of light reaches the sensor: + // => light = 4 * (10 ^ (raw * 330 / 4095 / 47)) + + return powf(10, value * (330.0f / 4095.0f / 47.0f)) * 4; +} + +int gp2a_light_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || event == NULL) + return -EINVAL; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -EINVAL; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + if (input_event.code == ABS_MISC) + event->light = gp2a_light_convert(input_event.value); + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT) + event->timestamp = input_timestamp(&input_event); + } + } while (input_event.type != EV_SYN); + + return 0; +} + +struct smdk4210_sensors_handlers gp2a_light = { + .name = "GP2A Light", + .handle = SENSOR_TYPE_LIGHT, + .init = gp2a_light_init, + .deinit = gp2a_light_deinit, + .activate = gp2a_light_activate, + .deactivate = gp2a_light_deactivate, + .set_delay = gp2a_light_set_delay, + .get_data = gp2a_light_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/gp2a_proximity.c b/sensors/gp2a_proximity.c new file mode 100644 index 0000000..d5fde88 --- /dev/null +++ b/sensors/gp2a_proximity.c @@ -0,0 +1,213 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> +#include <sys/types.h> +#include <linux/ioctl.h> +#include <linux/input.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +struct gp2a_proximity_data { + char path_enable[PATH_MAX]; +}; + +int gp2a_proximity_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct gp2a_proximity_data *data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL) + return -EINVAL; + + data = (struct gp2a_proximity_data *) calloc(1, sizeof(struct gp2a_proximity_data)); + + input_fd = input_open("proximity_sensor"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; + } + + rc = sysfs_path_prefix("proximity_sensor", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(data->path_enable, PATH_MAX, "%s/enable", path); + + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (input_fd >= 0) + close(input_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int gp2a_proximity_deinit(struct smdk4210_sensors_handlers *handlers) +{ + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL) + return -EINVAL; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + if (handlers->data != NULL) + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int gp2a_proximity_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct gp2a_proximity_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct gp2a_proximity_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int gp2a_proximity_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct gp2a_proximity_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct gp2a_proximity_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int gp2a_proximity_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +{ + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + return 0; +} + +float gp2a_proximity_convert(int value) +{ + return (float) value * 5.0f; +} + +int gp2a_proximity_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || event == NULL) + return -EINVAL; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -EINVAL; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + if (input_event.code == ABS_DISTANCE) + event->distance = gp2a_proximity_convert(input_event.value); + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT) + event->timestamp = input_timestamp(&input_event); + } + } while (input_event.type != EV_SYN); + + return 0; +} + +struct smdk4210_sensors_handlers gp2a_proximity = { + .name = "GP2A Proximity", + .handle = SENSOR_TYPE_PROXIMITY, + .init = gp2a_proximity_init, + .deinit = gp2a_proximity_deinit, + .activate = gp2a_proximity_activate, + .deactivate = gp2a_proximity_deactivate, + .set_delay = gp2a_proximity_set_delay, + .get_data = gp2a_proximity_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/input.c b/sensors/input.c new file mode 100644 index 0000000..1c1b26e --- /dev/null +++ b/sensors/input.c @@ -0,0 +1,335 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> +#include <dirent.h> +#include <linux/ioctl.h> +#include <linux/input.h> +#include <linux/uinput.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +void input_event_set(struct input_event *event, int type, int code, int value) +{ + if (event == NULL) + return; + + memset(event, 0, sizeof(struct input_event)); + + event->type = type, + event->code = code; + event->value = value; + + gettimeofday(&event->time, NULL); +} + +long int timestamp(struct timeval *time) +{ + if (time == NULL) + return -1; + + return time->tv_sec * 1000000000LL + time->tv_usec * 1000; +} + +long int input_timestamp(struct input_event *event) +{ + if (event == NULL) + return -1; + + return timestamp(&event->time); +} + +int uinput_rel_create(const char *name) +{ + struct uinput_user_dev uinput_dev; + int uinput_fd; + int rc; + + if (name == NULL) + return -1; + + uinput_fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); + if (uinput_fd < 0) { + ALOGE("%s: Unable to open uinput device", __func__); + goto error; + } + + memset(&uinput_dev, 0, sizeof(uinput_dev)); + + strncpy(uinput_dev.name, name, sizeof(uinput_dev.name)); + uinput_dev.id.bustype = BUS_I2C; + uinput_dev.id.vendor = 0; + uinput_dev.id.product = 0; + uinput_dev.id.version = 0; + + rc = 0; + rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_REL); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_X); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_Y); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_Z); + rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_SYN); + + if (rc < 0) { + ALOGE("%s: Unable to set uinput bits", __func__); + goto error; + } + + rc = write(uinput_fd, &uinput_dev, sizeof(uinput_dev)); + if (rc < 0) { + ALOGE("%s: Unable to write uinput device", __func__); + goto error; + } + + rc = ioctl(uinput_fd, UI_DEV_CREATE); + if (rc < 0) { + ALOGE("%s: Unable to create uinput device", __func__); + goto error; + } + + usleep(3000); + + return uinput_fd; + +error: + if (uinput_fd >= 0) + close(uinput_fd); + + return -1; +} + +void uinput_destroy(int uinput_fd) +{ + if (uinput_fd < 0) + return; + + ioctl(uinput_fd, UI_DEV_DESTROY); +} + +int input_open(char *name) +{ + DIR *d; + struct dirent *di; + + char input_name[80] = { 0 }; + char path[PATH_MAX]; + char *c; + int fd; + int rc; + + if (name == NULL) + return -EINVAL; + + d = opendir("/dev/input"); + if (d == NULL) + return -1; + + while ((di = readdir(d))) { + if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) + continue; + + snprintf(path, PATH_MAX, "/dev/input/%s", di->d_name); + fd = open(path, O_RDONLY | O_NONBLOCK); + if (fd < 0) + continue; + + rc = ioctl(fd, EVIOCGNAME(sizeof(input_name) - 1), &input_name); + if (rc < 0) + continue; + + c = strstr((char *) &input_name, "\n"); + if (c != NULL) + *c = '\0'; + + if (strcmp(input_name, name) == 0) + return fd; + else + close(fd); + } + + return -1; +} + +int sysfs_path_prefix(char *name, char *path_prefix) +{ + DIR *d; + struct dirent *di; + + char input_name[80] = { 0 }; + char path[PATH_MAX]; + char *c; + int fd; + + if (name == NULL || path_prefix == NULL) + return -EINVAL; + + d = opendir("/sys/class/input"); + if (d == NULL) + return -1; + + while ((di = readdir(d))) { + if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) + continue; + + snprintf(path, PATH_MAX, "/sys/class/input/%s/name", di->d_name); + + fd = open(path, O_RDONLY); + if (fd < 0) + continue; + + read(fd, &input_name, sizeof(input_name)); + close(fd); + + c = strstr((char *) &input_name, "\n"); + if (c != NULL) + *c = '\0'; + + if (strcmp(input_name, name) == 0) { + snprintf(path_prefix, PATH_MAX, "/sys/class/input/%s", di->d_name); + return 0; + } + } + + return -1; +} + +int sysfs_value_read(char *path) +{ + char buffer[100]; + int value; + int fd = -1; + int rc; + + if (path == NULL) + return -1; + + fd = open(path, O_RDONLY); + if (fd < 0) + goto error; + + rc = read(fd, &buffer, sizeof(buffer)); + if (rc <= 0) + goto error; + + value = atoi(buffer); + goto complete; + +error: + value = -1; + +complete: + if (fd >= 0) + close(fd); + + return value; +} + +int sysfs_value_write(char *path, int value) +{ + char buffer[100]; + int fd = -1; + int rc; + + if (path == NULL) + return -1; + + fd = open(path, O_WRONLY); + if (fd < 0) + goto error; + + snprintf((char *) &buffer, sizeof(buffer), "%d\n", value); + + rc = write(fd, buffer, strlen(buffer)); + if (rc < (int) strlen(buffer)) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +int sysfs_string_read(char *path, char *buffer, size_t length) +{ + int fd = -1; + int rc; + + if (path == NULL || buffer == NULL || length == 0) + return -1; + + fd = open(path, O_RDONLY); + if (fd < 0) + goto error; + + rc = read(fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +int sysfs_string_write(char *path, char *buffer, size_t length) +{ + int fd = -1; + int rc; + + if (path == NULL || buffer == NULL || length == 0) + return -1; + + fd = open(path, O_WRONLY); + if (fd < 0) + goto error; + + rc = write(fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} diff --git a/sensors/k3dh.c b/sensors/k3dh.c new file mode 100644 index 0000000..aa80292 --- /dev/null +++ b/sensors/k3dh.c @@ -0,0 +1,403 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" +#include "k3dh.h" + +struct k3dh_data { + struct smdk4210_sensors_handlers *orientation_sensor; + + long int delay; + int device_fd; + int uinput_fd; + + pthread_t thread; + pthread_mutex_t mutex; + int thread_continue; +}; + +void *k3dh_thread(void *thread_data) +{ + struct smdk4210_sensors_handlers *handlers = NULL; + struct k3dh_data *data = NULL; + struct input_event event; + struct timeval time; + struct k3dh_acceldata acceleration_data; + long int before, after; + int diff; + int device_fd; + int uinput_fd; + int rc; + + if (thread_data == NULL) + return NULL; + + handlers = (struct smdk4210_sensors_handlers *) thread_data; + if (handlers->data == NULL) + return NULL; + + data = (struct k3dh_data *) handlers->data; + + device_fd = data->device_fd; + if (device_fd < 0) + return NULL; + + uinput_fd = data->uinput_fd; + if (uinput_fd < 0) + return NULL; + + while (data->thread_continue) { + pthread_mutex_lock(&data->mutex); + if (!data->thread_continue) + break; + + while (handlers->activated) { + gettimeofday(&time, NULL); + before = timestamp(&time); + + memset(&acceleration_data, 0, sizeof(acceleration_data)); + + rc = ioctl(device_fd, K3DH_IOCTL_READ_ACCEL_XYZ, &acceleration_data); + if (rc < 0) { + ALOGE("%s: Unable to get k3dh data", __func__); + return NULL; + } + + input_event_set(&event, EV_REL, REL_X, (int) (acceleration_data.x * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_Y, (int) (acceleration_data.y * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_Z, (int) (acceleration_data.z * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_SYN, 0, 0); + write(uinput_fd, &event, sizeof(event)); + +next: + gettimeofday(&time, NULL); + after = timestamp(&time); + + diff = (int) (data->delay - (after - before)) / 1000; + if (diff <= 0) + continue; + + usleep(diff); + } + } + return NULL; +} + +int k3dh_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct k3dh_data *data = NULL; + pthread_attr_t thread_attr; + int device_fd = -1; + int uinput_fd = -1; + int input_fd = -1; + int rc; + int i; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL || device == NULL) + return -EINVAL; + + data = (struct k3dh_data *) calloc(1, sizeof(struct k3dh_data)); + + for (i = 0; i < device->handlers_count; i++) { + if (device->handlers[i] == NULL) + continue; + + if (device->handlers[i]->handle == SENSOR_TYPE_ORIENTATION) + data->orientation_sensor = device->handlers[i]; + } + + device_fd = open("/dev/accelerometer", O_RDONLY); + if (device_fd < 0) { + ALOGE("%s: Unable to open device", __func__); + goto error; + } + + uinput_fd = uinput_rel_create("acceleration"); + if (uinput_fd < 0) { + ALOGD("%s: Unable to create uinput", __func__); + goto error; + } + + input_fd = input_open("acceleration"); + if (input_fd < 0) { + ALOGE("%s: Unable to open acceleration input", __func__); + goto error; + } + + data->thread_continue = 1; + + pthread_mutex_init(&data->mutex, NULL); + pthread_mutex_lock(&data->mutex); + + pthread_attr_init(&thread_attr); + pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED); + + rc = pthread_create(&data->thread, &thread_attr, k3dh_thread, (void *) handlers); + if (rc < 0) { + ALOGE("%s: Unable to create k3dh thread", __func__); + pthread_mutex_destroy(&data->mutex); + goto error; + } + + data->device_fd = device_fd; + data->uinput_fd = uinput_fd; + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (uinput_fd >= 0) + close(uinput_fd); + + if (input_fd >= 0) + close(input_fd); + + if (device_fd >= 0) + close(device_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int k3dh_deinit(struct smdk4210_sensors_handlers *handlers) +{ + struct k3dh_data *data = NULL; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3dh_data *) handlers->data; + + handlers->activated = 0; + data->thread_continue = 0; + pthread_mutex_unlock(&data->mutex); + + pthread_mutex_destroy(&data->mutex); + + if (data->uinput_fd >= 0) { + uinput_destroy(data->uinput_fd); + close(data->uinput_fd); + } + data->uinput_fd = -1; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + if (data->device_fd >= 0) + close(data->device_fd); + data->device_fd = -1; + + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int k3dh_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct k3dh_data *data; + int device_fd; + int enable; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3dh_data *) handlers->data; + + device_fd = data->device_fd; + if (device_fd < 0) + return -1; + + enable = 1; + rc = ioctl(device_fd, K3DH_IOCTL_SET_ENABLE, &enable); + if (rc < 0) { + ALOGE("%s: Unable to set k3dh enable", __func__); + return -1; + } + + handlers->activated = 1; + pthread_mutex_unlock(&data->mutex); + + return 0; +} + +int k3dh_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct k3dh_data *data; + int device_fd; + int enable; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3dh_data *) handlers->data; + + device_fd = data->device_fd; + if (device_fd < 0) + return -1; + + enable = 0; + rc = ioctl(device_fd, K3DH_IOCTL_SET_ENABLE, &enable); + if (rc < 0) { + ALOGE("%s: Unable to set k3dh enable", __func__); + return -1; + } + + handlers->activated = 0; + + return 0; +} + +int k3dh_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +{ + struct k3dh_data *data; + int64_t d; + int device_fd; + int rc; + + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3dh_data *) handlers->data; + + device_fd = data->device_fd; + if (device_fd < 0) + return -1; + + d = (int64_t) delay; + rc = ioctl(device_fd, K3DH_IOCTL_SET_DELAY, &d); + if (rc < 0) { + ALOGE("%s: Unable to set k3dh delay", __func__); + return -1; + } + + data->delay = delay; + + return 0; +} + +float k3dh_convert(int value) +{ + return (float) (value / 1000.0f) * GRAVITY_EARTH * 0.7031f / 90.0f / 8.0f; +} + +int k3dh_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct k3dh_data *data; + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || handlers->data == NULL || event == NULL) + return -EINVAL; + + data = (struct k3dh_data *) handlers->data; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -1; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_REL) { + switch (input_event.code) { + case REL_X: + event->acceleration.y = k3dh_convert(input_event.value * -1); + break; + case REL_Y: + event->acceleration.x = k3dh_convert(input_event.value * -1); + break; + case REL_Z: + event->acceleration.z = k3dh_convert(input_event.value); + break; + default: + continue; + } + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT) + event->timestamp = input_timestamp(&input_event); + } + } while (input_event.type != EV_SYN); + + if (data->orientation_sensor != NULL) + orientation_fill(data->orientation_sensor, &event->acceleration, NULL); + + return 0; +} + +struct smdk4210_sensors_handlers k3dh = { + .name = "K3DH", + .handle = SENSOR_TYPE_ACCELEROMETER, + .init = k3dh_init, + .deinit = k3dh_deinit, + .activate = k3dh_activate, + .deactivate = k3dh_deactivate, + .set_delay = k3dh_set_delay, + .get_data = k3dh_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/k3dh.h b/sensors/k3dh.h new file mode 100644 index 0000000..2c2ad0d --- /dev/null +++ b/sensors/k3dh.h @@ -0,0 +1,41 @@ +/* + * STMicroelectronics k3dh acceleration sensor driver + * + * Copyright (C) 2010 Samsung Electronics Co.Ltd + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef __K3DH_ACC_HEADER__ +#define __K3DH__ACC_HEADER__ + +#include <linux/types.h> +#include <linux/ioctl.h> + +extern struct class *sec_class; + +struct k3dh_acceldata { + __s16 x; + __s16 y; + __s16 z; +}; + +/* dev info */ +#define ACC_DEV_NAME "accelerometer" + +/* k3dh ioctl command label */ +#define K3DH_IOCTL_BASE 'a' +#define K3DH_IOCTL_SET_DELAY _IOW(K3DH_IOCTL_BASE, 0, int64_t) +#define K3DH_IOCTL_GET_DELAY _IOR(K3DH_IOCTL_BASE, 1, int64_t) +#define K3DH_IOCTL_READ_ACCEL_XYZ _IOR(K3DH_IOCTL_BASE, 8, \ + struct k3dh_acceldata) +#define K3DH_IOCTL_SET_ENABLE _IOW(K3DH_IOCTL_BASE, 9, int) +#endif diff --git a/sensors/k3g.c b/sensors/k3g.c new file mode 100644 index 0000000..5a635e3 --- /dev/null +++ b/sensors/k3g.c @@ -0,0 +1,254 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <sys/types.h> +#include <linux/ioctl.h> +#include <linux/input.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +struct k3g_data { + char path_enable[PATH_MAX]; + char path_delay[PATH_MAX]; + + sensors_vec_t gyro; +}; + +int k3g_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct k3g_data *data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL) + return -EINVAL; + + data = (struct k3g_data *) calloc(1, sizeof(struct k3g_data)); + + input_fd = input_open("gyro_sensor"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; + } + + rc = sysfs_path_prefix("gyro_sensor", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(data->path_enable, PATH_MAX, "%s/enable", path); + snprintf(data->path_delay, PATH_MAX, "%s/poll_delay", path); + + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (input_fd >= 0) + close(input_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int k3g_deinit(struct smdk4210_sensors_handlers *handlers) +{ + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL) + return -EINVAL; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + if (handlers->data != NULL) + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int k3g_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct k3g_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3g_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int k3g_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct k3g_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3g_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int k3g_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +{ + struct k3g_data *data; + int rc; + + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct k3g_data *) handlers->data; + + rc = sysfs_value_write(data->path_delay, (int) delay); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + return 0; +} + +float k3g_convert(int value) +{ + return value * (70.0f / 4000.0f) * (3.1415926535f / 180.0f); +} + +int k3g_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct k3g_data *data; + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || handlers->data == NULL || event == NULL) + return -EINVAL; + + data = (struct k3g_data *) handlers->data; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -EINVAL; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + event->gyro.x = data->gyro.x; + event->gyro.y = data->gyro.y; + event->gyro.z = data->gyro.z; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_REL) { + switch (input_event.code) { + case REL_RX: + event->gyro.y = k3g_convert(input_event.value * -1); + break; + case REL_RY: + event->gyro.x = k3g_convert(input_event.value * -1); + break; + case REL_RZ: + event->gyro.z = k3g_convert(input_event.value * -1); + break; + default: + continue; + } + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT) + event->timestamp = input_timestamp(&input_event); + } + } while (input_event.type != EV_SYN); + + data->gyro.x = event->gyro.x; + data->gyro.y = event->gyro.y; + data->gyro.z = event->gyro.z; + + return 0; +} + +struct smdk4210_sensors_handlers k3g = { + .name = "K3G", + .handle = SENSOR_TYPE_GYROSCOPE, + .init = k3g_init, + .deinit = k3g_deinit, + .activate = k3g_activate, + .deactivate = k3g_deactivate, + .set_delay = k3g_set_delay, + .get_data = k3g_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/orientation.c b/sensors/orientation.c new file mode 100644 index 0000000..fe50605 --- /dev/null +++ b/sensors/orientation.c @@ -0,0 +1,444 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stddef.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <linux/ioctl.h> +#include <linux/uinput.h> +#include <linux/input.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +struct orientation_data { + struct smdk4210_sensors_handlers *acceleration_sensor; + struct smdk4210_sensors_handlers *magnetic_sensor; + + sensors_vec_t orientation; + sensors_vec_t acceleration; + sensors_vec_t magnetic; + + long int delay; + int uinput_fd; + + pthread_t thread; + pthread_mutex_t mutex; + int thread_continue; +}; + +static float rad2deg(float v) +{ + return (v * 180.0f / 3.1415926535f); +} + +static float vector_scalar(sensors_vec_t *v, sensors_vec_t *d) +{ + return v->x * d->x + v->y * d->y + v->z * d->z; +} + +static float vector_length(sensors_vec_t *v) +{ + return sqrtf(vector_scalar(v, v)); +} + +void orientation_calculate(sensors_vec_t *a, sensors_vec_t *m, sensors_vec_t *o) +{ + float azimuth, pitch, roll; + float la, sinp, cosp, sinr, cosr, x, y; + + if (a == NULL || m == NULL || o == NULL) + return; + + la = vector_length(a); + pitch = asinf(-(a->y) / la); + roll = asinf((a->x) / la); + + sinp = sinf(pitch); + cosp = cosf(pitch); + sinr = sinf(roll); + cosr = cosf(roll); + + y = -(m->x) * cosr + m->z * sinr; + x = m->x * sinp * sinr + m->y * cosp + m->z * sinp * cosr; + azimuth = atan2f(y, x); + + o->azimuth = rad2deg(azimuth); + o->pitch = rad2deg(pitch); + o->roll = rad2deg(roll); + + if (o->azimuth < 0) + o->azimuth += 360.0f; +} + +void *orientation_thread(void *thread_data) +{ + struct smdk4210_sensors_handlers *handlers = NULL; + struct orientation_data *data = NULL; + struct input_event event; + struct timeval time; + long int before, after; + int diff; + int uinput_fd; + + if (thread_data == NULL) + return NULL; + + handlers = (struct smdk4210_sensors_handlers *) thread_data; + if (handlers->data == NULL) + return NULL; + + data = (struct orientation_data *) handlers->data; + + uinput_fd = data->uinput_fd; + if (uinput_fd < 0) + return NULL; + + while (data->thread_continue) { + pthread_mutex_lock(&data->mutex); + if (!data->thread_continue) + break; + + while (handlers->activated) { + gettimeofday(&time, NULL); + before = timestamp(&time); + + orientation_calculate(&data->acceleration, &data->magnetic, &data->orientation); + + input_event_set(&event, EV_REL, REL_X, (int) (data->orientation.azimuth * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_Y, (int) (data->orientation.pitch * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_Z, (int) (data->orientation.roll * 1000)); + write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_SYN, 0, 0); + write(uinput_fd, &event, sizeof(event)); + + gettimeofday(&time, NULL); + after = timestamp(&time); + + diff = (int) (data->delay - (after - before)) / 1000; + if (diff <= 0) + continue; + + usleep(diff); + } + } + + return NULL; +} + +int orientation_fill(struct smdk4210_sensors_handlers *handlers, + sensors_vec_t *acceleration, sensors_vec_t *magnetic) +{ + struct orientation_data *data; + +// ALOGD("%s(%p, %p, %p)", __func__, handlers, acceleration, magnetic); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct orientation_data *) handlers->data; + + if (acceleration != NULL) { + data->acceleration.x = acceleration->x; + data->acceleration.y = acceleration->y; + data->acceleration.z = acceleration->z; + } + + if (magnetic != NULL) { + data->magnetic.x = magnetic->x; + data->magnetic.y = magnetic->y; + data->magnetic.z = magnetic->z; + } + + return 0; +} + +int orientation_init(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device) +{ + struct orientation_data *data = NULL; + pthread_attr_t thread_attr; + int uinput_fd = -1; + int input_fd = -1; + int rc; + int i; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL || device == NULL) + return -EINVAL; + + data = (struct orientation_data *) calloc(1, sizeof(struct orientation_data)); + + for (i = 0; i < device->handlers_count; i++) { + if (device->handlers[i] == NULL) + continue; + + if (device->handlers[i]->handle == SENSOR_TYPE_ACCELEROMETER) + data->acceleration_sensor = device->handlers[i]; + else if (device->handlers[i]->handle == SENSOR_TYPE_MAGNETIC_FIELD) + data->magnetic_sensor = device->handlers[i]; + } + + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) { + ALOGE("%s: Missing sensors for orientation", __func__); + goto error; + } + + uinput_fd = uinput_rel_create("orientation"); + if (uinput_fd < 0) { + ALOGD("%s: Unable to create uinput", __func__); + goto error; + } + + input_fd = input_open("orientation"); + if (input_fd < 0) { + ALOGE("%s: Unable to open orientation input", __func__); + goto error; + } + + data->thread_continue = 1; + + pthread_mutex_init(&data->mutex, NULL); + pthread_mutex_lock(&data->mutex); + + pthread_attr_init(&thread_attr); + pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED); + + rc = pthread_create(&data->thread, &thread_attr, orientation_thread, (void *) handlers); + if (rc < 0) { + ALOGE("%s: Unable to create orientation thread", __func__); + pthread_mutex_destroy(&data->mutex); + goto error; + } + + data->uinput_fd = uinput_fd; + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (uinput_fd >= 0) + close(uinput_fd); + + if (input_fd >= 0) + close(input_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int orientation_deinit(struct smdk4210_sensors_handlers *handlers) +{ + struct orientation_data *data; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct orientation_data *) handlers->data; + + handlers->activated = 0; + data->thread_continue = 0; + pthread_mutex_unlock(&data->mutex); + + pthread_mutex_destroy(&data->mutex); + + if (data->uinput_fd >= 0) { + uinput_destroy(data->uinput_fd); + close(data->uinput_fd); + } + data->uinput_fd = -1; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int orientation_activate(struct smdk4210_sensors_handlers *handlers) +{ + struct orientation_data *data; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct orientation_data *) handlers->data; + + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) + return -1; + + data->acceleration_sensor->needed |= SMDK4210_SENSORS_NEEDED_ORIENTATION; + if (data->acceleration_sensor->needed == SMDK4210_SENSORS_NEEDED_ORIENTATION) + data->acceleration_sensor->activate(data->acceleration_sensor); + + data->magnetic_sensor->needed |= SMDK4210_SENSORS_NEEDED_ORIENTATION; + if (data->magnetic_sensor->needed == SMDK4210_SENSORS_NEEDED_ORIENTATION) + data->magnetic_sensor->activate(data->magnetic_sensor); + + handlers->activated = 1; + pthread_mutex_unlock(&data->mutex); + + return 0; +} + +int orientation_deactivate(struct smdk4210_sensors_handlers *handlers) +{ + struct orientation_data *data; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct orientation_data *) handlers->data; + + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) + return -1; + + data->acceleration_sensor->needed &= ~(SMDK4210_SENSORS_NEEDED_ORIENTATION); + if (data->acceleration_sensor->needed == 0) + data->acceleration_sensor->deactivate(data->acceleration_sensor); + + data->magnetic_sensor->needed &= ~(SMDK4210_SENSORS_NEEDED_ORIENTATION); + if (data->magnetic_sensor->needed == 0) + data->magnetic_sensor->deactivate(data->magnetic_sensor); + + handlers->activated = 0; + + return 0; +} + +int orientation_set_delay(struct smdk4210_sensors_handlers *handlers, + long int delay) +{ + struct orientation_data *data; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct orientation_data *) handlers->data; + + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) + return -1; + + if (data->acceleration_sensor->needed == SMDK4210_SENSORS_NEEDED_ORIENTATION) + data->acceleration_sensor->set_delay(data->acceleration_sensor, delay); + + if (data->magnetic_sensor->needed == SMDK4210_SENSORS_NEEDED_ORIENTATION) + data->magnetic_sensor->set_delay(data->magnetic_sensor, delay); + + data->delay = delay; + + return 0; +} + +float orientation_convert(int value) +{ + return (float) value / 1000.0f; +} + +int orientation_get_data(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct input_event input_event; + int input_fd = -1; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || event == NULL) + return -EINVAL; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -EINVAL; + + memset(event, 0, sizeof(struct sensors_event_t)); + event->version = sizeof(struct sensors_event_t); + event->sensor = handlers->handle; + event->type = handlers->handle; + + event->orientation.status = SENSOR_STATUS_ACCURACY_MEDIUM; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_REL) { + switch (input_event.code) { + case REL_X: + event->orientation.azimuth = orientation_convert(input_event.value); + break; + case REL_Y: + event->orientation.pitch = orientation_convert(input_event.value); + break; + case REL_Z: + event->orientation.roll = orientation_convert(input_event.value); + break; + default: + continue; + } + } else if (input_event.type == EV_SYN) { + if (input_event.code == SYN_REPORT) + event->timestamp = input_timestamp(&input_event); + } + } while (input_event.type != EV_SYN); + + return 0; +} + +struct smdk4210_sensors_handlers orientation = { + .name = "Orientation", + .handle = SENSOR_TYPE_ORIENTATION, + .init = orientation_init, + .deinit = orientation_deinit, + .activate = orientation_activate, + .deactivate = orientation_deactivate, + .set_delay = orientation_set_delay, + .get_data = orientation_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/sensors/smdk4210_sensors.c b/sensors/smdk4210_sensors.c new file mode 100644 index 0000000..6e1b92f --- /dev/null +++ b/sensors/smdk4210_sensors.c @@ -0,0 +1,292 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <unistd.h> +#include <stdint.h> +#include <fcntl.h> +#include <errno.h> +#include <poll.h> +#include <sys/select.h> +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#define LOG_TAG "smdk4210_sensors" +#include <utils/Log.h> + +#include "smdk4210_sensors.h" + +/* + * Sensors list + */ + +struct sensor_t smdk4210_sensors[] = { + { "K3DH Acceleration Sensor", "STMicroelectronics", 1, SENSOR_TYPE_ACCELEROMETER, + SENSOR_TYPE_ACCELEROMETER, 2 * GRAVITY_EARTH, GRAVITY_EARTH / 90.0f / 8.0f, 0.23f, 20000, {}, }, + { "AKM8975 Magnetic Sensor", "Asahi Kasei Microdevices", 1, SENSOR_TYPE_MAGNETIC_FIELD, + SENSOR_TYPE_MAGNETIC_FIELD, 2000.0f, 1.0f / 16, 6.8f, 16667, {}, }, + { "Orientation Sensor", "SMDK4210 Sensors", 1, SENSOR_TYPE_ORIENTATION, + SENSOR_TYPE_ORIENTATION, 360.0f, 0.1f, 0.0f, 16667, {}, }, + { "GP2A Light Sensor", "Sharp", 1, SENSOR_TYPE_LIGHT, + SENSOR_TYPE_LIGHT, 10240.0f, 1.0f, 0.75f, 0, {}, }, + { "GP2A Proximity Sensor", "Sharp", 1, SENSOR_TYPE_PROXIMITY, + SENSOR_TYPE_PROXIMITY, 5.0f, 5.0f, 0.75f, 0, {}, }, + { "K3G Gyroscope Sensor", "STMicroelectronics", 1, SENSOR_TYPE_GYROSCOPE, + SENSOR_TYPE_GYROSCOPE, 500.0f * (3.1415926535f / 180.0f), (70.0f / 4000.0f) * (3.1415926535f / 180.0f), 6.1f, 1190, {}, }, + { "BMP180 Pressure Sensor", "Bosch", 1, SENSOR_TYPE_PRESSURE, + SENSOR_TYPE_PRESSURE, 1100.0f, 0.01f, 0.67f, 20000, {}, }, +}; + +int smdk4210_sensors_count = sizeof(smdk4210_sensors) / sizeof(struct sensor_t); + +struct smdk4210_sensors_handlers *smdk4210_sensors_handlers[] = { + &k3dh, + &akm8975, + &orientation, + &gp2a_light, + &gp2a_proximity, + &k3g, + &bmp180, +}; + +int smdk4210_sensors_handlers_count = sizeof(smdk4210_sensors_handlers) / + sizeof(struct smdk4210_sensors_handlers *); + +/* + * SMDK4210 Sensors + */ + +int smdk4210_sensors_activate(struct sensors_poll_device_t *dev, int handle, + int enabled) +{ + struct smdk4210_sensors_device *device; + int i; + + ALOGD("%s(%p, %d, %d)", __func__, dev, handle, enabled); + + if (dev == NULL) + return -EINVAL; + + device = (struct smdk4210_sensors_device *) dev; + + if (device->handlers == NULL || device->handlers_count <= 0) + return -EINVAL; + + for (i = 0; i < device->handlers_count; i++) { + if (device->handlers[i] == NULL) + continue; + + if (device->handlers[i]->handle == handle) { + if (enabled && device->handlers[i]->activate != NULL) { + device->handlers[i]->needed |= SMDK4210_SENSORS_NEEDED_API; + if (device->handlers[i]->needed == SMDK4210_SENSORS_NEEDED_API) + return device->handlers[i]->activate(device->handlers[i]); + else + return 0; + } else if (!enabled && device->handlers[i]->deactivate != NULL) { + device->handlers[i]->needed &= ~SMDK4210_SENSORS_NEEDED_API; + if (device->handlers[i]->needed == 0) + return device->handlers[i]->deactivate(device->handlers[i]); + else + return 0; + } + } + } + + return -1; +} + +int smdk4210_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, + int64_t ns) +{ + struct smdk4210_sensors_device *device; + int i; + + ALOGD("%s(%p, %d, %ld)", __func__, dev, handle, (long int) ns); + + if (dev == NULL) + return -EINVAL; + + device = (struct smdk4210_sensors_device *) dev; + + if (device->handlers == NULL || device->handlers_count <= 0) + return -EINVAL; + + for (i = 0; i < device->handlers_count; i++) { + if (device->handlers[i] == NULL) + continue; + + if (device->handlers[i]->handle == handle && device->handlers[i]->set_delay != NULL) + return device->handlers[i]->set_delay(device->handlers[i], (long int) ns); + } + + return 0; +} + +int smdk4210_sensors_poll(struct sensors_poll_device_t *dev, + struct sensors_event_t* data, int count) +{ + struct smdk4210_sensors_device *device; + int i, j; + int c, n; + int poll_rc, rc; + +// ALOGD("%s(%p, %p, %d)", __func__, dev, data, count); + + if (dev == NULL) + return -EINVAL; + + device = (struct smdk4210_sensors_device *) dev; + + if (device->handlers == NULL || device->handlers_count <= 0 || + device->poll_fds == NULL || device->poll_fds_count <= 0) + return -EINVAL; + + n = 0; + + do { + poll_rc = poll(device->poll_fds, device->poll_fds_count, n > 0 ? 0 : -1); + if (poll_rc < 0) + return -1; + + for (i = 0; i < device->poll_fds_count; i++) { + if (!(device->poll_fds[i].revents & POLLIN)) + continue; + + for (j = 0; j < device->handlers_count; j++) { + if (device->handlers[j] == NULL || device->handlers[j]->poll_fd != device->poll_fds[i].fd || device->handlers[j]->get_data == NULL) + continue; + + rc = device->handlers[j]->get_data(device->handlers[j], &data[n]); + if (rc < 0) { + device->poll_fds[i].revents = 0; + poll_rc = -1; + } else { + n++; + count--; + } + } + } + } while ((poll_rc > 0 || n < 1) && count > 0); + + return n; +} + +/* + * Interface + */ + +int smdk4210_sensors_close(hw_device_t *device) +{ + struct smdk4210_sensors_device *smdk4210_sensors_device; + int i; + + ALOGD("%s(%p)", __func__, device); + + if (device == NULL) + return -EINVAL; + + smdk4210_sensors_device = (struct smdk4210_sensors_device *) device; + + if (smdk4210_sensors_device->poll_fds != NULL) + free(smdk4210_sensors_device->poll_fds); + + for (i = 0; i < smdk4210_sensors_device->handlers_count; i++) { + if (smdk4210_sensors_device->handlers[i] == NULL || smdk4210_sensors_device->handlers[i]->deinit == NULL) + continue; + + smdk4210_sensors_device->handlers[i]->deinit(smdk4210_sensors_device->handlers[i]); + } + + free(device); + + return 0; +} + +int smdk4210_sensors_open(const struct hw_module_t* module, const char *id, + struct hw_device_t** device) +{ + struct smdk4210_sensors_device *smdk4210_sensors_device; + int p, i; + + ALOGD("%s(%p, %s, %p)", __func__, module, id, device); + + if (module == NULL || device == NULL) + return -EINVAL; + + smdk4210_sensors_device = (struct smdk4210_sensors_device *) + calloc(1, sizeof(struct smdk4210_sensors_device)); + smdk4210_sensors_device->device.common.tag = HARDWARE_DEVICE_TAG; + smdk4210_sensors_device->device.common.version = 0; + smdk4210_sensors_device->device.common.module = (struct hw_module_t *) module; + smdk4210_sensors_device->device.common.close = smdk4210_sensors_close; + smdk4210_sensors_device->device.activate = smdk4210_sensors_activate; + smdk4210_sensors_device->device.setDelay = smdk4210_sensors_set_delay; + smdk4210_sensors_device->device.poll = smdk4210_sensors_poll; + smdk4210_sensors_device->handlers = smdk4210_sensors_handlers; + smdk4210_sensors_device->handlers_count = smdk4210_sensors_handlers_count; + smdk4210_sensors_device->poll_fds = (struct pollfd *) + calloc(1, smdk4210_sensors_handlers_count * sizeof(struct pollfd)); + + p = 0; + for (i = 0; i < smdk4210_sensors_handlers_count; i++) { + if (smdk4210_sensors_handlers[i] == NULL || smdk4210_sensors_handlers[i]->init == NULL) + continue; + + smdk4210_sensors_handlers[i]->init(smdk4210_sensors_handlers[i], smdk4210_sensors_device); + if (smdk4210_sensors_handlers[i]->poll_fd >= 0) { + smdk4210_sensors_device->poll_fds[p].fd = smdk4210_sensors_handlers[i]->poll_fd; + smdk4210_sensors_device->poll_fds[p].events = POLLIN; + p++; + } + } + + smdk4210_sensors_device->poll_fds_count = p; + + *device = &(smdk4210_sensors_device->device.common); + + return 0; +} + +int smdk4210_sensors_get_sensors_list(struct sensors_module_t* module, + const struct sensor_t **sensors_p) +{ + ALOGD("%s(%p, %p)", __func__, module, sensors_p); + + if (sensors_p == NULL) + return -EINVAL; + + *sensors_p = smdk4210_sensors; + return smdk4210_sensors_count; +} + +struct hw_module_methods_t smdk4210_sensors_module_methods = { + .open = smdk4210_sensors_open, +}; + +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 = "SMDK4210 Sensors", + .author = "Paul Kocialkowski", + .methods = &smdk4210_sensors_module_methods, + }, + .get_sensors_list = smdk4210_sensors_get_sensors_list, +}; diff --git a/sensors/smdk4210_sensors.h b/sensors/smdk4210_sensors.h new file mode 100644 index 0000000..fdbb0cb --- /dev/null +++ b/sensors/smdk4210_sensors.h @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdint.h> +#include <poll.h> +#include <linux/input.h> + +#include <hardware/sensors.h> +#include <hardware/hardware.h> + +#ifndef _SMDK4210_SENSORS_H_ +#define _SMDK4210_SENSORS_H_ + +#define SMDK4210_SENSORS_NEEDED_API (1 << 0) +#define SMDK4210_SENSORS_NEEDED_ORIENTATION (1 << 1) + +struct smdk4210_sensors_device; + +struct smdk4210_sensors_handlers { + char *name; + int handle; + + int (*init)(struct smdk4210_sensors_handlers *handlers, + struct smdk4210_sensors_device *device); + int (*deinit)(struct smdk4210_sensors_handlers *handlers); + int (*activate)(struct smdk4210_sensors_handlers *handlers); + int (*deactivate)(struct smdk4210_sensors_handlers *handlers); + int (*set_delay)(struct smdk4210_sensors_handlers *handlers, + long int delay); + int (*get_data)(struct smdk4210_sensors_handlers *handlers, + struct sensors_event_t *event); + + int activated; + int needed; + int poll_fd; + + void *data; +}; + +struct smdk4210_sensors_device { + struct sensors_poll_device_t device; + + struct smdk4210_sensors_handlers **handlers; + int handlers_count; + + struct pollfd *poll_fds; + int poll_fds_count; +}; + +extern struct smdk4210_sensors_handlers *smdk4210_sensors_handlers[]; +extern int smdk4210_sensors_handlers_count; + +int smdk4210_sensors_activate(struct sensors_poll_device_t *dev, int handle, + int enabled); +int smdk4210_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, + int64_t ns); +int smdk4210_sensors_poll(struct sensors_poll_device_t *dev, + struct sensors_event_t* data, int count); + +/* + * Input + */ + +void input_event_set(struct input_event *event, int type, int code, int value); +long int timestamp(struct timeval *time); +long int input_timestamp(struct input_event *event); +int uinput_rel_create(const char *name); +void uinput_destroy(int uinput_fd); +int input_open(char *name); +int sysfs_path_prefix(char *name, char *path_prefix); +int sysfs_value_read(char *path); +int sysfs_value_write(char *path, int value); +int sysfs_string_read(char *path, char *buffer, size_t length); +int sysfs_string_write(char *path, char *buffer, size_t length); + +/* + * Sensors + */ + +int orientation_fill(struct smdk4210_sensors_handlers *handlers, + sensors_vec_t *acceleration, sensors_vec_t *magnetic); + +extern struct smdk4210_sensors_handlers k3dh; +extern struct smdk4210_sensors_handlers akm8975; +extern struct smdk4210_sensors_handlers orientation; +extern struct smdk4210_sensors_handlers gp2a_light; +extern struct smdk4210_sensors_handlers gp2a_proximity; +extern struct smdk4210_sensors_handlers k3g; +extern struct smdk4210_sensors_handlers bmp180; + +#endif |