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/Android.mk | 23 +- libsensors/bh1721.c | 105 +++--- libsensors/bma250.c | 155 ++++----- libsensors/geomagneticd/geomagneticd.c | 619 ++++++++++++++++++++------------- libsensors/geomagneticd/geomagneticd.h | 59 ++++ libsensors/geomagneticd/input.c | 335 ++++++++++++++++++ libsensors/gp2a_light.c | 113 +++--- libsensors/gp2a_proximity.c | 93 +++-- libsensors/input.c | 216 +++++++++++- 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 +++++ libsensors/piranha_sensors.c | 60 ++-- libsensors/piranha_sensors.h | 41 ++- libsensors/yas530.c | 260 ++++++++++++++ libsensors/yas530c.c | 293 ---------------- libsensors/yas_orientation.c | 286 ++++++--------- 20 files changed, 2290 insertions(+), 1378 deletions(-) create mode 100644 libsensors/geomagneticd/geomagneticd.h create mode 100644 libsensors/geomagneticd/input.c create mode 100644 libsensors/orientationd/bma250.c delete mode 100644 libsensors/orientationd/vector.c create mode 100644 libsensors/orientationd/yas530.c create mode 100644 libsensors/yas530.c delete mode 100644 libsensors/yas530c.c (limited to 'libsensors') diff --git a/libsensors/Android.mk b/libsensors/Android.mk index 45edc5f..39cc493 100644 --- a/libsensors/Android.mk +++ b/libsensors/Android.mk @@ -1,5 +1,4 @@ -# -# 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 @@ -13,7 +12,6 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . -# LOCAL_PATH := $(call my-dir) PIRANHA_SENSORS_PATH := $(LOCAL_PATH) @@ -23,12 +21,15 @@ include $(CLEAR_VARS) LOCAL_SRC_FILES := \ piranha_sensors.c \ input.c \ - bma250.c \ - yas530c.c \ - yas_orientation.c \ bh1721.c \ + bma250.c \ gp2a_light.c \ - gp2a_proximity.c + gp2a_proximity.c \ + yas530.c \ + yas_orientation.c + +LOCAL_C_INCLUDES := \ + $(LOCAL_PATH) LOCAL_SHARED_LIBRARIES := libutils libcutils liblog libhardware LOCAL_PRELINK_MODULE := false @@ -57,7 +58,8 @@ LOCAL_PATH := $(PIRANHA_SENSORS_PATH)/geomagneticd include $(CLEAR_VARS) LOCAL_SRC_FILES := \ - geomagneticd.c + geomagneticd.c \ + input.c LOCAL_SHARED_LIBRARIES := libutils libcutils liblog LOCAL_PRELINK_MODULE := false @@ -72,9 +74,10 @@ LOCAL_PATH := $(PIRANHA_SENSORS_PATH)/orientationd include $(CLEAR_VARS) LOCAL_SRC_FILES := \ + orientationd.c \ input.c \ - vector.c \ - orientationd.c + bma250.c \ + yas530.c LOCAL_SHARED_LIBRARIES := libutils libcutils liblog LOCAL_PRELINK_MODULE := false diff --git a/libsensors/bh1721.c b/libsensors/bh1721.c index 1486ef9..34d4594 100644 --- a/libsensors/bh1721.c +++ b/libsensors/bh1721.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 @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -36,7 +38,8 @@ struct bh1721_data { char path_delay[PATH_MAX]; }; -int bh1721_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device) +int bh1721_init(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device) { struct bh1721_data *data = NULL; char path[PATH_MAX] = { 0 }; @@ -48,6 +51,8 @@ int bh1721_init(struct piranha_sensors_handlers *handlers, struct piranha_sensor if (handlers == NULL) return -EINVAL; + data = (struct bh1721_data *) calloc(1, sizeof(struct bh1721_data)); + input_fd = input_open("light_sensor"); if (input_fd < 0) { ALOGE("%s: Unable to open input", __func__); @@ -60,8 +65,6 @@ int bh1721_init(struct piranha_sensors_handlers *handlers, struct piranha_sensor goto error; } - data = (struct bh1721_data *) calloc(1, sizeof(struct bh1721_data)); - snprintf(data->path_enable, PATH_MAX, "%s/enable", path); snprintf(data->path_delay, PATH_MAX, "%s/poll_delay", path); @@ -71,12 +74,12 @@ int bh1721_init(struct piranha_sensors_handlers *handlers, struct piranha_sensor return 0; error: - if (input_fd >= 0) - close(input_fd); - if (data != NULL) free(data); + if (input_fd >= 0) + close(input_fd); + handlers->poll_fd = -1; handlers->data = NULL; @@ -85,22 +88,17 @@ error: int bh1721_deinit(struct piranha_sensors_handlers *handlers) { - int input_fd; - ALOGD("%s(%p)", __func__, handlers); if (handlers == NULL) return -EINVAL; - input_fd = handlers->poll_fd; - if (input_fd >= 0) - close(input_fd); - + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); handlers->poll_fd = -1; if (handlers->data != NULL) free(handlers->data); - handlers->data = NULL; return 0; @@ -109,8 +107,7 @@ int bh1721_deinit(struct piranha_sensors_handlers *handlers) int bh1721_activate(struct piranha_sensors_handlers *handlers) { struct bh1721_data *data; - char enable[] = "1\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -119,15 +116,12 @@ int bh1721_activate(struct piranha_sensors_handlers *handlers) data = (struct bh1721_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - handlers->activated = 1; return 0; @@ -136,8 +130,7 @@ int bh1721_activate(struct piranha_sensors_handlers *handlers) int bh1721_deactivate(struct piranha_sensors_handlers *handlers) { struct bh1721_data *data; - char enable[] = "0\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -146,54 +139,41 @@ int bh1721_deactivate(struct piranha_sensors_handlers *handlers) data = (struct bh1721_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - - handlers->activated = 0; + handlers->activated = 1; return 0; } -int bh1721_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay) +int bh1721_set_delay(struct piranha_sensors_handlers *handlers, long int delay) { struct bh1721_data *data; - char *value = NULL; - int c; - int fd; + int rc; -// ALOGD("%s(%p, %ld)", __func__, handlers, (long int) delay); + ALOGD("%s(%p, %ld)", __func__, handlers, delay); if (handlers == NULL || handlers->data == NULL) return -EINVAL; data = (struct bh1721_data *) handlers->data; - c = asprintf(&value, "%ld\n", (long int) delay); - - fd = open(data->path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); + rc = sysfs_value_write(data->path_delay, (int) delay); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, value, c); - close(fd); - - if (value != NULL) - free(value); - return 0; } -float bh1721_light(int value) +float bh1721_convert(int value) { - return (float) value * 0.712f; + return value * 0.712f; } int bh1721_get_data(struct piranha_sensors_handlers *handlers, @@ -203,6 +183,8 @@ int bh1721_get_data(struct piranha_sensors_handlers *handlers, int input_fd; int rc; +// ALOGD("%s(%p, %p)", __func__, handlers, event); + if (handlers == NULL || event == NULL) return -EINVAL; @@ -210,18 +192,24 @@ int bh1721_get_data(struct piranha_sensors_handlers *handlers, if (input_fd < 0) return -EINVAL; - rc = read(input_fd, &input_event, sizeof(input_event)); - if (rc < (int) sizeof(input_event)) - return -EINVAL; - - if (input_event.type != EV_ABS || input_event.code != ABS_MISC) - return -1; - + memset(event, 0, sizeof(struct sensors_event_t)); event->version = sizeof(struct sensors_event_t); event->sensor = handlers->handle; event->type = handlers->handle; - event->timestamp = input_timestamp(&input_event); - event->light = bh1721_light(input_event.value); + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_REL) { + if (input_event.code == REL_MISC) + event->light = bh1721_convert(input_event.value); + } 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; } @@ -236,6 +224,7 @@ struct piranha_sensors_handlers bh1721 = { .set_delay = bh1721_set_delay, .get_data = bh1721_get_data, .activated = 0, + .needed = 0, .poll_fd = -1, .data = NULL, }; diff --git a/libsensors/bma250.c b/libsensors/bma250.c index ce851d7..91d7fc0 100644 --- a/libsensors/bma250.c +++ b/libsensors/bma250.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 @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -31,11 +33,6 @@ #include "piranha_sensors.h" -#define FLAG_X (1 << 0) -#define FLAG_Y (1 << 1) -#define FLAG_Z (1 << 2) -#define FLAG_ALL (FLAG_X | FLAG_Y | FLAG_Z) - struct bma250_data { char path_enable[PATH_MAX]; char path_delay[PATH_MAX]; @@ -43,7 +40,8 @@ struct bma250_data { sensors_vec_t acceleration; }; -int bma250_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device) +int bma250_init(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device) { struct bma250_data *data = NULL; char path[PATH_MAX] = { 0 }; @@ -55,6 +53,8 @@ int bma250_init(struct piranha_sensors_handlers *handlers, struct piranha_sensor if (handlers == NULL) return -EINVAL; + data = (struct bma250_data *) calloc(1, sizeof(struct bma250_data)); + input_fd = input_open("accelerometer"); if (input_fd < 0) { ALOGE("%s: Unable to open input", __func__); @@ -67,8 +67,6 @@ int bma250_init(struct piranha_sensors_handlers *handlers, struct piranha_sensor goto error; } - data = (struct bma250_data *) calloc(1, sizeof(struct bma250_data)); - snprintf(data->path_enable, PATH_MAX, "%s/enable", path); snprintf(data->path_delay, PATH_MAX, "%s/delay", path); @@ -78,12 +76,12 @@ int bma250_init(struct piranha_sensors_handlers *handlers, struct piranha_sensor return 0; error: - if (input_fd >= 0) - close(input_fd); - if (data != NULL) free(data); + if (input_fd >= 0) + close(input_fd); + handlers->poll_fd = -1; handlers->data = NULL; @@ -92,22 +90,17 @@ error: int bma250_deinit(struct piranha_sensors_handlers *handlers) { - int input_fd; - ALOGD("%s(%p)", __func__, handlers); if (handlers == NULL) return -EINVAL; - input_fd = handlers->poll_fd; - if (input_fd >= 0) - close(input_fd); - + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); handlers->poll_fd = -1; if (handlers->data != NULL) free(handlers->data); - handlers->data = NULL; return 0; @@ -116,8 +109,7 @@ int bma250_deinit(struct piranha_sensors_handlers *handlers) int bma250_activate(struct piranha_sensors_handlers *handlers) { struct bma250_data *data; - char enable[] = "1\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -126,15 +118,12 @@ int bma250_activate(struct piranha_sensors_handlers *handlers) data = (struct bma250_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - handlers->activated = 1; return 0; @@ -143,8 +132,7 @@ int bma250_activate(struct piranha_sensors_handlers *handlers) int bma250_deactivate(struct piranha_sensors_handlers *handlers) { struct bma250_data *data; - char enable[] = "0\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -153,60 +141,47 @@ int bma250_deactivate(struct piranha_sensors_handlers *handlers) data = (struct bma250_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - - handlers->activated = 0; + handlers->activated = 1; return 0; } -int bma250_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay) +int bma250_set_delay(struct piranha_sensors_handlers *handlers, long int delay) { struct bma250_data *data; - char *value = NULL; int d; - int c; - int fd; + int rc; -// ALOGD("%s(%p, %ld)", __func__, handlers, (long int) delay); + ALOGD("%s(%p, %ld)", __func__, handlers, delay); if (handlers == NULL || handlers->data == NULL) return -EINVAL; data = (struct bma250_data *) handlers->data; - if (delay < 1000000) - d = 0; + if (delay < 10000000) + d = 10; else - d = (int) (delay / 1000000); + d = delay / 1000000; - c = asprintf(&value, "%d\n", d); - - fd = open(data->path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); + rc = sysfs_value_write(data->path_delay, d); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, value, c); - close(fd); - - if (value != NULL) - free(value); - return 0; } -float bma250_acceleration(int value) +float bma250_convert(int value) { - return (float) (value * GRAVITY_EARTH) / 256.0f; + return value * (GRAVITY_EARTH / 256.0f); } int bma250_get_data(struct piranha_sensors_handlers *handlers, @@ -215,9 +190,10 @@ int bma250_get_data(struct piranha_sensors_handlers *handlers, struct bma250_data *data; struct input_event input_event; int input_fd; - int flag; int rc; +// ALOGD("%s(%p, %p)", __func__, handlers, event); + if (handlers == NULL || handlers->data == NULL || event == NULL) return -EINVAL; @@ -227,6 +203,7 @@ int bma250_get_data(struct piranha_sensors_handlers *handlers, 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; @@ -234,46 +211,35 @@ int bma250_get_data(struct piranha_sensors_handlers *handlers, event->acceleration.x = data->acceleration.x; event->acceleration.y = data->acceleration.y; event->acceleration.z = data->acceleration.z; - event->acceleration.status = SENSOR_STATUS_ACCURACY_MEDIUM; - flag = 0; - while ((flag & FLAG_ALL) != FLAG_ALL) { + do { rc = read(input_fd, &input_event, sizeof(input_event)); - if (rc < (int) sizeof(input_event)) { - if (flag & FLAG_ALL) - break; - else - return -1; + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + switch (input_event.code) { + case ABS_X: + event->acceleration.x = bma250_convert(input_event.value); + break; + case ABS_Y: + event->acceleration.y = bma250_convert(input_event.value); + break; + case ABS_Z: + event->acceleration.z = bma250_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); - if (input_event.type != EV_ABS) - continue; - - switch (input_event.code) { - case ABS_X: - flag |= FLAG_X; - event->acceleration.x = bma250_acceleration(input_event.value); - break; - case ABS_Y: - flag |= FLAG_Y; - event->acceleration.y = bma250_acceleration(input_event.value); - break; - case ABS_Z: - flag |= FLAG_Z; - event->acceleration.z = bma250_acceleration(input_event.value); - break; - default: - continue; - } - event->timestamp = input_timestamp(&input_event); - } - - if (data->acceleration.x != event->acceleration.x) - data->acceleration.x = event->acceleration.x; - if (data->acceleration.y != event->acceleration.y) - data->acceleration.y = event->acceleration.y; - if (data->acceleration.z != event->acceleration.z) - data->acceleration.z = event->acceleration.z; + data->acceleration.x = event->acceleration.x; + data->acceleration.y = event->acceleration.y; + data->acceleration.z = event->acceleration.z; return 0; } @@ -288,6 +254,7 @@ struct piranha_sensors_handlers bma250 = { .set_delay = bma250_set_delay, .get_data = bma250_get_data, .activated = 0, + .needed = 0, .poll_fd = -1, .data = NULL, }; diff --git a/libsensors/geomagneticd/geomagneticd.c b/libsensors/geomagneticd/geomagneticd.c index 918edbf..61c4795 100644 --- a/libsensors/geomagneticd/geomagneticd.c +++ b/libsensors/geomagneticd/geomagneticd.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,353 +16,506 @@ */ #include -#include -#include +#include #include #include -#include -#include -#include +#include #include +#include +#include + #define LOG_TAG "geomagneticd" #include -/* - * This is a very intuitive implementation of what's going on with p5100/p3100 - * geomagneticd daemon. It seemed that geomagneticd sets an offset so that - * the biggest value (after setting the offset) is 45µT or negative -45µT. - * On the X axis, it happens more often to find the max around 40µT/-40µT. - * The reference offsets I used were: 5005 420432 1153869, and we're getting - * pretty close to this with that implementation. - * - */ +#include "geomagneticd.h" + +// This geomagnetic daemon is in charge of finding the correct calibration +// offsets to apply to the YAS530 magnetic field sensor. +// This is done by finding raw data extrema (minimum and maximum) for each axis +// and calculating the offset so that these values are -45uT and 45uT. /* - * Input + * Config */ -int input_open(char *name) +int geomagneticd_config_read(struct geomagneticd_data *data) { - DIR *d; - struct dirent *di; - - char input_name[80] = { 0 }; - char path[PATH_MAX]; - char *c; - int fd; + char buffer[100] = { 0 }; + int config_fd = -1; int rc; - if (name == NULL) + if (data == NULL) return -EINVAL; - d = opendir("/dev/input"); - if (d == NULL) - return -1; + config_fd = open(GEOMAGNETICD_CONFIG_PATH, O_RDONLY); + if (config_fd < 0) { + ALOGE("%s: Unable to open config", __func__); + goto error; + } - while ((di = readdir(d))) { - if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) - continue; + rc = read(config_fd, buffer, sizeof(buffer)); + if (rc <= 0) { + ALOGE("%s: Unable to read config", __func__); + goto error; + } - snprintf(path, PATH_MAX, "/dev/input/%s", di->d_name); - fd = open(path, O_RDONLY); - if (fd < 0) - continue; + rc = sscanf(buffer, "%d,%d,%d,%d,%d,%d,%d", + &data->hard_offsets[0], &data->hard_offsets[1], &data->hard_offsets[2], + &data->calib_offsets[0], &data->calib_offsets[1], &data->calib_offsets[2], + &data->accuracy); + if (rc != 7) { + ALOGE("%s: Unable to parse config", __func__); + goto error; + } - rc = ioctl(fd, EVIOCGNAME(sizeof(input_name) - 1), &input_name); - if (rc < 0) - continue; + rc = 0; + goto complete; - c = strstr((char *) &input_name, "\n"); - if (c != NULL) - *c = '\0'; +error: + rc = -1; - if (strcmp(input_name, name) == 0) - return fd; - else - close(fd); - } +complete: + if (config_fd >= 0) + close(config_fd); - return -1; + return rc; } -int sysfs_path_prefix(char *name, char *path_prefix) +int geomagneticd_config_write(struct geomagneticd_data *data) { - DIR *d; - struct dirent *di; - - char input_name[80] = { 0 }; - char path[PATH_MAX]; - char *c; - int fd; + char buffer[100] = { 0 }; + int config_fd = -1; + int rc; - if (name == NULL || path_prefix == NULL) + if (data == NULL) return -EINVAL; - d = opendir("/sys/class/input"); - if (d == NULL) - return -1; + config_fd = open(GEOMAGNETICD_CONFIG_BACKUP_PATH, O_WRONLY | O_TRUNC | O_CREAT, 0644); + if (config_fd < 0) { + ALOGE("%s: Unable to open config", __func__); + goto error; + } - while ((di = readdir(d))) { - if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) - continue; + sprintf(buffer, "%d,%d,%d,%d,%d,%d,%d", + data->hard_offsets[0], data->hard_offsets[1], data->hard_offsets[2], + data->calib_offsets[0], data->calib_offsets[1], data->calib_offsets[2], + data->accuracy); - snprintf(path, PATH_MAX, "/sys/class/input/%s/name", di->d_name); + rc = write(config_fd, buffer, strlen(buffer) + 1); + if (rc < (int) strlen(buffer) + 1) { + ALOGE("%s: Unable to write config", __func__); + goto error; + } - fd = open(path, O_RDONLY); - if (fd < 0) - continue; + rename(GEOMAGNETICD_CONFIG_BACKUP_PATH, GEOMAGNETICD_CONFIG_PATH); - read(fd, &input_name, sizeof(input_name)); - close(fd); + rc = 0; + goto complete; - c = strstr((char *) &input_name, "\n"); - if (c != NULL) - *c = '\0'; +error: + rc = -1; - if (strcmp(input_name, name) == 0) { - snprintf(path_prefix, PATH_MAX, "/sys/class/input/%s", di->d_name); - return 0; - } - } +complete: + if (config_fd >= 0) + close(config_fd); - return -1; + return rc; } /* - * Geomagneticd + * Offsets */ -int offset_read(char *path, int *hard_offset, int *calib_offset, int *accuracy) +int geomagneticd_offsets_read(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; + char buffer[100] = { 0 }; + int offsets_fd = -1; int rc; - fd = open(path, O_RDONLY); - if (fd < 0) - return -1; + if (data == NULL) + return -EINVAL; - rc = read(fd, &buf, sizeof(buf)); - close(fd); - if (rc <= 0) - return -1; + offsets_fd = open(data->path_offsets, O_RDONLY); + if (offsets_fd < 0) { + ALOGE("%s: Unable to open offsets", __func__); + goto error; + } - rc = sscanf(buf, "%d %d %d %d %d %d %d", - &hard_offset[0], &hard_offset[1], &hard_offset[2], - &calib_offset[0], &calib_offset[1], &calib_offset[2], accuracy); + rc = read(offsets_fd, buffer, sizeof(buffer)); + if (rc <= 0) { + ALOGE("%s: Unable to read offsets", __func__); + goto error; + } - if (rc != 7) - return -1; + rc = sscanf(buffer, "%d %d %d %d %d %d %d", + &data->hard_offsets[0], &data->hard_offsets[1], &data->hard_offsets[2], + &data->calib_offsets[0], &data->calib_offsets[1], &data->calib_offsets[2], + &data->accuracy); + if (rc != 7) { + ALOGE("%s: Unable to parse offsets", __func__); + goto error; + } - return 0; + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (offsets_fd >= 0) + close(offsets_fd); + + return rc; } -int offset_write(char *path, int *hard_offset, int *calib_offset, int accuracy) +int geomagneticd_offsets_write(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; + char buffer[100] = { 0 }; + int offsets_fd = -1; int rc; - sprintf(buf, "%d %d %d %d %d %d %d\n", - hard_offset[0], hard_offset[1], hard_offset[2], - calib_offset[0], calib_offset[1], calib_offset[2], accuracy); + if (data == NULL) + return -EINVAL; - fd = open(path, O_WRONLY); - if (fd < 0) - return -1; + offsets_fd = open(data->path_offsets, O_WRONLY); + if (offsets_fd < 0) { + ALOGE("%s: Unable to open offsets", __func__); + goto error; + } - write(fd, buf, strlen(buf) + 1); - close(fd); + sprintf(buffer, "%d %d %d %d %d %d %d\n", + data->hard_offsets[0], data->hard_offsets[1], data->hard_offsets[2], + data->calib_offsets[0], data->calib_offsets[1], data->calib_offsets[2], + data->accuracy); - return 0; + rc = write(offsets_fd, buffer, strlen(buffer) + 1); + if (rc < (int) strlen(buffer) + 1) { + ALOGE("%s: Unable to write offsets", __func__); + goto error; + } + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (offsets_fd >= 0) + close(offsets_fd); + + return rc; } -int yas_cfg_read(int *hard_offset, int *calib_offset, int *accuracy) +int geomagneticd_offsets_init(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; - int rc; + int count; + int i; - fd = open("/data/system/yas.cfg", O_RDONLY); - if (fd < 0) - return -1; + if (data == NULL) + return -EINVAL; - rc = read(fd, &buf, sizeof(buf)); - close(fd); - if (rc <= 0) - return -1; + count = sizeof(data->hard_offsets) / sizeof(int); - rc = sscanf(buf, "%d,%d,%d,%d,%d,%d,%d", - &hard_offset[0], &hard_offset[1], &hard_offset[2], - &calib_offset[0], &calib_offset[1], &calib_offset[2], accuracy); + // 0x7f is an invalid value for hard offsets + for (i = 0; i < count; i++) + data->hard_offsets[i] = 0x7f; - if (rc != 7) - return -1; + count = sizeof(data->calib_offsets) / sizeof(int); + + // 0x0x7fffffff is an invalid value for calib offsets + for (i = 0; i < count; i++) + data->calib_offsets[i] = 0x7fffffff; return 0; } -int yas_cfg_write(int *hard_offset, int *calib_offset, int accuracy) +int geomagneticd_offsets_check(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; - int rc; + int count; + int i; - fd = open("/data/system/yas-backup.cfg", O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP); - if (fd < 0) - return -1; + if (data == NULL) + return -EINVAL; - sprintf(buf, "%d,%d,%d,%d,%d,%d,%d\n", - hard_offset[0], hard_offset[1], hard_offset[2], - calib_offset[0], calib_offset[1], calib_offset[2], accuracy); + count = sizeof(data->hard_offsets) / sizeof(int); - write(fd, buf, strlen(buf) + 1); - close(fd); + // 0x7f is an invalid value for hard offsets + for (i = 0; i < count; i++) + if (data->hard_offsets[i] == 0x7f) + return 0; - chmod("/data/system/yas-backup.cfg", 0644); - rename("/data/system/yas-backup.cfg", "/data/system/yas.cfg"); + count = sizeof(data->calib_offsets) / sizeof(int); - return 0; + // 0x0x7fffffff is an invalid value for calib offsets + for (i = 0; i < count; i++) + if (data->calib_offsets[i] == 0x7fffffff) + return 0; + + return 1; } -int main(int argc, char *argv[]) +/* + * Geomagneticd + */ + +int geomagneticd_magnetic_extrema_init(struct geomagneticd_data *data) { - struct input_event event; + int count; + int i; - char path[PATH_MAX] = { 0 }; - char path_offset[PATH_MAX] = { 0 }; + if (data == NULL) + return -EINVAL; + + count = sizeof(data->calib_offsets) / sizeof(int); + + // Approximate the previous extrema from the calib offsets + for (i = 0; i < count; i++) { + data->magnetic_extrema[0][i] = -(data->calib_offsets[i] + 45) * 1000 + 5000; + data->magnetic_extrema[1][i] = (data->calib_offsets[i] + 45) * 1000 - 5000; + } - int offset_fd; - int input_fd; + return 0; +} + +int geomagneticd_magnetic_extrema(struct geomagneticd_data *data, int index, + int value) +{ + if (data == NULL || index < 0 || index >= 3) + return -EINVAL; - int max_coeff[3] = { 40, 45, 45 }; - int hard_offset[3] = { 0 }; - int calib_offset[3] = { 0 }; - int accuracy = 0; + if (value == 0) + return 0; - int axis_min[3] = { 0 }; - int axis_max[3] = { 0 }; - int axis_calib[3] = { 0 }; + // Update the extrema from the current value if needed + if (value < data->magnetic_extrema[0][index] || data->magnetic_extrema[0][index] == 0) + data->magnetic_extrema[0][index] = value; + if (value > data->magnetic_extrema[1][index] || data->magnetic_extrema[1][index] == 0) + data->magnetic_extrema[1][index] = value; - int x, y, z; + return 0; +} +int geomagneticd_calib_offsets(struct geomagneticd_data *data) +{ + int calib_offsets[3]; + int offsets[2]; + int update; + int count; int rc; int i; - /* - * Wait for something to be ready and properly report the hard coeff. - * Without that, the hard coeff are reported to be around 127. - */ + if (data == NULL) + return -EINVAL; - ALOGD("Geomagneticd start"); + // Calculating the offset is only meaningful when enough values were + // obtained. There is no need to calculate it too often either. + if (data->count % 10 != 0) + return 0; - input_fd = input_open("geomagnetic_raw"); - if (input_fd < 0) - goto sleep_loop; + update = 0; - rc = sysfs_path_prefix("geomagnetic_raw", &path); - if (rc < 0) - goto sleep_loop; + count = sizeof(data->calib_offsets) / sizeof(int); - snprintf(path_offset, PATH_MAX, "%s/offsets", path); + // Calculate the calib offset for each axis to have values in [-45;45] uT + for (i = 0; i < count; i++) { + offsets[0] = data->magnetic_extrema[0][i] + 45 * 1000; + offsets[1] = data->magnetic_extrema[1][i] - 45 * 1000; + calib_offsets[i] = (offsets[0] + offsets[1]) / 2; - for (i=0 ; i < 3 ; i++) { - axis_min[i] = 0; - axis_max[i] = 0; - calib_offset[i] = 0; + if (calib_offsets[i] != data->calib_offsets[i]) { + data->calib_offsets[i] = calib_offsets[i]; + update = 1; + } } - ALOGD("Reading config"); - - rc = yas_cfg_read(&hard_offset, &calib_offset, &accuracy); - if (rc == 0) { - ALOGD("Setting initial offsets: %d %d %d, %d %d %d", hard_offset[0], hard_offset[1], hard_offset[2], calib_offset[0], calib_offset[1], calib_offset[2]); - - offset_write(path_offset, &hard_offset, &calib_offset, accuracy); + if (update) { + data->accuracy = 1; - for (i=0 ; i < 3 ; i++) { - axis_min[i] = - calib_offset[i] - max_coeff[i] * 1000; - axis_max[i] = calib_offset[i] + max_coeff[i] * 1000; - axis_calib[i] = calib_offset[i]; + rc = geomagneticd_offsets_write(data); + if (rc < 0) { + ALOGE("%s: Unable to write offsets", __func__); + return -1; } - } else { - offset_read(path_offset, &hard_offset, &calib_offset, &accuracy); - ALOGD("Reading initial offsets: %d %d %d", hard_offset[0], hard_offset[1], hard_offset[2]); - for (i=0 ; i < 3 ; i++) { - axis_min[i] = 0; - axis_max[i] = 0; - calib_offset[i] = 0; + rc = geomagneticd_config_write(data); + if (rc < 0) { + ALOGE("%s: Unable to write config", __func__); + return -1; } } -loop: - while (1) { - read(input_fd, &event, sizeof(event)); - - if (event.type == EV_SYN) { - for (i=0 ; i < 3 ; i++) { - if (-axis_min[i] < axis_max[i]) { - axis_calib[i] = axis_max[i] - max_coeff[i] * 1000; - } else { - axis_calib[i] = axis_min[i] + max_coeff[i] * 1000; - } + return 0; +} - axis_calib[i] = axis_calib[i] < 0 ? -axis_calib[i] : axis_calib[i]; +int geomagneticd_poll(struct geomagneticd_data *data) +{ + struct input_event input_event; + struct pollfd poll_fd; + int rc; - if (axis_calib[i] != calib_offset[i]) { - calib_offset[i] = axis_calib[i]; - accuracy = 1; + if (data == NULL) + return -EINVAL; - offset_write(path_offset, &hard_offset, &calib_offset, accuracy); - yas_cfg_write(&hard_offset, &calib_offset, accuracy); - } + if (data->input_fd < 0) + return -1; -// printf("axis_calib[%d]=%d\n", i, axis_calib[i]); - } + memset(&poll_fd, 0, sizeof(poll_fd)); + poll_fd.fd = data->input_fd; + poll_fd.events = POLLIN; - if (hard_offset[0] == 127 && hard_offset[1] == 127 && hard_offset[2] == 127) { - offset_read(path_offset, &hard_offset, &calib_offset, &accuracy); + while (1) { + rc = poll(&poll_fd, 1, -1); + if (rc < 0) { + ALOGE("%s: poll failure", __func__); + goto error; + } - if (hard_offset[0] != 127 || hard_offset[1] != 127 || hard_offset[2] != 127) { - ALOGD("Reading offsets: %d %d %d", hard_offset[0], hard_offset[1], hard_offset[2]); - yas_cfg_write(&hard_offset, &calib_offset, accuracy); - } - } + if (!(poll_fd.revents & POLLIN)) + continue; + + poll_fd.revents = 0; + + rc = read(data->input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) { + ALOGE("%s: Unable to read input event", __func__); + continue; } - if(event.type == EV_ABS) { - switch (event.code) { + // Update the extrema from the current value + if(input_event.type == EV_ABS) { + switch (input_event.code) { case ABS_X: - x = event.value; - if (x < axis_min[0]) - axis_min[0] = x; - if (x > axis_max[0]) - axis_max[0] = x; + geomagneticd_magnetic_extrema(data, 0, input_event.value); break; case ABS_Y: - y = event.value; - if (y < axis_min[1]) - axis_min[1] = y; - if (y > axis_max[1]) - axis_max[1] = y; + geomagneticd_magnetic_extrema(data, 1, input_event.value); break; case ABS_Z: - z = event.value; - if (z < axis_min[2]) - axis_min[2] = z; - if (z > axis_max[2]) - axis_max[2] = z; + geomagneticd_magnetic_extrema(data, 2, input_event.value); break; } } + + if (input_event.type == EV_SYN) { + // Sometimes, the hard offsets cannot be read at startup + // so we need to do it now + if (!geomagneticd_offsets_check(data)) { + rc = geomagneticd_offsets_read(data); + if (rc < 0) { + ALOGE("%s: Unable to read offsets", __func__); + continue; + } + + // Most likely, the calib offset will be invalid + if (geomagneticd_offsets_check(data)) { + data->accuracy = 1; + geomagneticd_magnetic_extrema_init(data); + } + + rc = geomagneticd_config_write(data); + if (rc < 0) { + ALOGE("%s: Unable to write config", __func__); + continue; + } + } + + data->count++; + + rc = geomagneticd_calib_offsets(data); + if (rc < 0) { + ALOGE("%s: Unable to calib offsets", __func__); + continue; + } + } } -sleep_loop: - while (1) { - sleep(3600); + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + return rc; +} + +int main(int argc, char *argv[]) +{ + struct geomagneticd_data *geomagneticd_data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + geomagneticd_data = (struct geomagneticd_data *) + calloc(1, sizeof(struct geomagneticd_data)); + + input_fd = input_open("geomagnetic_raw"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; } - return 0; + rc = sysfs_path_prefix("geomagnetic_raw", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(geomagneticd_data->path_offsets, PATH_MAX, "%s/offsets", path); + + geomagneticd_data->input_fd = input_fd; + + geomagneticd_offsets_init(geomagneticd_data); + + // Attempt to read the offsets from the config + rc = geomagneticd_config_read(geomagneticd_data); + if (rc < 0 || !geomagneticd_offsets_check(geomagneticd_data)) { + // Read the offsets from the driver + rc = geomagneticd_offsets_read(geomagneticd_data); + if (rc < 0) { + ALOGE("%s: Unable to read offsets", __func__); + goto error; + } + + // Most likely, the calib offset will be invalid and the hard + // offset may be invalid as well + if (geomagneticd_offsets_check(geomagneticd_data)) { + geomagneticd_data->accuracy = 1; + geomagneticd_magnetic_extrema_init(geomagneticd_data); + } + } else { + // Get the magnetic extrema from the config's offsets + geomagneticd_magnetic_extrema_init(geomagneticd_data); + + rc = geomagneticd_offsets_write(geomagneticd_data); + if (rc < 0) { + ALOGE("%s: Unable to write offsets", __func__); + goto error; + } + } + + rc = geomagneticd_poll(geomagneticd_data); + if (rc < 0) + goto error; + + rc = 0; + goto complete; + +error: + while (1) + sleep(3600); + + rc = 1; + +complete: + if (input_fd >= 0) + close(input_fd); + + if (geomagneticd_data != NULL) + free(geomagneticd_data); + + return rc; } diff --git a/libsensors/geomagneticd/geomagneticd.h b/libsensors/geomagneticd/geomagneticd.h new file mode 100644 index 0000000..cd57eb9 --- /dev/null +++ b/libsensors/geomagneticd/geomagneticd.h @@ -0,0 +1,59 @@ +/* + * 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 + +#ifndef _GEOMAGNETICD_H_ +#define _GEOMAGNETICD_H_ + +#define GEOMAGNETICD_CONFIG_PATH "/data/system/yas.cfg" +#define GEOMAGNETICD_CONFIG_BACKUP_PATH "/data/system/yas-backup.cfg" + +struct geomagneticd_data { + int magnetic_extrema[2][3]; + int hard_offsets[3]; + int calib_offsets[3]; + int accuracy; + + int input_fd; + char path_offsets[PATH_MAX]; + + int count; +}; + +/* + * Input + */ + +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); + +#endif diff --git a/libsensors/geomagneticd/input.c b/libsensors/geomagneticd/input.c new file mode 100644 index 0000000..7e9ca4a --- /dev/null +++ b/libsensors/geomagneticd/input.c @@ -0,0 +1,335 @@ +/* + * 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 + +#define LOG_TAG "geomagneticd" +#include + +#include "geomagneticd.h" + +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 time->tv_sec * 1000000000LL + time->tv_usec * 1000; +} + +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; + + char input_name[80] = { 0 }; + char path[PATH_MAX]; + char *c; + int fd; + int rc; + + if (name == NULL) + return -EINVAL; + + d = opendir("/dev/input"); + if (d == NULL) + return -1; + + while ((di = readdir(d))) { + if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) + continue; + + snprintf(path, PATH_MAX, "/dev/input/%s", di->d_name); + fd = open(path, O_RDWR | O_NONBLOCK); + if (fd < 0) + continue; + + rc = ioctl(fd, EVIOCGNAME(sizeof(input_name) - 1), &input_name); + if (rc < 0) + continue; + + c = strstr((char *) &input_name, "\n"); + if (c != NULL) + *c = '\0'; + + if (strcmp(input_name, name) == 0) + return fd; + else + close(fd); + } + + return -1; +} + +int sysfs_path_prefix(char *name, char *path_prefix) +{ + DIR *d; + struct dirent *di; + + char input_name[80] = { 0 }; + char path[PATH_MAX]; + char *c; + int fd; + + if (name == NULL || path_prefix == NULL) + return -EINVAL; + + d = opendir("/sys/class/input"); + if (d == NULL) + return -1; + + while ((di = readdir(d))) { + if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) + continue; + + snprintf(path, PATH_MAX, "/sys/class/input/%s/name", di->d_name); + + fd = open(path, O_RDONLY); + if (fd < 0) + continue; + + read(fd, &input_name, sizeof(input_name)); + close(fd); + + c = strstr((char *) &input_name, "\n"); + if (c != NULL) + *c = '\0'; + + if (strcmp(input_name, name) == 0) { + snprintf(path_prefix, PATH_MAX, "/sys/class/input/%s", di->d_name); + return 0; + } + } + + 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/gp2a_light.c b/libsensors/gp2a_light.c index 9f2d37e..3a41187 100644 --- a/libsensors/gp2a_light.c +++ b/libsensors/gp2a_light.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 @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -37,7 +38,8 @@ struct gp2a_light_data { char path_delay[PATH_MAX]; }; -int gp2a_light_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device) +int gp2a_light_init(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device) { struct gp2a_light_data *data = NULL; char path[PATH_MAX] = { 0 }; @@ -49,6 +51,8 @@ int gp2a_light_init(struct piranha_sensors_handlers *handlers, struct piranha_se if (handlers == NULL) return -EINVAL; + data = (struct gp2a_light_data *) calloc(1, sizeof(struct gp2a_light_data)); + input_fd = input_open("light_sensor"); if (input_fd < 0) { ALOGE("%s: Unable to open input", __func__); @@ -61,8 +65,6 @@ int gp2a_light_init(struct piranha_sensors_handlers *handlers, struct piranha_se goto error; } - data = (struct gp2a_light_data *) calloc(1, sizeof(struct gp2a_light_data)); - snprintf(data->path_enable, PATH_MAX, "%s/enable", path); snprintf(data->path_delay, PATH_MAX, "%s/poll_delay", path); @@ -72,12 +74,12 @@ int gp2a_light_init(struct piranha_sensors_handlers *handlers, struct piranha_se return 0; error: - if (input_fd >= 0) - close(input_fd); - if (data != NULL) free(data); + if (input_fd >= 0) + close(input_fd); + handlers->poll_fd = -1; handlers->data = NULL; @@ -86,22 +88,17 @@ error: int gp2a_light_deinit(struct piranha_sensors_handlers *handlers) { - int input_fd; - ALOGD("%s(%p)", __func__, handlers); if (handlers == NULL) return -EINVAL; - input_fd = handlers->poll_fd; - if (input_fd >= 0) - close(input_fd); - + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); handlers->poll_fd = -1; if (handlers->data != NULL) free(handlers->data); - handlers->data = NULL; return 0; @@ -110,8 +107,7 @@ int gp2a_light_deinit(struct piranha_sensors_handlers *handlers) int gp2a_light_activate(struct piranha_sensors_handlers *handlers) { struct gp2a_light_data *data; - char enable[] = "1\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -120,15 +116,12 @@ int gp2a_light_activate(struct piranha_sensors_handlers *handlers) data = (struct gp2a_light_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - handlers->activated = 1; return 0; @@ -137,8 +130,7 @@ int gp2a_light_activate(struct piranha_sensors_handlers *handlers) int gp2a_light_deactivate(struct piranha_sensors_handlers *handlers) { struct gp2a_light_data *data; - char enable[] = "0\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -147,54 +139,50 @@ int gp2a_light_deactivate(struct piranha_sensors_handlers *handlers) data = (struct gp2a_light_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - - handlers->activated = 0; + handlers->activated = 1; return 0; } -int gp2a_light_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay) +int gp2a_light_set_delay(struct piranha_sensors_handlers *handlers, long int delay) { struct gp2a_light_data *data; - char *value = NULL; - int c; - int fd; + int rc; -// ALOGD("%s(%p, %ld)", __func__, handlers, (long int) delay); + ALOGD("%s(%p, %ld)", __func__, handlers, delay); if (handlers == NULL || handlers->data == NULL) return -EINVAL; data = (struct gp2a_light_data *) handlers->data; - c = asprintf(&value, "%ld\n", (long int) delay); - - fd = open(data->path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); + rc = sysfs_value_write(data->path_delay, (int) delay); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, value, c); - close(fd); - - if (value != NULL) - free(value); - return 0; } -float gp2a_light_light(int value) +float gp2a_light_convert(int value) { - return (float) powf(10, value * (125.0f / 1023.0f / 24.0f)) * 4; + // Converting the raw value to lux units is done with: + // I = 10 * log(light) uA + // U = raw * 3300 / 4095 (max ADC value is 3.3V for 4095 LSB) + // R = 47kOhm + // => light = 10 ^ (I / 10) = 10 ^ (U / R / 10) + // => light = 10 ^ (raw * 330 / 4095 / 47) + // Only 1/4 of light reaches the sensor: + // => light = 4 * (10 ^ (raw * 330 / 4095 / 47)) + + return powf(10, value * (330.0f / 4095.0f / 47.0f)) * 4; } int gp2a_light_get_data(struct piranha_sensors_handlers *handlers, @@ -204,6 +192,8 @@ int gp2a_light_get_data(struct piranha_sensors_handlers *handlers, int input_fd; int rc; +// ALOGD("%s(%p, %p)", __func__, handlers, event); + if (handlers == NULL || event == NULL) return -EINVAL; @@ -211,18 +201,24 @@ int gp2a_light_get_data(struct piranha_sensors_handlers *handlers, if (input_fd < 0) return -EINVAL; - rc = read(input_fd, &input_event, sizeof(input_event)); - if (rc < (int) sizeof(input_event)) - return -EINVAL; - - if (input_event.type != EV_ABS || input_event.code != ABS_MISC) - return -1; - + memset(event, 0, sizeof(struct sensors_event_t)); event->version = sizeof(struct sensors_event_t); event->sensor = handlers->handle; event->type = handlers->handle; - event->timestamp = input_timestamp(&input_event); - event->light = gp2a_light_light(input_event.value); + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_REL) { + if (input_event.code == REL_MISC) + event->light = gp2a_light_convert(input_event.value); + } 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; } @@ -237,6 +233,7 @@ struct piranha_sensors_handlers gp2a_light = { .set_delay = gp2a_light_set_delay, .get_data = gp2a_light_get_data, .activated = 0, + .needed = 0, .poll_fd = -1, .data = NULL, }; diff --git a/libsensors/gp2a_proximity.c b/libsensors/gp2a_proximity.c index 873ad94..b2c9813 100644 --- a/libsensors/gp2a_proximity.c +++ b/libsensors/gp2a_proximity.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 @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include @@ -36,7 +36,8 @@ struct gp2a_proximity_data { char path_enable[PATH_MAX]; }; -int gp2a_proximity_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device) +int gp2a_proximity_init(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device) { struct gp2a_proximity_data *data = NULL; char path[PATH_MAX] = { 0 }; @@ -48,6 +49,8 @@ int gp2a_proximity_init(struct piranha_sensors_handlers *handlers, struct piranh if (handlers == NULL) return -EINVAL; + data = (struct gp2a_proximity_data *) calloc(1, sizeof(struct gp2a_proximity_data)); + input_fd = input_open("proximity_sensor"); if (input_fd < 0) { ALOGE("%s: Unable to open input", __func__); @@ -60,8 +63,6 @@ int gp2a_proximity_init(struct piranha_sensors_handlers *handlers, struct piranh goto error; } - data = (struct gp2a_proximity_data *) calloc(1, sizeof(struct gp2a_proximity_data)); - snprintf(data->path_enable, PATH_MAX, "%s/enable", path); handlers->poll_fd = input_fd; @@ -70,12 +71,12 @@ int gp2a_proximity_init(struct piranha_sensors_handlers *handlers, struct piranh return 0; error: - if (input_fd >= 0) - close(input_fd); - if (data != NULL) free(data); + if (input_fd >= 0) + close(input_fd); + handlers->poll_fd = -1; handlers->data = NULL; @@ -84,22 +85,17 @@ error: int gp2a_proximity_deinit(struct piranha_sensors_handlers *handlers) { - int input_fd; - ALOGD("%s(%p)", __func__, handlers); if (handlers == NULL) return -EINVAL; - input_fd = handlers->poll_fd; - if (input_fd >= 0) - close(input_fd); - + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); handlers->poll_fd = -1; if (handlers->data != NULL) free(handlers->data); - handlers->data = NULL; return 0; @@ -108,8 +104,7 @@ int gp2a_proximity_deinit(struct piranha_sensors_handlers *handlers) int gp2a_proximity_activate(struct piranha_sensors_handlers *handlers) { struct gp2a_proximity_data *data; - char enable[] = "1\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -118,15 +113,12 @@ int gp2a_proximity_activate(struct piranha_sensors_handlers *handlers) data = (struct gp2a_proximity_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - handlers->activated = 1; return 0; @@ -135,8 +127,7 @@ int gp2a_proximity_activate(struct piranha_sensors_handlers *handlers) int gp2a_proximity_deactivate(struct piranha_sensors_handlers *handlers) { struct gp2a_proximity_data *data; - char enable[] = "0\n"; - int fd; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -145,36 +136,25 @@ int gp2a_proximity_deactivate(struct piranha_sensors_handlers *handlers) data = (struct gp2a_proximity_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - - handlers->activated = 0; + handlers->activated = 1; return 0; } -int gp2a_proximity_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay) +int gp2a_proximity_set_delay(struct piranha_sensors_handlers *handlers, long int delay) { - struct gp2a_proximity_data *data; - char *value = NULL; - int c; - int fd; - -// ALOGD("%s(%p, %ld)", __func__, handlers, (long int) delay); - - if (handlers == NULL || handlers->data == NULL) - return -EINVAL; + ALOGD("%s(%p, %ld)", __func__, handlers, delay); return 0; } -float gp2a_proximity_proximity(int value) +float gp2a_proximity_convert(int value) { return (float) value * 5.0f; } @@ -186,6 +166,8 @@ int gp2a_proximity_get_data(struct piranha_sensors_handlers *handlers, int input_fd; int rc; +// ALOGD("%s(%p, %p)", __func__, handlers, event); + if (handlers == NULL || event == NULL) return -EINVAL; @@ -193,18 +175,24 @@ int gp2a_proximity_get_data(struct piranha_sensors_handlers *handlers, if (input_fd < 0) return -EINVAL; - rc = read(input_fd, &input_event, sizeof(input_event)); - if (rc < (int) sizeof(input_event)) - return -EINVAL; - - if (input_event.type != EV_ABS || input_event.code != ABS_DISTANCE) - return -1; - + memset(event, 0, sizeof(struct sensors_event_t)); event->version = sizeof(struct sensors_event_t); event->sensor = handlers->handle; event->type = handlers->handle; - event->timestamp = input_timestamp(&input_event); - event->distance = gp2a_proximity_proximity(input_event.value); + + do { + rc = read(input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + if (input_event.code == ABS_DISTANCE) + event->distance = gp2a_proximity_convert(input_event.value); + } 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; } @@ -219,6 +207,7 @@ struct piranha_sensors_handlers gp2a_proximity = { .set_delay = gp2a_proximity_set_delay, .get_data = gp2a_proximity_get_data, .activated = 0, + .needed = 0, .poll_fd = -1, .data = NULL, }; diff --git a/libsensors/input.c b/libsensors/input.c index f82cfd2..c48f17f 100644 --- a/libsensors/input.c +++ b/libsensors/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 @@ -23,18 +23,107 @@ #include #include #include +#include #define LOG_TAG "piranha_sensors" #include #include "piranha_sensors.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 time->tv_sec * 1000000000LL + time->tv_usec * 1000; +} + +long int input_timestamp(struct input_event *event) { if (event == NULL) return -1; - return event->time.tv_sec*1000000000LL + event->time.tv_usec*1000; + 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) @@ -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/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, +}; diff --git a/libsensors/piranha_sensors.c b/libsensors/piranha_sensors.c index 2d1ff71..3600196 100644 --- a/libsensors/piranha_sensors.c +++ b/libsensors/piranha_sensors.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 @@ -21,7 +21,7 @@ #include #include #include - +#include #include #include @@ -35,8 +35,8 @@ */ struct sensor_t piranha_sensors[] = { - { "BMA254 Acceleration Sensor", "Bosch", 1, SENSOR_TYPE_ACCELEROMETER, - SENSOR_TYPE_ACCELEROMETER, 19.6f, 0.0383f, 0.13f, 10000, {}, }, + { "BMA250 Acceleration Sensor", "Bosch", 1, SENSOR_TYPE_ACCELEROMETER, + SENSOR_TYPE_ACCELEROMETER, 2 * GRAVITY_EARTH, GRAVITY_EARTH / 256.0f, 0.13f, 10000, {}, }, { "YAS530 Magnetic Sensor", "Yamaha", 1, SENSOR_TYPE_MAGNETIC_FIELD, SENSOR_TYPE_MAGNETIC_FIELD, 800.0f, 0.3f, 4.0f, 10000, {}, }, { "YAS Orientation Sensor", "Yamaha", 1, SENSOR_TYPE_ORIENTATION, @@ -46,10 +46,10 @@ struct sensor_t piranha_sensors[] = { SENSOR_TYPE_LIGHT, 0.0f, 0.0f, 0.0f, 0, {}, }, #endif #ifdef TARGET_DEVICE_P3100 - { "GP2A Light Sensor", "SHARP", 1, SENSOR_TYPE_LIGHT, - SENSOR_TYPE_LIGHT, 0.0f, 0.0f, 0.0f, 0, {}, }, - { "GP2A Proximity Sensor", "SHARP", 1, SENSOR_TYPE_PROXIMITY, - SENSOR_TYPE_PROXIMITY, 5.0f, 0.0f, 0.0f, 0, {}, }, + { "GP2A Light Sensor", "Sharp", 1, SENSOR_TYPE_LIGHT, + SENSOR_TYPE_LIGHT, 10240.0f, 1.0f, 0.75f, 0, {}, }, + { "GP2A Proximity Sensor", "Sharp", 1, SENSOR_TYPE_PROXIMITY, + SENSOR_TYPE_PROXIMITY, 5.0f, 5.0f, 0.75f, 0, {}, }, #endif }; @@ -57,7 +57,7 @@ int piranha_sensors_count = sizeof(piranha_sensors) / sizeof(struct sensor_t); struct piranha_sensors_handlers *piranha_sensors_handlers[] = { &bma250, - &yas530c, + &yas530, &yas_orientation, #ifdef TARGET_DEVICE_P5100 &bh1721, @@ -75,7 +75,8 @@ int piranha_sensors_handlers_count = sizeof(piranha_sensors_handlers) / * Piranha Sensors */ -int piranha_sensors_activate(struct sensors_poll_device_t *dev, int handle, int enabled) +int piranha_sensors_activate(struct sensors_poll_device_t *dev, int handle, + int enabled) { struct piranha_sensors_device *device; int i; @@ -90,22 +91,32 @@ int piranha_sensors_activate(struct sensors_poll_device_t *dev, int handle, int if (device->handlers == NULL || device->handlers_count <= 0) return -EINVAL; - for (i=0 ; i < device->handlers_count ; i++) { + for (i = 0; i < device->handlers_count; i++) { if (device->handlers[i] == NULL) continue; if (device->handlers[i]->handle == handle) { - if (enabled && device->handlers[i]->activate != NULL) - return device->handlers[i]->activate(device->handlers[i]); - else if (!enabled && device->handlers[i]->deactivate != NULL) - return device->handlers[i]->deactivate(device->handlers[i]); + if (enabled && device->handlers[i]->activate != NULL) { + device->handlers[i]->needed |= PIRANHA_SENSORS_NEEDED_API; + if (device->handlers[i]->needed == PIRANHA_SENSORS_NEEDED_API) + return device->handlers[i]->activate(device->handlers[i]); + else + return 0; + } else if (!enabled && device->handlers[i]->deactivate != NULL) { + device->handlers[i]->needed &= ~PIRANHA_SENSORS_NEEDED_API; + if (device->handlers[i]->needed == 0) + return device->handlers[i]->deactivate(device->handlers[i]); + else + return 0; + } } } return -1; } -int piranha_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, int64_t ns) +int piranha_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, + int64_t ns) { struct piranha_sensors_device *device; int i; @@ -120,12 +131,12 @@ int piranha_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, int if (device->handlers == NULL || device->handlers_count <= 0) return -EINVAL; - for (i=0 ; i < device->handlers_count ; i++) { + for (i = 0; i < device->handlers_count; i++) { if (device->handlers[i] == NULL) continue; if (device->handlers[i]->handle == handle && device->handlers[i]->set_delay != NULL) - return device->handlers[i]->set_delay(device->handlers[i], ns); + return device->handlers[i]->set_delay(device->handlers[i], (long int) ns); } return 0; @@ -153,15 +164,15 @@ int piranha_sensors_poll(struct sensors_poll_device_t *dev, n = 0; do { - poll_rc = poll(device->poll_fds, device->poll_fds_count, PIRANHA_POLL_DELAY); + poll_rc = poll(device->poll_fds, device->poll_fds_count, n > 0 ? 0 : -1); if (poll_rc < 0) return -1; - for (i=0 ; i < device->poll_fds_count ; i++) { + for (i = 0; i < device->poll_fds_count; i++) { if (!(device->poll_fds[i].revents & POLLIN)) continue; - for (j=0 ; j < device->handlers_count ; j++) { + for (j = 0; j < device->handlers_count; j++) { if (device->handlers[j] == NULL || device->handlers[j]->poll_fd != device->poll_fds[i].fd || device->handlers[j]->get_data == NULL) continue; @@ -174,9 +185,6 @@ int piranha_sensors_poll(struct sensors_poll_device_t *dev, count--; } } - - if (count <= 0) - break; } } while ((poll_rc > 0 || n < 1) && count > 0); @@ -202,7 +210,7 @@ int piranha_sensors_close(hw_device_t *device) if (piranha_sensors_device->poll_fds != NULL) free(piranha_sensors_device->poll_fds); - for (i=0 ; i < piranha_sensors_device->handlers_count ; i++) { + for (i = 0; i < piranha_sensors_device->handlers_count; i++) { if (piranha_sensors_device->handlers[i] == NULL || piranha_sensors_device->handlers[i]->deinit == NULL) continue; @@ -240,7 +248,7 @@ int piranha_sensors_open(const struct hw_module_t* module, const char *id, calloc(1, piranha_sensors_handlers_count * sizeof(struct pollfd)); p = 0; - for (i=0 ; i < piranha_sensors_handlers_count ; i++) { + for (i = 0; i < piranha_sensors_handlers_count; i++) { if (piranha_sensors_handlers[i] == NULL || piranha_sensors_handlers[i]->init == NULL) continue; diff --git a/libsensors/piranha_sensors.h b/libsensors/piranha_sensors.h index b93f65e..4de8c99 100644 --- a/libsensors/piranha_sensors.h +++ b/libsensors/piranha_sensors.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 @@ -17,16 +17,16 @@ #include #include - #include #include #include -#ifndef _SENSORS_H_ -#define _SENSORS_H_ +#ifndef _PIRANHA_SENSORS_H_ +#define _PIRANHA_SENSORS_H_ -#define PIRANHA_POLL_DELAY 100 +#define PIRANHA_SENSORS_NEEDED_API (1 << 0) +#define PIRANHA_SENSORS_NEEDED_ORIENTATION (1 << 1) struct piranha_sensors_device; @@ -34,14 +34,18 @@ struct piranha_sensors_handlers { char *name; int handle; - int (*init)(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device); + int (*init)(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device); int (*deinit)(struct piranha_sensors_handlers *handlers); int (*activate)(struct piranha_sensors_handlers *handlers); int (*deactivate)(struct piranha_sensors_handlers *handlers); - int (*set_delay)(struct piranha_sensors_handlers *handlers, int64_t delay); - int (*get_data)(struct piranha_sensors_handlers *handlers, struct sensors_event_t *event); + int (*set_delay)(struct piranha_sensors_handlers *handlers, + long int delay); + int (*get_data)(struct piranha_sensors_handlers *handlers, + struct sensors_event_t *event); int activated; + int needed; int poll_fd; void *data; @@ -60,8 +64,10 @@ struct piranha_sensors_device { extern struct piranha_sensors_handlers *piranha_sensors_handlers[]; extern int piranha_sensors_handlers_count; -int piranha_sensors_activate(struct sensors_poll_device_t *dev, int handle, int enabled); -int piranha_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, int64_t ns); +int piranha_sensors_activate(struct sensors_poll_device_t *dev, int handle, + int enabled); +int piranha_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, + int64_t ns); int piranha_sensors_poll(struct sensors_poll_device_t *dev, struct sensors_event_t* data, int count); @@ -69,16 +75,27 @@ int piranha_sensors_poll(struct sensors_poll_device_t *dev, * Input */ -int64_t input_timestamp(struct input_event *event); +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 */ +int orientation_fill(struct piranha_sensors_handlers *handlers, + sensors_vec_t *acceleration, sensors_vec_t *magnetic); + extern struct piranha_sensors_handlers bma250; -extern struct piranha_sensors_handlers yas530c; +extern struct piranha_sensors_handlers yas530; extern struct piranha_sensors_handlers yas_orientation; extern struct piranha_sensors_handlers bh1721; extern struct piranha_sensors_handlers gp2a_light; diff --git a/libsensors/yas530.c b/libsensors/yas530.c new file mode 100644 index 0000000..81ff60d --- /dev/null +++ b/libsensors/yas530.c @@ -0,0 +1,260 @@ +/* + * 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 "piranha_sensors" +#include + +#include "piranha_sensors.h" + +struct yas530_data { + char path_enable[PATH_MAX]; + char path_delay[PATH_MAX]; + + sensors_vec_t magnetic; +}; + +int yas530_init(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device) +{ + struct yas530_data *data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + ALOGD("%s(%p, %p)", __func__, handlers, device); + + if (handlers == NULL) + return -EINVAL; + + data = (struct yas530_data *) calloc(1, sizeof(struct yas530_data)); + + input_fd = input_open("geomagnetic"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; + } + + rc = sysfs_path_prefix("geomagnetic", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(data->path_enable, PATH_MAX, "%s/enable", path); + snprintf(data->path_delay, PATH_MAX, "%s/delay", path); + + handlers->poll_fd = input_fd; + handlers->data = (void *) data; + + return 0; + +error: + if (data != NULL) + free(data); + + if (input_fd >= 0) + close(input_fd); + + handlers->poll_fd = -1; + handlers->data = NULL; + + return -1; +} + +int yas530_deinit(struct piranha_sensors_handlers *handlers) +{ + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL) + return -EINVAL; + + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); + handlers->poll_fd = -1; + + if (handlers->data != NULL) + free(handlers->data); + handlers->data = NULL; + + return 0; +} + +int yas530_activate(struct piranha_sensors_handlers *handlers) +{ + struct yas530_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct yas530_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int yas530_deactivate(struct piranha_sensors_handlers *handlers) +{ + struct yas530_data *data; + int rc; + + ALOGD("%s(%p)", __func__, handlers); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct yas530_data *) handlers->data; + + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + handlers->activated = 1; + + return 0; +} + +int yas530_set_delay(struct piranha_sensors_handlers *handlers, long int delay) +{ + struct yas530_data *data; + int d; + int rc; + + ALOGD("%s(%p, %ld)", __func__, handlers, delay); + + if (handlers == NULL || handlers->data == NULL) + return -EINVAL; + + data = (struct yas530_data *) handlers->data; + + if (delay < 10000000) + d = 10; + else + d = delay / 1000000; + + rc = sysfs_value_write(data->path_delay, d); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; + } + + return 0; +} + +float yas530_convert(int value) +{ + return value / 1000.0f; +} + +int yas530_get_data(struct piranha_sensors_handlers *handlers, + struct sensors_event_t *event) +{ + struct yas530_data *data; + struct input_event input_event; + int input_fd; + int rc; + +// ALOGD("%s(%p, %p)", __func__, handlers, event); + + if (handlers == NULL || handlers->data == NULL || event == NULL) + return -EINVAL; + + data = (struct yas530_data *) handlers->data; + + 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->magnetic.x = data->magnetic.x; + event->magnetic.y = data->magnetic.y; + event->magnetic.z = data->magnetic.z; + + 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: + event->magnetic.x = yas530_convert(input_event.value); + break; + case ABS_Y: + event->magnetic.y = yas530_convert(input_event.value); + break; + case ABS_Z: + event->magnetic.z = yas530_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); + + data->magnetic.x = event->magnetic.x; + data->magnetic.y = event->magnetic.y; + data->magnetic.z = event->magnetic.z; + + return 0; +} + +struct piranha_sensors_handlers yas530 = { + .name = "YAS530", + .handle = SENSOR_TYPE_MAGNETIC_FIELD, + .init = yas530_init, + .deinit = yas530_deinit, + .activate = yas530_activate, + .deactivate = yas530_deactivate, + .set_delay = yas530_set_delay, + .get_data = yas530_get_data, + .activated = 0, + .needed = 0, + .poll_fd = -1, + .data = NULL, +}; diff --git a/libsensors/yas530c.c b/libsensors/yas530c.c deleted file mode 100644 index f13fb61..0000000 --- a/libsensors/yas530c.c +++ /dev/null @@ -1,293 +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 - -#define LOG_TAG "piranha_sensors" -#include - -#include "piranha_sensors.h" - -#define FLAG_X (1 << 0) -#define FLAG_Y (1 << 1) -#define FLAG_Z (1 << 2) -#define FLAG_ALL (FLAG_X | FLAG_Y | FLAG_Z) - -struct yas530c_data { - char path_enable[PATH_MAX]; - char path_delay[PATH_MAX]; - - sensors_vec_t magnetic; -}; - -int yas530c_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device) -{ - struct yas530c_data *data = NULL; - char path[PATH_MAX] = { 0 }; - int input_fd = -1; - int rc; - - ALOGD("%s(%p, %p)", __func__, handlers, device); - - if (handlers == NULL) - return -EINVAL; - - input_fd = input_open("geomagnetic"); - if (input_fd < 0) { - ALOGE("%s: Unable to open input", __func__); - goto error; - } - - rc = sysfs_path_prefix("geomagnetic", (char *) &path); - if (rc < 0 || path[0] == '\0') { - ALOGE("%s: Unable to open sysfs", __func__); - goto error; - } - - data = (struct yas530c_data *) calloc(1, sizeof(struct yas530c_data)); - - snprintf(data->path_enable, PATH_MAX, "%s/enable", path); - snprintf(data->path_delay, PATH_MAX, "%s/delay", path); - - handlers->poll_fd = input_fd; - handlers->data = (void *) data; - - return 0; - -error: - if (input_fd >= 0) - close(input_fd); - - if (data != NULL) - free(data); - - handlers->poll_fd = -1; - handlers->data = NULL; - - return -1; -} - -int yas530c_deinit(struct piranha_sensors_handlers *handlers) -{ - int input_fd; - - ALOGD("%s(%p)", __func__, handlers); - - if (handlers == NULL) - return -EINVAL; - - input_fd = handlers->poll_fd; - if (input_fd >= 0) - close(input_fd); - - handlers->poll_fd = -1; - - if (handlers->data != NULL) - free(handlers->data); - - handlers->data = NULL; - - return 0; -} - -int yas530c_activate(struct piranha_sensors_handlers *handlers) -{ - struct yas530c_data *data; - char enable[] = "1\n"; - int fd; - - ALOGD("%s(%p)", __func__, handlers); - - if (handlers == NULL || handlers->data == NULL) - return -EINVAL; - - data = (struct yas530c_data *) handlers->data; - - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); - return -1; - } - - write(fd, &enable, sizeof(enable)); - close(fd); - - handlers->activated = 1; - - return 0; -} - -int yas530c_deactivate(struct piranha_sensors_handlers *handlers) -{ - struct yas530c_data *data; - char enable[] = "0\n"; - int fd; - - ALOGD("%s(%p)", __func__, handlers); - - if (handlers == NULL || handlers->data == NULL) - return -EINVAL; - - data = (struct yas530c_data *) handlers->data; - - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); - return -1; - } - - write(fd, &enable, sizeof(enable)); - close(fd); - - handlers->activated = 0; - - return 0; -} - -int yas530c_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay) -{ - struct yas530c_data *data; - char *value = NULL; - int d; - int c; - int fd; - -// ALOGD("%s(%p, %ld)", __func__, handlers, (long int) delay); - - if (handlers == NULL || handlers->data == NULL) - return -EINVAL; - - data = (struct yas530c_data *) handlers->data; - - if (delay < 1000000) - d = 0; - else - d = (int) (delay / 1000000); - - c = asprintf(&value, "%d\n", d); - - fd = open(data->path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); - return -1; - } - - write(fd, value, c); - close(fd); - - if (value != NULL) - free(value); - - return 0; -} - -float yas530c_magnetic(int value) -{ - return (float) value / 1000.0f; -} - -int yas530c_get_data(struct piranha_sensors_handlers *handlers, - struct sensors_event_t *event) -{ - struct yas530c_data *data; - struct input_event input_event; - int input_fd; - int flag; - int rc; - - if (handlers == NULL || handlers->data == NULL || event == NULL) - return -EINVAL; - - data = (struct yas530c_data *) handlers->data; - - input_fd = handlers->poll_fd; - if (input_fd < 0) - return -EINVAL; - - event->version = sizeof(struct sensors_event_t); - event->sensor = handlers->handle; - event->type = handlers->handle; - - event->magnetic.x = data->magnetic.x; - event->magnetic.y = data->magnetic.y; - event->magnetic.z = data->magnetic.z; - event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; - - flag = 0; - while ((flag & FLAG_ALL) != FLAG_ALL) { - rc = read(input_fd, &input_event, sizeof(input_event)); - if (rc < (int) sizeof(input_event)) { - if (flag & FLAG_ALL) - break; - else - return -EINVAL; - } - - if (input_event.type != EV_ABS) - continue; - - switch (input_event.code) { - case ABS_X: - flag |= FLAG_X; - event->magnetic.x = yas530c_magnetic(input_event.value); - break; - case ABS_Y: - flag |= FLAG_Y; - event->magnetic.y = yas530c_magnetic(input_event.value); - break; - case ABS_Z: - flag |= FLAG_Z; - event->magnetic.z = yas530c_magnetic(input_event.value); - break; - default: - continue; - } - event->timestamp = input_timestamp(&input_event); - } - - if (data->magnetic.x != event->magnetic.x) - data->magnetic.x = event->magnetic.x; - if (data->magnetic.y != event->magnetic.y) - data->magnetic.y = event->magnetic.y; - if (data->magnetic.z != event->magnetic.z) - data->magnetic.z = event->magnetic.z; - - return 0; -} - -struct piranha_sensors_handlers yas530c = { - .name = "YAS530C", - .handle = SENSOR_TYPE_MAGNETIC_FIELD, - .init = yas530c_init, - .deinit = yas530c_deinit, - .activate = yas530c_activate, - .deactivate = yas530c_deactivate, - .set_delay = yas530c_set_delay, - .get_data = yas530c_get_data, - .activated = 0, - .poll_fd = -1, - .data = NULL, -}; diff --git a/libsensors/yas_orientation.c b/libsensors/yas_orientation.c index 2cd23d3..3668d8b 100644 --- a/libsensors/yas_orientation.c +++ b/libsensors/yas_orientation.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 @@ -18,9 +18,10 @@ #include #include #include -#include #include #include +#include +#include #include #include @@ -32,38 +33,45 @@ #include "piranha_sensors.h" -#define FLAG_X (1 << 0) -#define FLAG_Y (1 << 1) -#define FLAG_Z (1 << 2) -#define FLAG_ALL (FLAG_X | FLAG_Y | FLAG_Z) - struct yas_orientation_data { - struct piranha_sensors_device *device; + struct piranha_sensors_handlers *acceleration_sensor; + struct piranha_sensors_handlers *magnetic_sensor; char path_enable[PATH_MAX]; char path_delay[PATH_MAX]; - - char acc_path_enable[PATH_MAX]; - char acc_path_delay[PATH_MAX]; - - char mag_path_enable[PATH_MAX]; - char mag_path_delay[PATH_MAX]; - - sensors_vec_t orientation; }; -int yas_orientation_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device) +int yas_orientation_init(struct piranha_sensors_handlers *handlers, + struct piranha_sensors_device *device) { struct yas_orientation_data *data = NULL; char path[PATH_MAX] = { 0 }; int input_fd = -1; int rc; + int i; ALOGD("%s(%p, %p)", __func__, handlers, device); - if (handlers == NULL || device == NULL) + if (handlers == NULL) return -EINVAL; + data = (struct yas_orientation_data *) calloc(1, sizeof(struct yas_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; + } + input_fd = input_open("orientation"); if (input_fd < 0) { ALOGE("%s: Unable to open input", __func__); @@ -76,46 +84,21 @@ int yas_orientation_init(struct piranha_sensors_handlers *handlers, struct piran goto error; } - data = (struct yas_orientation_data *) calloc(1, sizeof(struct yas_orientation_data)); - data->device = device; - snprintf(data->path_enable, PATH_MAX, "%s/enable", path); snprintf(data->path_delay, PATH_MAX, "%s/delay", path); - memset(&path, 0, sizeof(path)); - - rc = sysfs_path_prefix("accelerometer", (char *) &path); - if (rc < 0 || path[0] == '\0') { - ALOGE("%s: Unable to open sysfs", __func__); - goto error; - } - - snprintf(data->acc_path_enable, PATH_MAX, "%s/enable", path); - snprintf(data->acc_path_delay, PATH_MAX, "%s/delay", path); - - memset(&path, 0, sizeof(path)); - - rc = sysfs_path_prefix("geomagnetic", (char *) &path); - if (rc < 0 || path[0] == '\0') { - ALOGE("%s: Unable to open sysfs", __func__); - goto error; - } - - snprintf(data->mag_path_enable, PATH_MAX, "%s/enable", path); - snprintf(data->mag_path_delay, PATH_MAX, "%s/delay", path); - handlers->poll_fd = input_fd; handlers->data = (void *) data; return 0; error: - if (input_fd >= 0) - close(input_fd); - if (data != NULL) free(data); + if (input_fd >= 0) + close(input_fd); + handlers->poll_fd = -1; handlers->data = NULL; @@ -124,22 +107,17 @@ error: int yas_orientation_deinit(struct piranha_sensors_handlers *handlers) { - int input_fd; - ALOGD("%s(%p)", __func__, handlers); if (handlers == NULL) return -EINVAL; - input_fd = handlers->poll_fd; - if (input_fd >= 0) - close(input_fd); - + if (handlers->poll_fd >= 0) + close(handlers->poll_fd); handlers->poll_fd = -1; if (handlers->data != NULL) free(handlers->data); - handlers->data = NULL; return 0; @@ -148,8 +126,6 @@ int yas_orientation_deinit(struct piranha_sensors_handlers *handlers) int yas_orientation_activate(struct piranha_sensors_handlers *handlers) { struct yas_orientation_data *data; - char enable[] = "1\n"; - int fd; int rc; ALOGD("%s(%p)", __func__, handlers); @@ -159,33 +135,23 @@ int yas_orientation_activate(struct piranha_sensors_handlers *handlers) data = (struct yas_orientation_data *) handlers->data; - fd = open(data->acc_path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) return -1; - } - - write(fd, &enable, sizeof(enable)); - close(fd); - fd = open(data->mag_path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); - return -1; - } + data->acceleration_sensor->needed |= PIRANHA_SENSORS_NEEDED_ORIENTATION; + if (data->acceleration_sensor->needed == PIRANHA_SENSORS_NEEDED_ORIENTATION) + data->acceleration_sensor->activate(data->acceleration_sensor); - write(fd, &enable, sizeof(enable)); - close(fd); + data->magnetic_sensor->needed |= PIRANHA_SENSORS_NEEDED_ORIENTATION; + if (data->magnetic_sensor->needed == PIRANHA_SENSORS_NEEDED_ORIENTATION) + data->magnetic_sensor->activate(data->magnetic_sensor); - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + rc = sysfs_value_write(data->path_enable, 1); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, &enable, sizeof(enable)); - close(fd); - handlers->activated = 1; return 0; @@ -194,9 +160,7 @@ int yas_orientation_activate(struct piranha_sensors_handlers *handlers) int yas_orientation_deactivate(struct piranha_sensors_handlers *handlers) { struct yas_orientation_data *data; - char enable[] = "0\n"; - int fd; - int i; + int rc; ALOGD("%s(%p)", __func__, handlers); @@ -205,171 +169,114 @@ int yas_orientation_deactivate(struct piranha_sensors_handlers *handlers) data = (struct yas_orientation_data *) handlers->data; - fd = open(data->path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) return -1; - } - - write(fd, &enable, sizeof(enable)); - close(fd); - for (i=0 ; i < data->device->handlers_count ; i++) { - if (data->device->handlers[i] == NULL) - continue; - - if (data->device->handlers[i]->handle == SENSOR_TYPE_ACCELEROMETER && !data->device->handlers[i]->activated) { - fd = open(data->acc_path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); - continue; - } + data->acceleration_sensor->needed &= ~(PIRANHA_SENSORS_NEEDED_ORIENTATION); + if (data->acceleration_sensor->needed == 0) + data->acceleration_sensor->deactivate(data->acceleration_sensor); - write(fd, &enable, sizeof(enable)); - close(fd); - } else if (data->device->handlers[i]->handle == SENSOR_TYPE_MAGNETIC_FIELD && !data->device->handlers[i]->activated) { - fd = open(data->mag_path_enable, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open enable path", __func__); - continue; - } + data->magnetic_sensor->needed &= ~(PIRANHA_SENSORS_NEEDED_ORIENTATION); + if (data->magnetic_sensor->needed == 0) + data->magnetic_sensor->deactivate(data->magnetic_sensor); - write(fd, &enable, sizeof(enable)); - close(fd); - } + rc = sysfs_value_write(data->path_enable, 0); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); + return -1; } - handlers->activated = 0; + handlers->activated = 1; return 0; } -int yas_orientation_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay) +int yas_orientation_set_delay(struct piranha_sensors_handlers *handlers, long int delay) { struct yas_orientation_data *data; - char *value = NULL; int d; - int c; - int fd; int rc; -// ALOGD("%s(%p, %ld)", __func__, handlers, (long int) delay); + ALOGD("%s(%p, %ld)", __func__, handlers, delay); if (handlers == NULL || handlers->data == NULL) return -EINVAL; data = (struct yas_orientation_data *) handlers->data; - if (delay < 1000000) - d = 0; - else - d = (int) (delay / 1000000); - - c = asprintf(&value, "%d\n", d); - - fd = open(data->acc_path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); + if (data->acceleration_sensor == NULL || data->magnetic_sensor == NULL) return -1; - } - write(fd, value, c); - close(fd); + if (data->acceleration_sensor->needed == PIRANHA_SENSORS_NEEDED_ORIENTATION) + data->acceleration_sensor->set_delay(data->acceleration_sensor, delay); - fd = open(data->mag_path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); - return -1; - } + if (data->magnetic_sensor->needed == PIRANHA_SENSORS_NEEDED_ORIENTATION) + data->magnetic_sensor->set_delay(data->magnetic_sensor, delay); - write(fd, value, c); - close(fd); + if (delay < 10000000) + d = 10; + else + d = delay / 1000000; - fd = open(data->path_delay, O_WRONLY); - if (fd < 0) { - ALOGE("%s: Unable to open delay path", __func__); + rc = sysfs_value_write(data->path_delay, d); + if (rc < 0) { + ALOGE("%s: Unable to write sysfs value", __func__); return -1; } - write(fd, value, c); - close(fd); - - if (value != NULL) - free(value); - return 0; } -float yas_orientation_orientation(int value) +float yas_orientation_convert(int value) { - return (float) value / 1000.f; + return value / 1000.0f; } int yas_orientation_get_data(struct piranha_sensors_handlers *handlers, struct sensors_event_t *event) { - struct yas_orientation_data *data; struct input_event input_event; int input_fd; - int flag; int rc; - if (handlers == NULL || handlers->data == NULL || event == NULL) - return -EINVAL; +// ALOGD("%s(%p, %p)", __func__, handlers, event); - data = (struct yas_orientation_data *) handlers->data; + 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.x = data->orientation.x; - event->orientation.y = data->orientation.y; - event->orientation.z = data->orientation.z; - event->orientation.status = SENSOR_STATUS_ACCURACY_MEDIUM; - - flag = 0; - while ((flag & FLAG_ALL) != FLAG_ALL) { + do { rc = read(input_fd, &input_event, sizeof(input_event)); - if (rc < (int) sizeof(input_event)) { - if (flag & FLAG_ALL) - break; - else - return -EINVAL; - } - - if (input_event.type != EV_ABS) - continue; - - switch (input_event.code) { - case ABS_X: - flag |= FLAG_X; - event->orientation.x = yas_orientation_orientation(input_event.value); - break; - case ABS_Y: - flag |= FLAG_Y; - event->orientation.y = yas_orientation_orientation(input_event.value); - break; - case ABS_Z: - flag |= FLAG_Z; - event->orientation.z = yas_orientation_orientation(input_event.value); - break; - default: - continue; + if (rc < (int) sizeof(input_event)) + break; + + if (input_event.type == EV_ABS) { + switch (input_event.code) { + case ABS_X: + event->orientation.azimuth = yas_orientation_convert(input_event.value); + break; + case ABS_Y: + event->orientation.pitch = yas_orientation_convert(input_event.value); + break; + case ABS_Z: + event->orientation.roll = yas_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); } - event->timestamp = input_timestamp(&input_event); - } - - if (data->orientation.x != event->orientation.x) - data->orientation.x = event->orientation.x; - if (data->orientation.y != event->orientation.y) - data->orientation.y = event->orientation.y; - if (data->orientation.z != event->orientation.z) - data->orientation.z = event->orientation.z; + } while (input_event.type != EV_SYN); return 0; } @@ -384,6 +291,7 @@ struct piranha_sensors_handlers yas_orientation = { .set_delay = yas_orientation_set_delay, .get_data = yas_orientation_get_data, .activated = 0, + .needed = 0, .poll_fd = -1, .data = NULL, }; -- cgit v1.1