From af6a75109ed68e89998f221ba27fe15980f01714 Mon Sep 17 00:00:00 2001 From: Paul Kocialkowski Date: Mon, 15 Jun 2015 16:22:16 +0200 Subject: espresso-common: libsensors: sync with replicant Change-Id: Idcb9a57af699833c504f236692a6bdb1ef33c98e --- libsensors/orientationd/bma250.c | 85 ++++++ libsensors/orientationd/input.c | 230 +++++++++++++++- libsensors/orientationd/orientationd.c | 477 +++++++++++++++------------------ libsensors/orientationd/orientationd.h | 72 +++-- libsensors/orientationd/vector.c | 61 ----- libsensors/orientationd/yas530.c | 85 ++++++ 6 files changed, 660 insertions(+), 350 deletions(-) create mode 100644 libsensors/orientationd/bma250.c delete mode 100644 libsensors/orientationd/vector.c create mode 100644 libsensors/orientationd/yas530.c (limited to 'libsensors/orientationd') diff --git a/libsensors/orientationd/bma250.c b/libsensors/orientationd/bma250.c new file mode 100644 index 0000000..88819ea --- /dev/null +++ b/libsensors/orientationd/bma250.c @@ -0,0 +1,85 @@ +/* + * 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 "orientationd" +#include + +#include "orientationd.h" + +float bma250_convert(int value) +{ + return value * (GRAVITY_EARTH / 256.0f); +} + +int bma250_get_data(struct orientationd_handlers *handlers, + struct orientationd_data *data) +{ + struct input_event input_event; + int input_fd; + int rc; + + if (handlers == NULL || data == NULL) + return -EINVAL; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -1; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + switch (input_event.code) { + case ABS_X: + data->acceleration.x = bma250_convert(input_event.value); + break; + case ABS_Y: + data->acceleration.y = bma250_convert(input_event.value); + break; + case ABS_Z: + data->acceleration.z = bma250_convert(input_event.value); + break; + default: + continue; + } + } + } while (input_event.type != EV_SYN); + + return 0; +} + +struct orientationd_handlers bma250 = { + .input_name = "accelerometer", + .handle = SENSOR_TYPE_ACCELEROMETER, + .poll_fd = -1, + .get_data = bma250_get_data, +}; diff --git a/libsensors/orientationd/input.c b/libsensors/orientationd/input.c index eea1494..1afa2b8 100644 --- a/libsensors/orientationd/input.c +++ b/libsensors/orientationd/input.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Paul Kocialkowski + * 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 @@ -16,28 +16,117 @@ */ #include -#include -#include #include -#include -#include +#include #include #include - +#include #include #include +#include + +#define LOG_TAG "orientationd" +#include #include "orientationd.h" -int64_t input_timestamp(struct input_event *event) +void input_event_set(struct input_event *event, int type, int code, int value) { if (event == NULL) + return; + + memset(event, 0, sizeof(struct input_event)); + + event->type = type, + event->code = code; + event->value = value; + + gettimeofday(&event->time, NULL); +} + +long int timestamp(struct timeval *time) +{ + if (time == NULL) return -1; - return event->time.tv_sec*1000000000LL + event->time.tv_usec*1000; + return time->tv_sec * 1000000000LL + time->tv_usec * 1000; } -int input_open(char *name, int write) +long int input_timestamp(struct input_event *event) +{ + if (event == NULL) + return -1; + + return timestamp(&event->time); +} + +int uinput_rel_create(const char *name) +{ + struct uinput_user_dev uinput_dev; + int uinput_fd; + int rc; + + if (name == NULL) + return -1; + + uinput_fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); + if (uinput_fd < 0) { + ALOGE("%s: Unable to open uinput device", __func__); + goto error; + } + + memset(&uinput_dev, 0, sizeof(uinput_dev)); + + strncpy(uinput_dev.name, name, sizeof(uinput_dev.name)); + uinput_dev.id.bustype = BUS_I2C; + uinput_dev.id.vendor = 0; + uinput_dev.id.product = 0; + uinput_dev.id.version = 0; + + rc = 0; + rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_REL); + 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_EVBIT, EV_SYN); + + if (rc < 0) { + ALOGE("%s: Unable to set uinput bits", __func__); + goto error; + } + + rc = write(uinput_fd, &uinput_dev, sizeof(uinput_dev)); + if (rc < 0) { + ALOGE("%s: Unable to write uinput device", __func__); + goto error; + } + + rc = ioctl(uinput_fd, UI_DEV_CREATE); + if (rc < 0) { + ALOGE("%s: Unable to create uinput device", __func__); + goto error; + } + + usleep(3000); + + return uinput_fd; + +error: + if (uinput_fd >= 0) + close(uinput_fd); + + return -1; +} + +void uinput_destroy(int uinput_fd) +{ + if (uinput_fd < 0) + return; + + ioctl(uinput_fd, UI_DEV_DESTROY); +} + +int input_open(char *name) { DIR *d; struct dirent *di; @@ -60,7 +149,7 @@ int input_open(char *name, int write) continue; snprintf(path, PATH_MAX, "/dev/input/%s", di->d_name); - fd = open(path, write ? O_RDWR : O_RDONLY | O_NONBLOCK); + fd = open(path, O_RDWR | O_NONBLOCK); if (fd < 0) continue; @@ -123,3 +212,124 @@ int sysfs_path_prefix(char *name, char *path_prefix) return -1; } + +int sysfs_value_read(char *path) +{ + char buffer[100]; + int value; + int fd = -1; + int rc; + + if (path == NULL) + return -1; + + fd = open(path, O_RDONLY); + if (fd < 0) + goto error; + + rc = read(fd, &buffer, sizeof(buffer)); + if (rc <= 0) + goto error; + + value = atoi(buffer); + goto complete; + +error: + value = -1; + +complete: + if (fd >= 0) + close(fd); + + return value; +} + +int sysfs_value_write(char *path, int value) +{ + char buffer[100]; + int fd = -1; + int rc; + + if (path == NULL) + return -1; + + fd = open(path, O_WRONLY); + if (fd < 0) + goto error; + + snprintf((char *) &buffer, sizeof(buffer), "%d\n", value); + + rc = write(fd, buffer, strlen(buffer)); + if (rc < (int) strlen(buffer)) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +int sysfs_string_read(char *path, char *buffer, size_t length) +{ + int fd = -1; + int rc; + + if (path == NULL || buffer == NULL || length == 0) + return -1; + + fd = open(path, O_RDONLY); + if (fd < 0) + goto error; + + rc = read(fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +int sysfs_string_write(char *path, char *buffer, size_t length) +{ + int fd = -1; + int rc; + + if (path == NULL || buffer == NULL || length == 0) + return -1; + + fd = open(path, O_WRONLY); + if (fd < 0) + goto error; + + rc = write(fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} 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 * * 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 . */ -#include #include -#include +#include +#include #include #include #include -#include #include -#include -#include -#include - -#include "orientationd.h" - #include +#include -#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 -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; } diff --git a/libsensors/orientationd/orientationd.h b/libsensors/orientationd/orientationd.h index 41aad5e..7e169e4 100644 --- a/libsensors/orientationd/orientationd.h +++ b/libsensors/orientationd/orientationd.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Paul Kocialkowski + * 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 @@ -15,36 +15,72 @@ * along with this program. If not, see . */ +#include +#include #include -#ifndef _ORIENTATION_H_ -#define _ORIENTATION_H_ +#include +#include -/* - * Vector - */ +#ifndef _ORIENTATIOND_H_ +#define _ORIENTATIOND_H_ -struct vector { - float x; - float y; - float z; +struct orientationd_data; +struct orientationd_handlers { + char *input_name; + int handle; + int poll_fd; + + int (*get_data)(struct orientationd_handlers *handlers, + struct orientationd_data *data); }; -typedef struct vector vector; +struct orientationd_data { + struct orientationd_handlers **handlers; + int handlers_count; + + struct pollfd *poll_fds; + int poll_fds_count; -void vector_add(vector *v, vector *a); -void vector_multiply(vector *v, float k); -void vector_cross(vector *v, vector *c, vector *out); -float vector_scalar(vector *v, vector *d); -float vector_length(vector *v); + sensors_vec_t orientation; + sensors_vec_t acceleration; + sensors_vec_t magnetic; + + unsigned int delay; + int input_fd; + + int activated; + + pthread_t thread; + pthread_mutex_t mutex; + int thread_continue; +}; + +extern struct orientationd_handlers *orientationd_handlers[]; +extern int orientationd_handlers_count; /* * Input */ -int64_t input_timestamp(struct input_event *event); -int input_open(char *name, int write); +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); +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); +int sysfs_string_read(char *path, char *buffer, size_t length); +int sysfs_string_write(char *path, char *buffer, size_t length); + +/* + * Sensors + */ + +extern struct orientationd_handlers bma250; +extern struct orientationd_handlers yas530; #endif diff --git a/libsensors/orientationd/vector.c b/libsensors/orientationd/vector.c deleted file mode 100644 index ee8ba49..0000000 --- a/libsensors/orientationd/vector.c +++ /dev/null @@ -1,61 +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 "orientationd.h" - -void vector_copy(vector *in, vector *out) -{ - out->x = in->x; - out->y = in->y; - out->z = in ->z; -} - -void vector_add(vector *v, vector *a) -{ - v->x += a->x; - v->y += a->y; - v->z += a->z; -} - -void vector_multiply(vector *v, float k) -{ - v->x *= k; - v->y *= k; - v->z *= k; -} - -void vector_cross(vector *v, vector *c, vector *out) -{ - struct vector t; - - t.x = v->x * c->z - v->z * c->y; - t.y = v->z * c->x - v->x * c->z; - t.y = v->y * c->y - v->y * c->x; - vector_copy(&t, out); -} - -float vector_scalar(vector *v, vector *d) -{ - return v->x * d->x + v->y * d->y + v->z * d->z; -} - -float vector_length(vector *v) -{ - return sqrtf(vector_scalar(v, v)); -} diff --git a/libsensors/orientationd/yas530.c b/libsensors/orientationd/yas530.c new file mode 100644 index 0000000..7942c50 --- /dev/null +++ b/libsensors/orientationd/yas530.c @@ -0,0 +1,85 @@ +/* + * 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 "orientationd" +#include + +#include "orientationd.h" + +float yas530_convert(int value) +{ + return value / 1000.0f; +} + +int yas530_get_data(struct orientationd_handlers *handlers, + struct orientationd_data *data) +{ + struct input_event input_event; + int input_fd; + int rc; + + if (handlers == NULL || data == NULL) + return -EINVAL; + + input_fd = handlers->poll_fd; + if (input_fd < 0) + return -1; + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + switch (input_event.code) { + case ABS_X: + data->magnetic.x = yas530_convert(input_event.value); + break; + case ABS_Y: + data->magnetic.y = yas530_convert(input_event.value); + break; + case ABS_Z: + data->magnetic.z = yas530_convert(input_event.value); + break; + default: + continue; + } + } + } while (input_event.type != EV_SYN); + + return 0; +} + +struct orientationd_handlers yas530 = { + .input_name = "geomagnetic", + .handle = SENSOR_TYPE_MAGNETIC_FIELD, + .poll_fd = -1, + .get_data = yas530_get_data, +}; -- cgit v1.1