From bf94705b139245d287393c67ee5f004665fb3a72 Mon Sep 17 00:00:00 2001 From: Wolfgang Wiedmeyer Date: Thu, 8 Jun 2017 18:33:07 +0200 Subject: sensors bringup Various fixes are ported from the i9300 sensors. A list of commits, where fixes were taken from, is attached below. commit 0f8e8cc Author: Christian Balster Date: Wed May 20 13:19:48 2015 +0200 i9300: libsensors: add real accuracy reporting for magnetometer Change-Id: I3dd56f4d7fb0de8feca2422c165fe0c3cd92761a commit 2104318 Author: Christian Balster Date: Wed May 20 17:14:11 2015 +0200 i9300: libsensors: fix typo in accelerometer data Change-Id: Iee501b65c23bed22bfe769496bafc852f939a8ac commit b69dd87 Author: Christian Balster Date: Wed May 20 17:16:29 2015 +0200 i9300: libsensors: report at least bogus accuracy for gyro Change-Id: I81ff71c8ae6bf25c1aabac4a6d8d6fcf64bd2e0b commit d786475 Author: Christian Balster Date: Thu May 21 17:46:00 2015 +0200 i9300: libsensors: fix magnetometer offset parameters not being saved also fix a typo Change-Id: I69246a96c53d7ec02ca90d73bc85dec4cbc73343 commit f7d2dfd Author: Christian Balster Date: Tue Jun 2 15:08:02 2015 +0200 i9300: libsensors: remove unused orientation sensor The Orientation Sensor is deprecated and no longer required. Additionally this implementation is never even used, since Android automatically replaces it by it's own implementation if the sensor HAL doesn't also provide a Rotation Vector sensor. Furthermore the current implementation only uses the accelerometer and magnetometer for the sensor fusion and doesn't make use of the Gyroscope. So should we at some point decide to implement our own complete sensor fusion this would have to be rewritten anyway. Change-Id: I45d8a9afd2089b49131e6cc69cdf2f3dfee46c92 commit fc862f3 Author: Dheeraj CVR Date: Thu Jun 11 16:44:25 2015 +0400 libsensors: switch to portable typedefs to match callbacks Using a strict basetype only coding style is not a good idea especially when we are dealing with callbacks which use portable typedef like int64_t. This kind of coding style would most likely end up in overflows at various places especially when dealing with timestamps and sensor delays which are caused when typecasting datatypes of different size. Switch to portable typedefs and get rid of "long". Change-Id: I75b9cace7602345dba9095f046292e6d4db07df4 commit a484daf Author: Christian Balster Date: Mon Jun 15 17:46:29 2015 +0200 i9300: libsensors: fix possible overflow while setting delay Even though we changed the signature of the set_delay() methods, we can still get an overflow while actually writing the values to sysfs. Let's adapt sysfs_value_read() and sysfs_value_write() too. Change-Id: If8eda7204831f0edabec890d4e3127be520fa3bf commit 594d5b8 Author: Dheeraj CVR Date: Thu Jun 11 17:57:26 2015 +0400 libsensors: update sensor flags Change-Id: Ia57026f4e8f5dd270da7619bc25289fc414bce30 Signed-off-by: Wolfgang Wiedmeyer --- sensors/Android.mk | 1 - sensors/akm8975.c | 78 ++++---- sensors/bmp180.c | 5 +- sensors/gp2a_light.c | 6 +- sensors/gp2a_proximity.c | 2 +- sensors/input.c | 15 +- sensors/k3dh.c | 23 +-- sensors/k3g.c | 8 +- sensors/orientation.c | 444 --------------------------------------------- sensors/smdk4210_sensors.c | 26 +-- sensors/smdk4210_sensors.h | 16 +- 11 files changed, 93 insertions(+), 531 deletions(-) delete mode 100644 sensors/orientation.c diff --git a/sensors/Android.mk b/sensors/Android.mk index 026bfa6..3736c26 100644 --- a/sensors/Android.mk +++ b/sensors/Android.mk @@ -20,7 +20,6 @@ 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 \ diff --git a/sensors/akm8975.c b/sensors/akm8975.c index c46b823..9c22bde 100644 --- a/sensors/akm8975.c +++ b/sensors/akm8975.c @@ -39,12 +39,10 @@ #define AKFS_PAT PAT3 struct akm8975_data { - struct smdk4210_sensors_handlers *orientation_sensor; - AK8975PRMS akfs_params; sensors_vec_t magnetic; - long int delay; + int64_t delay; int device_fd; int uinput_fd; @@ -56,7 +54,8 @@ struct akm8975_data { int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_data) { AK8975PRMS *params; - int rc; + int rc, aocret; + float radius; if (akm8975_data == NULL || magnetic_data == NULL) return -EINVAL; @@ -80,7 +79,7 @@ int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_d // AOC for magnetometer // Offset estimation is done in this function - AKFS_AOC(¶ms->m_aocv, params->mfv_hdata, ¶ms->mfv_ho); + aocret = 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. @@ -99,9 +98,25 @@ int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_d return -1; } + // Check the size of magnetic vector + radius = sqrtf( + (params->mfv_hvec.u.x * params->mfv_hvec.u.x) + + (params->mfv_hvec.u.y * params->mfv_hvec.u.y) + + (params->mfv_hvec.u.z * params->mfv_hvec.u.z)); + + // Sanity check result and set accuracy + if ((radius > MAGNETIC_FIELD_EARTH_MAX + 10) || (radius < MAGNETIC_FIELD_EARTH_MIN - 10)) { + params->mi_hstatus = SENSOR_STATUS_UNRELIABLE; + } else if(params->mi_hstatus == SENSOR_STATUS_UNRELIABLE) { + params->mi_hstatus = SENSOR_STATUS_ACCURACY_MEDIUM; + } else if (aocret == AKFS_SUCCESS) { + params->mi_hstatus = SENSOR_STATUS_ACCURACY_HIGH; + } + 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; + akm8975_data->magnetic.status = params->mi_hstatus; return 0; } @@ -151,7 +166,7 @@ void *akm8975_thread(void *thread_data) char i2c_data[SENSOR_DATA_SIZE] = { 0 }; short magnetic_data[3]; short mode; - long int before, after; + int64_t before, after; int diff; int device_fd; int uinput_fd; @@ -223,6 +238,8 @@ void *akm8975_thread(void *thread_data) 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_REL, REL_MISC, (int) data->magnetic.status); + write(uinput_fd, &event, sizeof(event)); input_event_set(&event, EV_SYN, 0, 0); write(uinput_fd, &event, sizeof(event)); @@ -259,14 +276,6 @@ int akm8975_init(struct smdk4210_sensors_handlers *handlers, 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__); @@ -308,7 +317,7 @@ int akm8975_init(struct smdk4210_sensors_handlers *handlers, 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__); + ALOGE("%s: Unable to read akm8975 WIA data", __func__); goto error; } @@ -434,8 +443,15 @@ int akm8975_activate(struct smdk4210_sensors_handlers *handlers) // 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__); + if (rc != AKM_SUCCESS) { + ALOGE("%s: Unable to read AKFS HO parameters", __func__); + akfs_params->mfv_ho.u.x = 0.0f; + akfs_params->mfv_ho.u.y = 0.0f; + akfs_params->mfv_ho.u.z = 0.0f; + } else { + ALOGD("AKM8975 HO (Offset Adjustment) parameters read are: (%f, %f, %f)", + akfs_params->mfv_ho.u.x, akfs_params->mfv_ho.u.y, akfs_params->mfv_ho.u.z); + } // Initialize buffer AKFS_InitBuffer(AKFS_HDATA_SIZE, akfs_params->mfv_hdata); @@ -446,7 +462,7 @@ int akm8975_activate(struct smdk4210_sensors_handlers *handlers) // Initialize for AOC AKFS_InitAOC(&akfs_params->m_aocv); // Initialize magnetic status - akfs_params->mi_hstatus = 0; + akfs_params->mi_hstatus = SENSOR_STATUS_UNRELIABLE; handlers->activated = 1; pthread_mutex_unlock(&data->mutex); @@ -478,18 +494,20 @@ int akm8975_deactivate(struct smdk4210_sensors_handlers *handlers) empty = 1; - for (i = 0; i < 3; i++) { - if (akfs_params->mfv_ho.v[i] != 0) { - empty = 0; - break; - } + if ((akfs_params->mfv_ho.u.x != 0.0f) || (akfs_params->mfv_ho.u.y != 0.0f) || + (akfs_params->mfv_ho.u.z != 0.0f)) { + empty = 0; } 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__); + if (rc != AKM_SUCCESS) { + ALOGE("%s: Unable to write AKFS HO parameters", __func__); + } else { + ALOGD("AKM8975 HO (Offset Adjustment) parameters written are: (%f, %f, %f)", + akfs_params->mfv_ho.u.x, akfs_params->mfv_ho.u.y, akfs_params->mfv_ho.u.z); + } } mode = AK8975_MODE_POWER_DOWN; @@ -502,7 +520,7 @@ int akm8975_deactivate(struct smdk4210_sensors_handlers *handlers) return 0; } -int akm8975_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +int akm8975_set_delay(struct smdk4210_sensors_handlers *handlers, int64_t delay) { struct akm8975_data *data; @@ -547,8 +565,6 @@ int akm8975_get_data(struct smdk4210_sensors_handlers *handlers, 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)) @@ -565,6 +581,9 @@ int akm8975_get_data(struct smdk4210_sensors_handlers *handlers, case REL_Z: event->magnetic.z = akm8975_convert(input_event.value); break; + case REL_MISC: + event->magnetic.status = input_event.value; + break; default: continue; } @@ -574,9 +593,6 @@ int akm8975_get_data(struct smdk4210_sensors_handlers *handlers, } } while (input_event.type != EV_SYN); - if (data->orientation_sensor != NULL) - orientation_fill(data->orientation_sensor, NULL, &event->magnetic); - return 0; } diff --git a/sensors/bmp180.c b/sensors/bmp180.c index cae97fb..1054e55 100644 --- a/sensors/bmp180.c +++ b/sensors/bmp180.c @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -150,12 +151,12 @@ int bmp180_deactivate(struct smdk4210_sensors_handlers *handlers) return 0; } -int bmp180_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +int bmp180_set_delay(struct smdk4210_sensors_handlers *handlers, int64_t delay) { struct bmp180_data *data; int rc; - ALOGD("%s(%p, %ld)", __func__, handlers, delay); + ALOGD("%s(%p, %" PRId64 ")", __func__, handlers, delay); if (handlers == NULL || handlers->data == NULL) return -EINVAL; diff --git a/sensors/gp2a_light.c b/sensors/gp2a_light.c index 7c4040c..f6341cc 100644 --- a/sensors/gp2a_light.c +++ b/sensors/gp2a_light.c @@ -150,19 +150,19 @@ int gp2a_light_deactivate(struct smdk4210_sensors_handlers *handlers) return 0; } -int gp2a_light_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +int gp2a_light_set_delay(struct smdk4210_sensors_handlers *handlers, int64_t delay) { struct gp2a_light_data *data; int rc; - ALOGD("%s(%p, %ld)", __func__, handlers, delay); + ALOGD("%s(%p, %" PRId64 ")", __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); + rc = sysfs_value_write(data->path_delay, delay); if (rc < 0) { ALOGE("%s: Unable to write sysfs value", __func__); return -1; diff --git a/sensors/gp2a_proximity.c b/sensors/gp2a_proximity.c index d5fde88..357112c 100644 --- a/sensors/gp2a_proximity.c +++ b/sensors/gp2a_proximity.c @@ -147,7 +147,7 @@ int gp2a_proximity_deactivate(struct smdk4210_sensors_handlers *handlers) return 0; } -int gp2a_proximity_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +int gp2a_proximity_set_delay(struct smdk4210_sensors_handlers *handlers, int64_t delay) { ALOGD("%s(%p, %ld)", __func__, handlers, delay); diff --git a/sensors/input.c b/sensors/input.c index 1c1b26e..685bc80 100644 --- a/sensors/input.c +++ b/sensors/input.c @@ -44,7 +44,7 @@ void input_event_set(struct input_event *event, int type, int code, int value) gettimeofday(&event->time, NULL); } -long int timestamp(struct timeval *time) +int64_t timestamp(struct timeval *time) { if (time == NULL) return -1; @@ -52,7 +52,7 @@ long int timestamp(struct timeval *time) return time->tv_sec * 1000000000LL + time->tv_usec * 1000; } -long int input_timestamp(struct input_event *event) +int64_t input_timestamp(struct input_event *event) { if (event == NULL) return -1; @@ -88,6 +88,7 @@ int uinput_rel_create(const char *name) 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_RELBIT, REL_MISC); rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_SYN); if (rc < 0) { @@ -213,10 +214,10 @@ int sysfs_path_prefix(char *name, char *path_prefix) return -1; } -int sysfs_value_read(char *path) +int64_t sysfs_value_read(char *path) { char buffer[100]; - int value; + int64_t value; int fd = -1; int rc; @@ -231,7 +232,7 @@ int sysfs_value_read(char *path) if (rc <= 0) goto error; - value = atoi(buffer); + value = (int64_t)strtoimax(buffer, NULL, 10); goto complete; error: @@ -244,7 +245,7 @@ complete: return value; } -int sysfs_value_write(char *path, int value) +int sysfs_value_write(char *path, int64_t value) { char buffer[100]; int fd = -1; @@ -257,7 +258,7 @@ int sysfs_value_write(char *path, int value) if (fd < 0) goto error; - snprintf((char *) &buffer, sizeof(buffer), "%d\n", value); + snprintf((char *) &buffer, sizeof(buffer), "%" PRId64 "\n", value); rc = write(fd, buffer, strlen(buffer)); if (rc < (int) strlen(buffer)) diff --git a/sensors/k3dh.c b/sensors/k3dh.c index 6335122..7ea0ead 100644 --- a/sensors/k3dh.c +++ b/sensors/k3dh.c @@ -31,9 +31,7 @@ #include "k3dh.h" struct k3dh_data { - struct smdk4210_sensors_handlers *orientation_sensor; - - long int delay; + int64_t delay; int device_fd; int uinput_fd; @@ -49,7 +47,7 @@ void *k3dh_thread(void *thread_data) struct input_event event; struct timeval time; struct k3dh_acceldata acceleration_data; - long int before, after; + int64_t before, after; int diff; int device_fd; int uinput_fd; @@ -130,14 +128,6 @@ int k3dh_init(struct smdk4210_sensors_handlers *handlers, 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__); @@ -301,14 +291,14 @@ int k3dh_deactivate(struct smdk4210_sensors_handlers *handlers) return 0; } -int k3dh_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +int k3dh_set_delay(struct smdk4210_sensors_handlers *handlers, int64_t delay) { struct k3dh_data *data; int64_t d; int device_fd; int rc; - ALOGD("%s(%p, %ld)", __func__, handlers, delay); + ALOGD("%s(%p, %" PRId64 ")", __func__, handlers, delay); if (handlers == NULL || handlers->data == NULL) return -EINVAL; @@ -360,7 +350,7 @@ int k3dh_get_data(struct smdk4210_sensors_handlers *handlers, event->sensor = handlers->handle; event->type = handlers->handle; - event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; + event->acceleration.status = SENSOR_STATUS_ACCURACY_MEDIUM; do { rc = read(input_fd, &input_event, sizeof(input_event)); @@ -387,9 +377,6 @@ int k3dh_get_data(struct smdk4210_sensors_handlers *handlers, } } while (input_event.type != EV_SYN); - if (data->orientation_sensor != NULL) - orientation_fill(data->orientation_sensor, &event->acceleration, NULL); - return 0; } diff --git a/sensors/k3g.c b/sensors/k3g.c index 5a635e3..345be0a 100644 --- a/sensors/k3g.c +++ b/sensors/k3g.c @@ -152,19 +152,19 @@ int k3g_deactivate(struct smdk4210_sensors_handlers *handlers) return 0; } -int k3g_set_delay(struct smdk4210_sensors_handlers *handlers, long int delay) +int k3g_set_delay(struct smdk4210_sensors_handlers *handlers, int64_t delay) { struct k3g_data *data; int rc; - ALOGD("%s(%p, %ld)", __func__, handlers, delay); + ALOGD("%s(%p, %" PRId64 ")", __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); + rc = sysfs_value_write(data->path_delay, delay); if (rc < 0) { ALOGE("%s: Unable to write sysfs value", __func__); return -1; @@ -206,6 +206,8 @@ int k3g_get_data(struct smdk4210_sensors_handlers *handlers, event->gyro.y = data->gyro.y; event->gyro.z = data->gyro.z; + event->gyro.status = SENSOR_STATUS_ACCURACY_MEDIUM; + do { rc = read(input_fd, &input_event, sizeof(input_event)); if (rc < (int) sizeof(input_event)) diff --git a/sensors/orientation.c b/sensors/orientation.c deleted file mode 100644 index fe50605..0000000 --- a/sensors/orientation.c +++ /dev/null @@ -1,444 +0,0 @@ -/* - * Copyright (C) 2013 Paul Kocialkowski - * - * 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 . - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#define LOG_TAG "smdk4210_sensors" -#include - -#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 index 6e1b92f..113d215 100644 --- a/sensors/smdk4210_sensors.c +++ b/sensors/smdk4210_sensors.c @@ -36,19 +36,22 @@ 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, {}, }, + SENSOR_TYPE_ACCELEROMETER, 2 * GRAVITY_EARTH, GRAVITY_EARTH / 90.0f / 8.0f, 0.23f, 20000, 0, 0, }, + { "AKM8975C 3-Axis Magnetic Sensor", "Asahi Kasei", 1, SENSOR_TYPE_MAGNETIC_FIELD, + SENSOR_TYPE_MAGNETIC_FIELD, 2000.0f, 1.0f / 16, 6.8f, 16667, 0, 0, SENSOR_STRING_TYPE_MAGNETIC_FIELD, 0, 0, + SENSOR_FLAG_CONTINUOUS_MODE, {}, }, { "GP2A Light Sensor", "Sharp", 1, SENSOR_TYPE_LIGHT, - SENSOR_TYPE_LIGHT, 10240.0f, 1.0f, 0.75f, 0, {}, }, + SENSOR_TYPE_LIGHT, 10240.0f, 1.0f, 0.75f, 0, 0, 0, SENSOR_STRING_TYPE_LIGHT, 0, 0, + SENSOR_FLAG_ON_CHANGE_MODE, {}, }, { "GP2A Proximity Sensor", "Sharp", 1, SENSOR_TYPE_PROXIMITY, - SENSOR_TYPE_PROXIMITY, 5.0f, 5.0f, 0.75f, 0, {}, }, + SENSOR_TYPE_PROXIMITY, 5.0f, 5.0f, 0.75f, 0, 0, 0, SENSOR_STRING_TYPE_PROXIMITY, 0, 0, + SENSOR_FLAG_WAKE_UP | SENSOR_FLAG_ON_CHANGE_MODE, {}, }, { "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, {}, }, + SENSOR_TYPE_GYROSCOPE, 500.0f * (3.1415926535f / 180.0f), (70.0f / 4000.0f) * (3.1415926535f / 180.0f), 6.1f, 1190, 0, 0, SENSOR_STRING_TYPE_GYROSCOPE, 0, 0, + SENSOR_FLAG_CONTINUOUS_MODE, {}, }, { "BMP180 Pressure Sensor", "Bosch", 1, SENSOR_TYPE_PRESSURE, - SENSOR_TYPE_PRESSURE, 1100.0f, 0.01f, 0.67f, 20000, {}, }, + SENSOR_TYPE_PRESSURE, 1100.0f, 0.01f, 0.67f, 20000, 0, 0, SENSOR_STRING_TYPE_PRESSURE, 0, 20000, + SENSOR_FLAG_CONTINUOUS_MODE, {}, }, }; int smdk4210_sensors_count = sizeof(smdk4210_sensors) / sizeof(struct sensor_t); @@ -56,7 +59,6 @@ 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, @@ -116,7 +118,7 @@ int smdk4210_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, struct smdk4210_sensors_device *device; int i; - ALOGD("%s(%p, %d, %ld)", __func__, dev, handle, (long int) ns); + ALOGD("%s(%p, %d, %" PRId64 ")", __func__, dev, handle, ns); if (dev == NULL) return -EINVAL; @@ -131,7 +133,7 @@ int smdk4210_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, 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 device->handlers[i]->set_delay(device->handlers[i], ns); } return 0; diff --git a/sensors/smdk4210_sensors.h b/sensors/smdk4210_sensors.h index fdbb0cb..0dbc028 100644 --- a/sensors/smdk4210_sensors.h +++ b/sensors/smdk4210_sensors.h @@ -18,6 +18,8 @@ #include #include #include +#define __STDC_FORMAT_MACROS +#include #include #include @@ -40,7 +42,7 @@ struct smdk4210_sensors_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); + int64_t delay); int (*get_data)(struct smdk4210_sensors_handlers *handlers, struct sensors_event_t *event); @@ -76,14 +78,14 @@ int smdk4210_sensors_poll(struct sensors_poll_device_t *dev, */ 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); +int64_t timestamp(struct timeval *time); +int64_t 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); +int64_t sysfs_value_read(char *path); +int sysfs_value_write(char *path, int64_t value); int sysfs_string_read(char *path, char *buffer, size_t length); int sysfs_string_write(char *path, char *buffer, size_t length); @@ -91,12 +93,8 @@ 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; -- cgit v1.1