summaryrefslogtreecommitdiffstats
path: root/libsensors/BoschYamaha.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors/BoschYamaha.cpp')
-rw-r--r--libsensors/BoschYamaha.cpp1000
1 files changed, 500 insertions, 500 deletions
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 <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
- 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 <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
+ 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;
+}
+
+
+
generated by cgit v1.1 at 2025-01-08 02:11:35 +0000