diff options
Diffstat (limited to 'libsensors/BoschYamaha.cpp')
| -rw-r--r-- | libsensors/BoschYamaha.cpp | 500 |
1 files changed, 0 insertions, 500 deletions
diff --git a/libsensors/BoschYamaha.cpp b/libsensors/BoschYamaha.cpp deleted file mode 100644 index 5836d19..0000000 --- a/libsensors/BoschYamaha.cpp +++ /dev/null @@ -1,500 +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 <cutils/log.h> - -#include "BoschYamaha.h" - -/*****************************************************************************/ - -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(uint64_t delay) { - return -ENODEV; -} - -BoschYamaha::BoschYamaha() -: SensorBase(NULL, NULL), - //mEnabled(0), - mPendingMask(0), - mInputReaderMagnetic(4), - mInputReaderAccel(4) -{ - - counterAccel = 0; - - /* FIXME set input device name for magnetic sensor */ - data_name = "input0"; - data_compass_fd = openInput("geomagnetic"); - - - //Open Compass - if (data_compass_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(MagneticField, 1); - - } - - /* FIXME set input device name for accelereometer sensor */ - data_accel_name = "event6"; - data_fd = openInput("SMB380-Sensor"); - - //Open Compass - if (data_fd) { - strcpy(input_accel_sysfs_path, "/sys/class/input/"); - strcat(input_accel_sysfs_path, data_accel_name); - strcat(input_accel_sysfs_path, "/device/"); - input_accel_sysfs_path_len = strlen(input_accel_sysfs_path); - - enable(Accelerometer, 1); - - } - - - memset(mPendingEvents, 0, sizeof(mPendingEvents)); - - mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); - mPendingEvents[Accelerometer].sensor = ID_A; - mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; - mPendingEvents[Accelerometer].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; - - mPendingEvents[MagneticField].version = sizeof(sensors_event_t); - mPendingEvents[MagneticField].sensor = ID_M; - mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; - mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; - - mPendingEvents[Orientation].version = sizeof(sensors_event_t); - mPendingEvents[Orientation].sensor = ID_O; - mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; - mPendingEvents[Orientation].orientation.status = SENSOR_STATUS_ACCURACY_HIGH; - - - - - for (int i=0 ; i<numSensors ; i++) - mDelays[i] = 200000000; // 200 ms by default - -} - -BoschYamaha::~BoschYamaha() -{ - if(compassEnabled){ - enable(MagneticField, 0); - } - if(accelEnabled){ - enable(Accelerometer, 0); - } - -} - -int BoschYamaha::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; - - /* - //FIXME enabling the right sensor over sysfs interface - if(what == MagneticField){ - what = Accelerometer; //Enable also Accel - //Compass - ALOGD("BoschYamaha::~enable Compass(0, %d)", en); - int flags = en ? 1 : 0; - if (flags != compassEnabled) { - int fd; - strcpy(&input_sysfs_path[input_sysfs_path_len], "enable"); - ALOGD("BoschYamaha::~enable Compass(0, %d) open %s",en, input_sysfs_path); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - ALOGD("CompassSensor::~enable(0, %d) opened %s",en, input_sysfs_path); - 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; - compassEnabled = flags; - return 0; - } - } - } - */ - - if(what == Accelerometer){ - //Accelerometer - ALOGD("BoschYamaha::~enable Accel(0, %d)", en); - int flags = en ? 1 : 0; - if (flags != accelEnabled) { - int fd; - strcpy(&input_accel_sysfs_path[input_accel_sysfs_path_len], "enable"); - ALOGD("BoschYamaha::~enable Accel(0, %d) open %s",en, input_accel_sysfs_path); - fd = open(input_accel_sysfs_path, O_RDWR); - if (fd >= 0) { - ALOGD("CompassSensor::~enable(0, %d) opened %s",en, input_accel_sysfs_path); - char buf[2]; - int err; - buf[1] = 0; - if (flags) { - // counterAccel++; - buf[0] = '1'; - } else { - // counterAccel--; - buf[0] = '0'; - } - //if(counterAccel <= 1){ - err = write(fd, buf, sizeof(buf)); - //} - close(fd); - //mEnabled = flags; - accelEnabled = flags; - return 0; - } - } - } - - - return err; -} - -int BoschYamaha::setDelay(int32_t handle, int64_t ns) -{ - 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; - - if (ns < 0) - return -EINVAL; - - mDelays[what] = ns; - return update_delay(); -} - -int BoschYamaha::update_delay() -{ - return 0; -} - - -bool BoschYamaha::hasPendingEvents() { - - //ALOGD("hasPendingEvents was called"); - if(accelEnabled || compassEnabled){ - //ALOGD("hasPendingEvents will return true"); - return true; - } - else { - //ALOGD("hasPendingEvents will return false"); - return false; - } -} - - - -int BoschYamaha::readEvents(sensors_event_t* data, int count) -{ - if (count < 1) - return -EINVAL; - - - int numEventReceived = 0; - - //ALOGD("Sensor: Read events was called with count: %d", count); - - if(compassEnabled) - { - //ALOGD("Sensor: Compass is enabled, going to take care of it, count: %d", count); - ssize_t n = mInputReaderMagnetic.fill(data_compass_fd); - if (n < 0) - return n; - - input_event const* event; - - while ( count && mInputReaderMagnetic.readEvent(&event)) { - int type = event->type; - //Everything is ABS! - if (type == EV_ABS) { - float value = event->value; - if (event->code == EVENT_TYPE_MAGV_X) { - mPendingEvents[MagneticField].magnetic.x = (value * CONVERT_M_X); - } else if (event->code == EVENT_TYPE_MAGV_Y) { - mPendingEvents[MagneticField].magnetic.y = (value * CONVERT_M_Y); - } else if (event->code == EVENT_TYPE_MAGV_Z) { - mPendingEvents[MagneticField].magnetic.z = (value * CONVERT_M_Z); - } - } - else if (type == EV_SYN) { - mPendingEvents[MagneticField].timestamp = timevalToNano(event->time); - if (compassEnabled) { - //ALOGD("Sensor: Compass was enabled, made a read and received a Sync"); - compassDataReady = 1; - compassLastRead[0] = mPendingEvents[MagneticField].magnetic.x; - compassLastRead[1] = mPendingEvents[MagneticField].magnetic.y; - compassLastRead[2] = mPendingEvents[MagneticField].magnetic.z; - - *data++ = mPendingEvents[MagneticField]; - count--; - numEventReceived++; - } - } else { - ALOGE("BoschYamaha: unknown event (type=%d, code=%d)", type, event->code); - - } - mInputReaderMagnetic.next(); - } - } - - //ALOGD("Sensor: Compass read quited, count: %d", count); - - - if(accelEnabled) - { - //ALOGD("Sensor: Accel is enabled, going to take care of it, count: %d", count); - - ssize_t n = mInputReaderAccel.fill(data_fd); - //ALOGD("Sensor: Accel Input Reader was filled up: %d", count); - if (n < 0) - return n; - - input_event const* event; - - while (count && mInputReaderAccel.readEvent(&event)) { - int type = event->type; - if (type == EV_ABS) { - //ALOGD("Sensor: Received one EV_ABS Event"); - float value = event->value; - if (event->code == EVENT_TYPE_ACCEL_X) { - mPendingEvents[Accelerometer].acceleration.x = value * CONVERT_A_X; - } else if (event->code == EVENT_TYPE_ACCEL_Y) { - mPendingEvents[Accelerometer].acceleration.y = value * CONVERT_A_Y; - } else if (event->code == EVENT_TYPE_ACCEL_Z) { - mPendingEvents[Accelerometer].acceleration.z = value * CONVERT_A_Z; - } - } else if (type == EV_SYN) { - mPendingEvents[Accelerometer].timestamp = timevalToNano(event->time); - if (accelEnabled) { - //ALOGD("Sensor: Accel was enabled, made a read and received a Sync"); - accelDataReady = 1; - accelLastRead[0] = mPendingEvents[Accelerometer].acceleration.x; - accelLastRead[1] = mPendingEvents[Accelerometer].acceleration.y; - accelLastRead[2] = mPendingEvents[Accelerometer].acceleration.z; - *data++ = mPendingEvents[Accelerometer]; - count--; - numEventReceived++; - } - } - else { - ALOGE("BoschYamaha: unknown event (type=%d, code=%d)", type, event->code); - - } - mInputReaderAccel.next(); - } - } - - //ALOGD("Sensor: Accel read quited, count: %d, acceldata Ready: %d, compassData Ready = %d", count, accelDataReady, compassDataReady); - - if( (accelDataReady == 1) && (compassDataReady == 1)){ - //ALOGD("BoschYamaha: Going to Process Orientation Data"); - accelDataReady = 0; - compassDataReady = 0; - processOrientation(); - *data++ = mPendingEvents[Orientation]; - numEventReceived++; - } - - - return numEventReceived; -} - - -int BoschYamaha::processOrientation(){ - - float matrix[9], euler[3]; - int rt, i; - - - rt = get_rotation_matrix(accelLastRead, compassLastRead, matrix); - - - if (rt < 0) { - for (i = 0; i < 3; i++) { - euler[i] = 0; - } - } - else { - get_euler(matrix, euler); - } - - //ALOGD("BoschYamaha: azimuth is %d", (int)(euler[0])); - //ALOGD("BoschYamaha: pitch is %d", (int)(euler[1])); - //ALOGD("BoschYamaha: roll is %d", (int)(euler[2])); - - //Use CONVERT_O_A - mPendingEvents[Orientation].orientation.azimuth = (int)(euler[0]); - mPendingEvents[Orientation].orientation.pitch = (int)(euler[1]); - mPendingEvents[Orientation].orientation.roll = (int)(euler[2]); - //mPendingEvents[Orientation].orientation.status = uint8_t(value & SENSOR_STATE_MASK); - - return 1; - -} - - -float BoschYamaha::calc_intensity(float x, float y, float z) -{ - return sqrt(x*x + y*y + z*z); -} - - -int BoschYamaha::get_rotation_matrix(const float *gsdata, const float *msdata, float *matrix) -{ - float m_intensity, g_intensity, a_intensity, b_intensity; - float gdata[3], mdata[3], adata[3], bdata[3]; - int i; - - if (gsdata == NULL || msdata == NULL || matrix == NULL) { - return -1; - } - g_intensity = calc_intensity(gsdata[0], gsdata[1], gsdata[2]); - m_intensity = calc_intensity(msdata[0], msdata[1], msdata[2]); - if (g_intensity == 0 || m_intensity == 0) { - return -1; - } - for (i = 0; i < 3; i++) { - gdata[i] = -gsdata[i] / g_intensity; - mdata[i] = msdata[i] / m_intensity; - } - - adata[0] = (gdata[1] * mdata[2] - gdata[2] * mdata[1]); - adata[1] = (gdata[2] * mdata[0] - gdata[0] * mdata[2]); - adata[2] = (gdata[0] * mdata[1] - gdata[1] * mdata[0]); - a_intensity = calc_intensity(adata[0], adata[1], adata[2]); - if (a_intensity == 0) { - return -1; - } - for (i = 0; i < 3; i++) { - adata[i] /= a_intensity; - } - - bdata[0] = (adata[1] * gdata[2] - adata[2] * gdata[1]); - bdata[1] = (adata[2] * gdata[0] - adata[0] * gdata[2]); - bdata[2] = (adata[0] * gdata[1] - adata[1] * gdata[0]); - b_intensity = calc_intensity(bdata[0], bdata[1], bdata[2]); - if (b_intensity == 0) { - return -1; - } - for (i = 0; i < 3; i++) { - bdata[i] /= b_intensity; - } - - matrix[0] = adata[0]; - matrix[1] = adata[1]; - matrix[2] = adata[2]; - matrix[3] = bdata[0]; - matrix[4] = bdata[1]; - matrix[5] = bdata[2]; - matrix[6] = -gdata[0]; - matrix[7] = -gdata[1]; - matrix[8] = -gdata[2]; - - return 0; -} - -int BoschYamaha::get_euler(const float *matrix, float *euler) -{ - float m11, m12; - float m21, m22; - float m31, m32, m33; - float yaw = 0, roll = 0, pitch = 0; - - if (matrix == NULL || euler == NULL) { - return -1; - } - - m11 = matrix[0]; - m12 = matrix[1]; - m21 = matrix[3]; - m22 = matrix[4]; - m31 = matrix[6]; - m32 = matrix[7]; - m33 = matrix[8]; - - yaw = atan2(m12-m21, m11+m22); - pitch = -asin(m32); - roll = asin(m31); - - yaw *= 180.0 / M_PI; - pitch *= 180.0 / M_PI; - roll *= 180.0 / M_PI; - - if (m33 < 0) { - pitch = -180 - pitch; - if (pitch < -180) { - pitch += 360; - } - } - if (yaw < 0) { - yaw += 360.0f; - } - - euler[0] = (float)(int)yaw; /* yaw */ - - euler[1] = (float)(int)pitch; /* pitch */ - euler[2] = (float)(int)roll; /* roll */ - - return 0; -} - - - |