diff options
Diffstat (limited to 'libsensors/orientationd/orientationd.c')
-rw-r--r-- | libsensors/orientationd/orientationd.c | 477 |
1 files changed, 216 insertions, 261 deletions
diff --git a/libsensors/orientationd/orientationd.c b/libsensors/orientationd/orientationd.c index 7cc9f0a..8ad84c5 100644 --- a/libsensors/orientationd/orientationd.c +++ b/libsensors/orientationd/orientationd.c @@ -1,8 +1,5 @@ /* - * Copyright (C) 2013 Paul Kocialkowski - * - * Orientation calculation based on AK8975_FS: - * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -18,56 +15,57 @@ * along with this program. If not, see <http://www.gnu.org/licenses/>. */ -#include <stdio.h> #include <stdlib.h> -#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> #include <errno.h> #include <poll.h> #include <math.h> -#include <linux/ioctl.h> #include <linux/input.h> -#include <sys/types.h> -#include <sys/stat.h> -#include <fcntl.h> - -#include "orientationd.h" - #include <hardware/sensors.h> +#include <hardware/hardware.h> -#define ABS_CONTROL_REPORT (ABS_THROTTLE) - -#define FLAG_X (1 << 0) -#define FLAG_Y (1 << 1) -#define FLAG_Z (1 << 2) -#define FLAG_ALL (FLAG_X | FLAG_Y | FLAG_Z) +#define LOG_TAG "orientationd" +#include <utils/Log.h> -struct sensor_device { - char *name; - int handle; - float (*get_data)(int value); - int (*set_data)(float value); +#include "orientationd.h" - int fd; +struct orientationd_handlers *orientationd_handlers[] = { + &bma250, + &yas530, }; -struct sensor_data { - vector v; - int flags; -}; +int orientationd_handlers_count = sizeof(orientationd_handlers) / + sizeof(struct orientationd_handlers *); -float rad2deg(float v) +static float rad2deg(float v) { return (v * 180.0f / 3.1415926535f); } -void orientation_calculate(vector *a, vector *m, vector *o) +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)); +} + +int orientation_calculate(struct orientationd_data *data) { + sensors_vec_t *a, *m, *o; float azimuth, pitch, roll; float la, sinp, cosp, sinr, cosr, x, y; - if (a == NULL || m == NULL || o == NULL) - return; + if (data == NULL) + return -EINVAL; + + a = &data->acceleration; + m = &data->magnetic; + o = &data->orientation; la = vector_length(a); pitch = asinf(-(a->y) / la); @@ -82,291 +80,248 @@ void orientation_calculate(vector *a, vector *m, vector *o) x = m->x * sinp * sinr + m->y * cosp + m->z * sinp * cosr; azimuth = atan2f(y, x); - o->x = rad2deg(azimuth); - o->y = rad2deg(pitch); - o->z = rad2deg(roll); + o->azimuth = rad2deg(azimuth); + o->pitch = rad2deg(pitch); + o->roll = rad2deg(roll); - if (o->x < 0) - o->x += 360.0f; -} + if (o->azimuth < 0) + o->azimuth += 360.0f; -float bma250_acceleration(int value) -{ - return (float) (value * GRAVITY_EARTH) / 256.0f; -} - -float yas530c_magnetic(int value) -{ - return (float) value / 1000.0f; + return 0; } -int yas_orientation(float value) +void *orientationd_thread(void *thread_data) { - return (int) (value * 1000); -} + struct orientationd_data *data; + struct input_event event; + struct timeval time; + long int before, after; + int diff; + int input_fd; + int rc; + + if (thread_data == NULL) + return NULL; + + data = (struct orientationd_data *) thread_data; + + input_fd = data->input_fd; + if (input_fd < 0) + return NULL; + + while (data->thread_continue) { + pthread_mutex_lock(&data->mutex); + if (!data->thread_continue) + break; + + while (data->activated) { + gettimeofday(&time, NULL); + before = timestamp(&time); + + rc = orientation_calculate(data); + if (rc < 0) { + ALOGE("%s: Unable to calculate orientation", __func__); + goto next; + } -struct sensor_device bma250_device = { - .name = "accelerometer", - .handle = SENSOR_TYPE_ACCELEROMETER, - .get_data = bma250_acceleration, - .set_data = NULL, - .fd = -1, -}; + input_event_set(&event, EV_ABS, ABS_X, (int) (data->orientation.azimuth * 1000)); + write(input_fd, &event, sizeof(event)); + input_event_set(&event, EV_ABS, ABS_Y, (int) (data->orientation.pitch * 1000)); + write(input_fd, &event, sizeof(event)); + input_event_set(&event, EV_ABS, ABS_Z, (int) (data->orientation.roll * 1000)); + write(input_fd, &event, sizeof(event)); + input_event_set(&event, EV_SYN, 0, 0); + write(input_fd, &event, sizeof(event)); -struct sensor_device yas530c_device = { - .name = "geomagnetic", - .handle = SENSOR_TYPE_MAGNETIC_FIELD, - .get_data = yas530c_magnetic, - .set_data = NULL, - .fd = -1, -}; +next: + gettimeofday(&time, NULL); + after = timestamp(&time); -struct sensor_device yas_orientation_device = { - .name = "orientation", - .handle = SENSOR_TYPE_ORIENTATION, - .get_data = NULL, - .set_data = yas_orientation, - .fd = -1, -}; - -struct sensor_device *sensor_devices[] = { - &bma250_device, - &yas530c_device, - &yas_orientation_device, -}; + diff = (int) (data->delay * 1000000 - (after - before)) / 1000; + if (diff <= 0) + continue; -int sensors_devices_count = sizeof(sensor_devices) / sizeof(struct sensor_device *); + usleep(diff); + } + } + return NULL; +} -int sensor_device_open(struct sensor_device *dev) +int orientation_get_data(struct orientationd_data *data) { - int fd; + struct input_event input_event; + int input_fd; + int activated; + unsigned int delay; + int rc; - if (dev == NULL || dev->name == NULL) + if (data == NULL) return -EINVAL; - printf("Opening %s\n", dev->name); - - fd = input_open(dev->name, dev->handle == SENSOR_TYPE_ORIENTATION ? 1 : 0); - if (fd < 0) + input_fd = data->input_fd; + if (input_fd < 0) return -1; - dev->fd = fd; + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; - return 0; -} + if (input_event.type == EV_ABS) { + switch (input_event.code) { + case ABS_THROTTLE: + data->activated = input_event.value & (1 << 16) ? 1 : 0; + data->delay = input_event.value & ~(1 << 16); -void sensor_device_close(struct sensor_device *dev) -{ - if (dev == NULL || dev->fd < 0) - return; + pthread_mutex_unlock(&data->mutex); + break; + default: + continue; + } + } + } while (input_event.type != EV_SYN); - close(dev->fd); - dev->fd = -1; + return 0; } -struct sensor_device *sensor_device_find_handle(int handle) +int orientationd_poll(struct orientationd_data *data) { - int i; + int count; + int i, j; + int rc; - for (i=0 ; i < sensors_devices_count ; i++) { - if (sensor_devices[i]->handle == handle) - return sensor_devices[i]; - } + if (data == NULL) + return -EINVAL; - return NULL; -} + ALOGD("Starting orientationd poll"); -struct sensor_device *sensor_device_find_fd(int fd) -{ - int i; + while (1) { + if (data->activated) + count = data->poll_fds_count; + else + count = 1; - for (i=0 ; i < sensors_devices_count ; i++) { - if (sensor_devices[i]->fd == fd) - return sensor_devices[i]; - } + rc = poll(data->poll_fds, count, -1); + if (rc < 0) { + ALOGE("%s: poll failure", __func__); + goto error; + } - return NULL; -} + for (i = 0; i < count; i++) { + if (data->poll_fds[i].revents & POLLIN) { + data->poll_fds[i].revents = 0; -int sensor_device_get_data(struct sensor_device *dev, struct sensor_data *d, - struct input_event *e) -{ - if (dev == NULL || d == NULL || e == NULL || dev->get_data == NULL) - return -EINVAL; + if (data->poll_fds[i].fd == data->input_fd) { + orientation_get_data(data); + continue; + } - if (e->type == EV_ABS) { - switch (e->code) { - case ABS_X: - d->v.x = dev->get_data(e->value); - d->flags |= FLAG_X; - return 0; - case ABS_Y: - d->v.y = dev->get_data(e->value); - d->flags |= FLAG_Y; - return 0; - case ABS_Z: - d->v.z = dev->get_data(e->value); - d->flags |= FLAG_Z; - return 0; + for (j = 0; j < data->handlers_count; j++) + if (data->handlers[j] != NULL && data->handlers[j]->poll_fd == data->poll_fds[i].fd && data->handlers[j]->get_data != NULL) + data->handlers[j]->get_data(data->handlers[j], data); + } } } - return -1; -} + rc = 0; + goto complete; -int sensor_device_set_data(struct sensor_device *dev, struct sensor_data *d) -{ - struct input_event event; - - if (dev == NULL || d == NULL || dev->set_data == NULL) - return -EINVAL; - - event.type = EV_ABS; - event.code = ABS_X; - event.value = dev->set_data(d->v.x); - gettimeofday(&event.time, NULL); - write(dev->fd, &event, sizeof(event)); - - event.type = EV_ABS; - event.code = ABS_Y; - event.value = dev->set_data(d->v.y); - gettimeofday(&event.time, NULL); - write(dev->fd, &event, sizeof(event)); - - event.type = EV_ABS; - event.code = ABS_Z; - event.value = dev->set_data(d->v.z); - gettimeofday(&event.time, NULL); - write(dev->fd, &event, sizeof(event)); - - event.type = EV_SYN; - event.code = SYN_REPORT; - event.value = 0; - gettimeofday(&event.time, NULL); - write(dev->fd, &event, sizeof(event)); - - return 0; -} - -int sensor_device_control(struct sensor_device *dev, struct input_event *e) -{ - int enabled; - - if (dev == NULL || e == NULL) - return -EINVAL; - - if (e->type == EV_ABS && e->code == ABS_CONTROL_REPORT) { - enabled = e->value & (1 << 16); - if (enabled) - return 1; - else - return 0; - } +error: + rc = -1; - return -1; +complete: + return rc; } int main(int argc, char *argv[]) { - struct input_event event; - struct sensor_data a, m, o; - - struct sensor_device *dev; - struct pollfd *poll_fds; - - int enabled, data; - int index; - - int rc, c, i; - - memset(&a, 0, sizeof(a)); - memset(&m, 0, sizeof(m)); - memset(&o, 0, sizeof(o)); + struct orientationd_data *orientationd_data = NULL; + pthread_attr_t thread_attr; + int input_fd = -1; + int poll_fd; + int p, i; + int rc; + + orientationd_data = (struct orientationd_data *) + calloc(1, sizeof(struct orientationd_data)); + orientationd_data->handlers = orientationd_handlers; + orientationd_data->handlers_count = orientationd_handlers_count; + orientationd_data->activated = 0; + orientationd_data->poll_fds = (struct pollfd *) + calloc(1, (orientationd_handlers_count + 1) * sizeof(struct pollfd)); + + p = 0; + + input_fd = input_open("orientation"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; + } - poll_fds = (struct pollfd *) calloc(1, sizeof(struct pollfd) * sensors_devices_count); + orientationd_data->input_fd = input_fd; - index = -1; - c = 0; + orientationd_data->poll_fds[p].fd = input_fd; + orientationd_data->poll_fds[p].events = POLLIN; + p++; - for (i=0 ; i < sensors_devices_count ; i++) { - rc = sensor_device_open(sensor_devices[i]); - if (rc < 0) + for (i = 0; i < orientationd_handlers_count; i++) { + if (orientationd_handlers[i] == NULL || orientationd_handlers[i]->input_name == NULL) continue; - poll_fds[c].fd = sensor_devices[i]->fd; - poll_fds[c].events = POLLIN; - - if (sensor_devices[i]->handle == SENSOR_TYPE_ORIENTATION && index < 0) - index = c; + poll_fd = input_open(orientationd_handlers[i]->input_name); + if (poll_fd < 0) { + ALOGE("%s: Unable to open input %s", __func__, orientationd_handlers[i]->input_name); + continue; + } - c++; + orientationd_handlers[i]->poll_fd = poll_fd; + orientationd_data->poll_fds[p].fd = poll_fd; + orientationd_data->poll_fds[p].events = POLLIN; + p++; } - if (c <= 0 || index <= 0) - goto exit; + orientationd_data->poll_fds_count = p; - printf("Starting main loop\n"); + orientationd_data->thread_continue = 1; - enabled = 0; - while (1) { - data = 0; + pthread_mutex_init(&orientationd_data->mutex, NULL); + pthread_mutex_lock(&orientationd_data->mutex); - if (enabled) - rc = poll(poll_fds, c, -1); - else - rc = poll(&poll_fds[index], 1, -1); + pthread_attr_init(&thread_attr); + pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED); - if (rc < 0) - goto exit; + rc = pthread_create(&orientationd_data->thread, &thread_attr, orientationd_thread, (void *) orientationd_data); + if (rc < 0) { + ALOGE("%s: Unable to create orientationd thread", __func__); + goto error; + } - for (i=0 ; i < c ; i++) { - if (poll_fds[i].revents & POLLIN) { - dev = sensor_device_find_fd(poll_fds[i].fd); - if (dev == NULL) - continue; + rc = orientationd_poll(orientationd_data); + if (rc < 0) + goto error; - read(dev->fd, &event, sizeof(event)); - - switch (dev->handle) { - case SENSOR_TYPE_ACCELEROMETER: - rc = sensor_device_get_data(dev, &a, &event); - if (rc >= 0) - data = 1; - break; - case SENSOR_TYPE_MAGNETIC_FIELD: - rc = sensor_device_get_data(dev, &m, &event); - if (rc >= 0) - data = 1; - break; - case SENSOR_TYPE_ORIENTATION: - rc = sensor_device_control(dev, &event); - if (rc == 1) - enabled = 1; - else if (rc == 0) - enabled = 0; - break; - } - } + rc = 0; + goto complete; - if (data && a.flags & FLAG_ALL && m.flags & FLAG_ALL) { - dev = sensor_device_find_handle(SENSOR_TYPE_ORIENTATION); - if (dev == NULL) - continue; +error: + while (1) + sleep(3600); - orientation_calculate(&a.v, &m.v, &o.v); - sensor_device_set_data(dev, &o); - } - } - } + rc = 1; +complete: + if (input_fd >= 0) + close(input_fd); -exit: - for (i=0 ; i < sensors_devices_count ; i++) - sensor_device_close(sensor_devices[i]); + if (orientationd_data != NULL) { + orientationd_data->thread_continue = 0; + pthread_mutex_destroy(&orientationd_data->mutex); - while (1) { - sleep(3600); + if (orientationd_data->poll_fds != NULL) + free(orientationd_data->poll_fds); + + free(orientationd_data); } - return 0; + return rc; } |