diff options
Diffstat (limited to 'libsensors/lsm330dlc_gyroscope.c')
| -rw-r--r-- | libsensors/lsm330dlc_gyroscope.c | 49 |
1 files changed, 31 insertions, 18 deletions
diff --git a/libsensors/lsm330dlc_gyroscope.c b/libsensors/lsm330dlc_gyroscope.c index a741a21..6a7f98d 100644 --- a/libsensors/lsm330dlc_gyroscope.c +++ b/libsensors/lsm330dlc_gyroscope.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Paul Kocialkowski + * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr> * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -20,26 +20,28 @@ #include <stdint.h> #include <fcntl.h> #include <errno.h> +#include <math.h> #include <sys/types.h> #include <linux/ioctl.h> -#include <linux/uinput.h> #include <linux/input.h> #include <hardware/sensors.h> #include <hardware/hardware.h> -#define LOG_TAG "exynos_sensors" +#define LOG_TAG "smdk4x12_sensors" #include <utils/Log.h> -#include "exynos_sensors.h" +#include "smdk4x12_sensors.h" struct lsm330dlc_gyroscope_data { char path_enable[PATH_MAX]; char path_delay[PATH_MAX]; + + sensors_vec_t gyro; }; -int lsm330dlc_gyroscope_init(struct exynos_sensors_handlers *handlers, - struct exynos_sensors_device *device) +int lsm330dlc_gyroscope_init(struct smdk4x12_sensors_handlers *handlers, + struct smdk4x12_sensors_device *device) { struct lsm330dlc_gyroscope_data *data = NULL; char path[PATH_MAX] = { 0 }; @@ -86,7 +88,7 @@ error: return -1; } -int lsm330dlc_gyroscope_deinit(struct exynos_sensors_handlers *handlers) +int lsm330dlc_gyroscope_deinit(struct smdk4x12_sensors_handlers *handlers) { ALOGD("%s(%p)", __func__, handlers); @@ -104,8 +106,7 @@ int lsm330dlc_gyroscope_deinit(struct exynos_sensors_handlers *handlers) return 0; } - -int lsm330dlc_gyroscope_activate(struct exynos_sensors_handlers *handlers) +int lsm330dlc_gyroscope_activate(struct smdk4x12_sensors_handlers *handlers) { struct lsm330dlc_gyroscope_data *data; int rc; @@ -128,7 +129,7 @@ int lsm330dlc_gyroscope_activate(struct exynos_sensors_handlers *handlers) return 0; } -int lsm330dlc_gyroscope_deactivate(struct exynos_sensors_handlers *handlers) +int lsm330dlc_gyroscope_deactivate(struct smdk4x12_sensors_handlers *handlers) { struct lsm330dlc_gyroscope_data *data; int rc; @@ -151,7 +152,7 @@ int lsm330dlc_gyroscope_deactivate(struct exynos_sensors_handlers *handlers) return 0; } -int lsm330dlc_gyroscope_set_delay(struct exynos_sensors_handlers *handlers, long int delay) +int lsm330dlc_gyroscope_set_delay(struct smdk4x12_sensors_handlers *handlers, long int delay) { struct lsm330dlc_gyroscope_data *data; int rc; @@ -174,29 +175,37 @@ int lsm330dlc_gyroscope_set_delay(struct exynos_sensors_handlers *handlers, long float lsm330dlc_gyroscope_convert(int value) { - return ((float) value * 0.3054326f) / 1000.0f; + return value * (70.0f / 4000.0f) * (3.1415926535f / 180.0f); } -int lsm330dlc_gyroscope_get_data(struct exynos_sensors_handlers *handlers, +int lsm330dlc_gyroscope_get_data(struct smdk4x12_sensors_handlers *handlers, struct sensors_event_t *event) { + struct lsm330dlc_gyroscope_data *data; struct input_event input_event; int input_fd; int rc; // ALOGD("%s(%p, %p)", __func__, handlers, event); - if (handlers == NULL || event == NULL) + if (handlers == NULL || handlers->data == NULL || event == NULL) return -EINVAL; + data = (struct lsm330dlc_gyroscope_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->gyro.x = data->gyro.x; + event->gyro.y = data->gyro.y; + event->gyro.z = data->gyro.z; + do { rc = read(input_fd, &input_event, sizeof(input_event)); if (rc < (int) sizeof(input_event)) @@ -205,13 +214,13 @@ int lsm330dlc_gyroscope_get_data(struct exynos_sensors_handlers *handlers, if (input_event.type == EV_REL) { switch (input_event.code) { case REL_RX: - event->magnetic.x = lsm330dlc_gyroscope_convert(input_event.value); + event->gyro.x = lsm330dlc_gyroscope_convert(input_event.value); break; case REL_RY: - event->magnetic.y = lsm330dlc_gyroscope_convert(input_event.value); + event->gyro.y = lsm330dlc_gyroscope_convert(input_event.value); break; case REL_RZ: - event->magnetic.z = lsm330dlc_gyroscope_convert(input_event.value); + event->gyro.z = lsm330dlc_gyroscope_convert(input_event.value); break; default: continue; @@ -222,10 +231,14 @@ int lsm330dlc_gyroscope_get_data(struct exynos_sensors_handlers *handlers, } } while (input_event.type != EV_SYN); + data->gyro.x = event->gyro.x; + data->gyro.y = event->gyro.y; + data->gyro.z = event->gyro.z; + return 0; } -struct exynos_sensors_handlers lsm330dlc_gyroscope = { +struct smdk4x12_sensors_handlers lsm330dlc_gyroscope = { .name = "LSM330DLC Gyroscope", .handle = SENSOR_TYPE_GYROSCOPE, .init = lsm330dlc_gyroscope_init, |