From 91c2ee3f53227cecf4c720171d9379955c9a4463 Mon Sep 17 00:00:00 2001 From: Pawit Pornkitprasan Date: Fri, 23 Dec 2011 19:31:26 +0700 Subject: libsensors: Fix line ending --- libsensors/BoschYamaha.cpp | 1000 ++++++++++++++++++++++---------------------- 1 file changed, 500 insertions(+), 500 deletions(-) (limited to 'libsensors/BoschYamaha.cpp') diff --git a/libsensors/BoschYamaha.cpp b/libsensors/BoschYamaha.cpp index ba88ff9..a6b9fa6 100644 --- a/libsensors/BoschYamaha.cpp +++ b/libsensors/BoschYamaha.cpp @@ -1,500 +1,500 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include - -#include - -#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) - 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 - LOGD("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"); - LOGD("BoschYamaha::~enable Compass(0, %d) open %s",en, input_sysfs_path); - fd = open(input_sysfs_path, O_RDWR); - if (fd >= 0) { - LOGD("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 - LOGD("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"); - LOGD("BoschYamaha::~enable Accel(0, %d) open %s",en, input_accel_sysfs_path); - fd = open(input_accel_sysfs_path, O_RDWR); - if (fd >= 0) { - LOGD("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() { - - //LOGD("hasPendingEvents was called"); - if(accelEnabled || compassEnabled){ - //LOGD("hasPendingEvents will return true"); - return true; - } - else { - //LOGD("hasPendingEvents will return false"); - return false; - } -} - - - -int BoschYamaha::readEvents(sensors_event_t* data, int count) -{ - if (count < 1) - return -EINVAL; - - - int numEventReceived = 0; - - //LOGD("Sensor: Read events was called with count: %d", count); - - if(compassEnabled) - { - //LOGD("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) { - //LOGD("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 { - LOGE("BoschYamaha: unknown event (type=%d, code=%d)", type, event->code); - - } - mInputReaderMagnetic.next(); - } - } - - //LOGD("Sensor: Compass read quited, count: %d", count); - - - if(accelEnabled) - { - //LOGD("Sensor: Accel is enabled, going to take care of it, count: %d", count); - - ssize_t n = mInputReaderAccel.fill(data_fd); - //LOGD("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) { - //LOGD("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) { - //LOGD("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 { - LOGE("BoschYamaha: unknown event (type=%d, code=%d)", type, event->code); - - } - mInputReaderAccel.next(); - } - } - - //LOGD("Sensor: Accel read quited, count: %d, acceldata Ready: %d, compassData Ready = %d", count, accelDataReady, compassDataReady); - - if( (accelDataReady == 1) && (compassDataReady == 1)){ - //LOGD("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); - } - - //LOGD("BoschYamaha: azimuth is %d", (int)(euler[0])); - //LOGD("BoschYamaha: pitch is %d", (int)(euler[1])); - //LOGD("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; -} - - - +/* + * 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#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) + 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 + LOGD("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"); + LOGD("BoschYamaha::~enable Compass(0, %d) open %s",en, input_sysfs_path); + fd = open(input_sysfs_path, O_RDWR); + if (fd >= 0) { + LOGD("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 + LOGD("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"); + LOGD("BoschYamaha::~enable Accel(0, %d) open %s",en, input_accel_sysfs_path); + fd = open(input_accel_sysfs_path, O_RDWR); + if (fd >= 0) { + LOGD("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() { + + //LOGD("hasPendingEvents was called"); + if(accelEnabled || compassEnabled){ + //LOGD("hasPendingEvents will return true"); + return true; + } + else { + //LOGD("hasPendingEvents will return false"); + return false; + } +} + + + +int BoschYamaha::readEvents(sensors_event_t* data, int count) +{ + if (count < 1) + return -EINVAL; + + + int numEventReceived = 0; + + //LOGD("Sensor: Read events was called with count: %d", count); + + if(compassEnabled) + { + //LOGD("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) { + //LOGD("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 { + LOGE("BoschYamaha: unknown event (type=%d, code=%d)", type, event->code); + + } + mInputReaderMagnetic.next(); + } + } + + //LOGD("Sensor: Compass read quited, count: %d", count); + + + if(accelEnabled) + { + //LOGD("Sensor: Accel is enabled, going to take care of it, count: %d", count); + + ssize_t n = mInputReaderAccel.fill(data_fd); + //LOGD("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) { + //LOGD("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) { + //LOGD("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 { + LOGE("BoschYamaha: unknown event (type=%d, code=%d)", type, event->code); + + } + mInputReaderAccel.next(); + } + } + + //LOGD("Sensor: Accel read quited, count: %d, acceldata Ready: %d, compassData Ready = %d", count, accelDataReady, compassDataReady); + + if( (accelDataReady == 1) && (compassDataReady == 1)){ + //LOGD("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); + } + + //LOGD("BoschYamaha: azimuth is %d", (int)(euler[0])); + //LOGD("BoschYamaha: pitch is %d", (int)(euler[1])); + //LOGD("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; +} + + + -- cgit v1.1