orientation, but it freezes android
authorPaul Kocialkowski <contact@paulk.fr>
Sat, 23 Feb 2013 08:45:23 +0000 (09:45 +0100)
committerPaul Kocialkowski <contact@paulk.fr>
Sat, 23 Feb 2013 08:45:23 +0000 (09:45 +0100)
Android.mk
bh1721.c
bma250.c
input.c
piranha_sensors.c
piranha_sensors.h
yas530c.c
yas_orientation.c [new file with mode: 0644]

index 9bb6c51..1c62649 100644 (file)
@@ -23,8 +23,9 @@ LOCAL_SRC_FILES := \
        piranha_sensors.c \
        input.c \
        bma250.c \
-       bh1721.c \
-       yas530c.c
+       yas530c.c \
+       yas_orientation.c \
+       bh1721.c
 
 LOCAL_SHARED_LIBRARIES := libutils libcutils liblog libhardware
 LOCAL_PRELINK_MODULE := false
index dcf0a26..6b0e76e 100644 (file)
--- a/bh1721.c
+++ b/bh1721.c
@@ -36,14 +36,14 @@ struct bh1721_data {
        char path_delay[PATH_MAX];
 };
 
-int bh1721_init(struct piranha_sensors_handlers *handlers)
+int bh1721_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device)
 {
        struct bh1721_data *data = NULL;
        char path[PATH_MAX] = { 0 };
        int input_fd = -1;
        int rc;
 
-       LOGD("%s(%p)", __func__, handlers);
+       LOGD("%s(%p, %p)", __func__, handlers, device);
 
        if (handlers == NULL)
                return -EINVAL;
@@ -174,14 +174,14 @@ int bh1721_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay)
 
        data = (struct bh1721_data *) handlers->data;
 
+       c = asprintf(&value, "%ld\n", (long int) delay);
+
        fd = open(data->path_delay, O_WRONLY);
        if (fd < 0) {
                LOGE("%s: Unable to open delay path", __func__);
                return -1;
        }
 
-       c = asprintf(&value, "%ld\n", (long int) delay);
-
        write(fd, value, c);
        close(fd);
 
index 09ada5d..09d2f84 100644 (file)
--- a/bma250.c
+++ b/bma250.c
@@ -43,14 +43,14 @@ struct bma250_data {
        sensors_vec_t acceleration;
 };
 
-int bma250_init(struct piranha_sensors_handlers *handlers)
+int bma250_init(struct piranha_sensors_handlers *handlers, struct piranha_sensors_device *device)
 {
        struct bma250_data *data = NULL;
        char path[PATH_MAX] = { 0 };
        int input_fd = -1;
        int rc;
 
-       LOGD("%s(%p)", __func__, handlers);
+       LOGD("%s(%p, %p)", __func__, handlers, device);
 
        if (handlers == NULL)
                return -EINVAL;
@@ -182,12 +182,6 @@ int bma250_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay)
 
        data = (struct bma250_data *) handlers->data;
 
-       fd = open(data->path_delay, O_WRONLY);
-       if (fd < 0) {
-               LOGE("%s: Unable to open delay path", __func__);
-               return -1;
-       }
-
        if (delay < 1000000)
                d = 0;
        else
@@ -195,6 +189,12 @@ int bma250_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay)
 
        c = asprintf(&value, "%d\n", d);
 
+       fd = open(data->path_delay, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open delay path", __func__);
+               return -1;
+       }
+
        write(fd, value, c);
        close(fd);
 
diff --git a/input.c b/input.c
index b871da9..1f8a427 100644 (file)
--- a/input.c
+++ b/input.c
@@ -67,7 +67,7 @@ int input_open(char *name)
                if (rc < 0)
                        continue;
 
-               if (strncmp(input_name, name, strlen(name)) == 0)
+               if (strcmp(input_name, name) == 0)
                        return fd;
                else
                        close(fd);
index c1ffd07..5c1532c 100644 (file)
@@ -39,6 +39,8 @@ struct sensor_t piranha_sensors[] = {
                SENSOR_TYPE_ACCELEROMETER, 19.6f, 0.0383f, 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,
+               SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 0.0f, 10000, {}, },
        { "BH1721 Light Sensor", "ROHM", 1, SENSOR_TYPE_LIGHT,
                SENSOR_TYPE_LIGHT, 0.0f, 0.0f, 0.0f, 0, {}, },
 };
@@ -47,8 +49,9 @@ int piranha_sensors_count = sizeof(piranha_sensors) / sizeof(struct sensor_t);
 
 struct piranha_sensors_handlers *piranha_sensors_handlers[] = {
        &bma250,
-       &bh1721,
        &yas530c,
+       &yas_orientation,
+       &bh1721,
 };
 
 int piranha_sensors_handlers_count = sizeof(piranha_sensors_handlers) /
@@ -164,7 +167,6 @@ int piranha_sensors_poll(struct sensors_poll_device_t *dev,
                                        continue;
 
                                if (device->poll_fds[i].revents & POLLIN && device->poll_fds[i].fd == device->handlers[j]->poll_fd && device->handlers[j]->get_data != NULL) {
-                                       device->poll_fds[i].revents = 0;
                                        rc = device->handlers[j]->get_data(device->handlers[j], &data[n]);
                                        if (rc < 0)
                                                continue;
@@ -243,7 +245,7 @@ int piranha_sensors_open(const struct hw_module_t* module, const char *id,
                if (piranha_sensors_handlers[i] == NULL || piranha_sensors_handlers[i]->init == NULL)
                        continue;
 
-               piranha_sensors_handlers[i]->init(piranha_sensors_handlers[i]);
+               piranha_sensors_handlers[i]->init(piranha_sensors_handlers[i], piranha_sensors_device);
        }
 
        *device = &(piranha_sensors_device->device.common);
index bc03835..6ed3053 100644 (file)
 #ifndef _SENSORS_H_
 #define _SENSORS_H_
 
+struct piranha_sensors_device;
+
 struct piranha_sensors_handlers {
        char *name;
        int handle;
 
-       int (*init)(struct piranha_sensors_handlers *handlers);
+       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);
@@ -52,6 +54,14 @@ struct piranha_sensors_device {
        struct pollfd *poll_fds;
 };
 
+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_poll(struct sensors_poll_device_t *dev,
+       struct sensors_event_t* data, int count);
+
 /*
  * Input
  */
@@ -65,7 +75,8 @@ int sysfs_path_prefix(char *name, char *path_prefix);
  */
 
 extern struct piranha_sensors_handlers bma250;
-extern struct piranha_sensors_handlers bh1721;
 extern struct piranha_sensors_handlers yas530c;
+extern struct piranha_sensors_handlers yas_orientation;
+extern struct piranha_sensors_handlers bh1721;
 
 #endif
index 60af109..5cf238d 100644 (file)
--- a/yas530c.c
+++ b/yas530c.c
@@ -43,14 +43,14 @@ struct yas530c_data {
        sensors_vec_t magnetic;
 };
 
-int yas530c_init(struct piranha_sensors_handlers *handlers)
+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;
 
-       LOGD("%s(%p)", __func__, handlers);
+       LOGD("%s(%p, %p)", __func__, handlers, device);
 
        if (handlers == NULL)
                return -EINVAL;
@@ -182,12 +182,6 @@ int yas530c_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay)
 
        data = (struct yas530c_data *) handlers->data;
 
-       fd = open(data->path_delay, O_WRONLY);
-       if (fd < 0) {
-               LOGE("%s: Unable to open delay path", __func__);
-               return -1;
-       }
-
        if (delay < 1000000)
                d = 0;
        else
@@ -195,6 +189,12 @@ int yas530c_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay)
 
        c = asprintf(&value, "%d\n", d);
 
+       fd = open(data->path_delay, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open delay path", __func__);
+               return -1;
+       }
+
        write(fd, value, c);
        close(fd);
 
diff --git a/yas_orientation.c b/yas_orientation.c
new file mode 100644 (file)
index 0000000..e6a04b9
--- /dev/null
@@ -0,0 +1,363 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <linux/ioctl.h>
+#include <linux/input.h>
+
+#include <hardware/sensors.h>
+#include <hardware/hardware.h>
+
+#define LOG_TAG "piranha_sensors"
+#include <utils/Log.h>
+
+#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;
+
+       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)
+{
+       struct yas_orientation_data *data = NULL;
+       char path[PATH_MAX] = { 0 };
+       int input_fd = -1;
+       int rc;
+
+       LOGD("%s(%p, %p)", __func__, handlers, device);
+
+       if (handlers == NULL || device == NULL)
+               return -EINVAL;
+
+       input_fd = input_open("orientation");
+       if (input_fd < 0) {
+               LOGE("%s: Unable to open input", __func__);
+               goto error;
+       }
+
+       rc = sysfs_path_prefix("orientation", (char *) &path);
+       if (rc < 0 || path[0] == '\0') {
+               LOGE("%s: Unable to open sysfs", __func__);
+               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') {
+               LOGE("%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') {
+               LOGE("%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);
+
+       handlers->poll_fd = -1;
+       handlers->data = NULL;
+
+       return -1;
+}
+
+int yas_orientation_deinit(struct piranha_sensors_handlers *handlers)
+{
+       int input_fd;
+
+       LOGD("%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 yas_orientation_activate(struct piranha_sensors_handlers *handlers)
+{
+       struct yas_orientation_data *data;
+       char enable[] = "1\n";
+       int fd;
+       int rc;
+
+       LOGD("%s(%p)", __func__, handlers);
+
+       if (handlers == NULL || handlers->data == NULL)
+               return -EINVAL;
+
+       data = (struct yas_orientation_data *) handlers->data;
+
+       fd = open(data->acc_path_enable, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open enable path", __func__);
+               return -1;
+       }
+
+       write(fd, &enable, sizeof(enable));
+       close(fd);
+
+       fd = open(data->mag_path_enable, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open enable path", __func__);
+               return -1;
+       }
+
+       write(fd, &enable, sizeof(enable));
+       close(fd);
+
+       fd = open(data->path_enable, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open enable path", __func__);
+               return -1;
+       }
+
+       write(fd, &enable, sizeof(enable));
+       close(fd);
+
+       handlers->activated = 1;
+
+       return 0;
+}
+
+int yas_orientation_deactivate(struct piranha_sensors_handlers *handlers)
+{
+       struct yas_orientation_data *data;
+       char enable[] = "0\n";
+       int fd;
+
+       LOGD("%s(%p)", __func__, handlers);
+
+       if (handlers == NULL || handlers->data == NULL)
+               return -EINVAL;
+
+       data = (struct yas_orientation_data *) handlers->data;
+
+       fd = open(data->path_enable, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open enable path", __func__);
+               return -1;
+       }
+
+       write(fd, &enable, sizeof(enable));
+       close(fd);
+
+       handlers->activated = 0;
+
+       return 0;
+}
+
+int yas_orientation_set_delay(struct piranha_sensors_handlers *handlers, int64_t delay)
+{
+       struct yas_orientation_data *data;
+       char *value = NULL;
+       int d;
+       int c;
+       int fd;
+       int rc;
+
+//     LOGD("%s(%p, %ld)", __func__, handlers, (long int) 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) {
+               LOGE("%s: Unable to open delay path", __func__);
+               return -1;
+       }
+
+       write(fd, value, c);
+       close(fd);
+
+       fd = open(data->mag_path_delay, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open delay path", __func__);
+               return -1;
+       }
+
+       write(fd, value, c);
+       close(fd);
+
+       fd = open(data->path_delay, O_WRONLY);
+       if (fd < 0) {
+               LOGE("%s: Unable to open delay path", __func__);
+               return -1;
+       }
+
+       write(fd, value, c);
+       close(fd);
+
+       if (value != NULL)
+               free(value);
+
+       return 0;
+}
+
+float yas_orientation_orientation(int value)
+{
+       return (float) value / 1000.f;
+}
+
+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;
+
+       data = (struct yas_orientation_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->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) {
+               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;
+               }
+               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;
+
+       return 0;
+}
+
+struct piranha_sensors_handlers yas_orientation = {
+       .name = "YAS Orientation",
+       .handle = SENSOR_TYPE_ORIENTATION,
+       .init = yas_orientation_init,
+       .deinit = yas_orientation_deinit,
+       .activate = yas_orientation_activate,
+       .deactivate = yas_orientation_deactivate,
+       .set_delay = yas_orientation_set_delay,
+       .get_data = yas_orientation_get_data,
+       .activated = 0,
+       .poll_fd = -1,
+       .data = NULL,
+};