hdaps.c

   1 /**
   2  * HDAPS accelerometer sensor
   3  *
   4  * Copyright (C) 2011 The Android-x86 Open Source Project
   5  *
   6  * by Stefan Seidel <stefans@android-x86.org>
   7  * Adaptation by Tanguy Pruvot <tpruvot@github>
   8  *
   9  * Licensed under GPLv2 or later
  10  *
  11  **/
  12
  13 /* #define LOG_NDEBUG 0 */
  14 #define LOG_TAG "HdapsSensors"
  15
  16 #include <stdint.h>
  17 #include <string.h>
  18 #include <unistd.h>
  19 #include <stdlib.h>
  20 #include <stdio.h>
  21 #include <fcntl.h>
  22 #include <errno.h>
  23 #include <dirent.h>
  24 #include <math.h>
  25 #include <poll.h>
  26 #include <pthread.h>
  27 #include <linux/input.h>
  28
  29 #include <cutils/atomic.h>
  30 #include <cutils/log.h>
  31 #include <cutils/properties.h>
  32 #include <hardware/sensors.h>
  33
  34 #define INPUT_DIR               "/dev/input"
  35 #define ARRAY_SIZE(a)           (sizeof(a) / sizeof(a[0]))
  36 #define ID_ACCELERATION         (SENSORS_HANDLE_BASE + 0)
  37
  38 #define AMIN(a,b)               (((a)<(fabs(b)))?(a):(b))
  39 #define SQUARE(x)               ((x)*(x))
  40 #define COS_ASIN(m,x)           (sqrt(SQUARE(m)-SQUARE(AMIN(m,x))))
  41 #define COS_ASIN_2D(m,x,y)      (COS_ASIN(m,x)*COS_ASIN(m,y)/(m))
  42
  43 typedef struct {
  44         const char *name;
  45         float conv[3];
  46         int swap[3];
  47         int avg_cnt;
  48 } accel_params;
  49
  50 /* precision, result data should give GRAVITY_EARTH ~9.8 */
  51 #define CONVERT                 (GRAVITY_EARTH / 156.0f)
  52 #define CONVERT_PEGA            (GRAVITY_EARTH / 256.0f)
  53 #define CONVERT_LIS             (GRAVITY_EARTH / 1024.0f)
  54
  55 /* axis swap for tablet pcs, X=0, Y=1, Z=2 */
  56 #define NO_SWAP                 { 0, 1, 2 }
  57 #define SWAP_YXZ                { 1, 0, 2 }
  58 #define SWAP_ZXY                { 2, 0, 1 }
  59
  60 static accel_params accel_list[] = {
  61         { "hdaps", { CONVERT, -CONVERT, 0 }, NO_SWAP, 1 },
  62         { "Pegatron Lucid Tablet Accelerometer", { CONVERT_PEGA, CONVERT_PEGA, CONVERT_PEGA }, NO_SWAP, 4 },
  63 //      { "ST LIS3LV02DL Accelerometer",  { -CONVERT_LIS, CONVERT_LIS, CONVERT_LIS }, SWAP_YXZ, 2 }, /* tablet mode */
  64         { "ST LIS3LV02DL Accelerometer",  { CONVERT_LIS, -CONVERT_LIS, CONVERT_LIS }, SWAP_ZXY, 2 }, /* pc mode */
  65 };
  66
  67 static accel_params accelerometer;
  68 static sensors_vec_t *events_q = NULL;
  69 static int event_cnt = 0;
  70 static unsigned int forced_delay = 0;
  71
  72 struct sensors_poll_context_t {
  73         struct sensors_poll_device_t device;
  74         int fd;
  75 };
  76
  77 static int common__close(struct hw_device_t *dev) {
  78         struct sensors_poll_context_t *ctx = (struct sensors_poll_context_t *) dev;
  79         free(ctx);
  80         free(events_q);
  81         events_q = NULL;
  82
  83         return 0;
  84 }
  85
  86 static int device__activate(struct sensors_poll_device_t *dev, int handle,
  87                 int enabled) {
  88
  89         return 0;
  90 }
  91
  92 static int device__set_delay(struct sensors_poll_device_t *device, int handle,
  93                 int64_t ns) {
  94         forced_delay = ns / 1000;
  95         return 0;
  96
  97 }
  98
  99 static int device__poll(struct sensors_poll_device_t *device,
 100                 sensors_event_t *data, int count) {
 101
 102         struct input_event event;
 103         struct sensors_poll_context_t *dev =
 104                         (struct sensors_poll_context_t *) device;
 105
 106         accel_params signs;
 107         char prop[PROPERTY_VALUE_MAX] = "";
 108         float val;
 109         int x, y, z;
 110
 111         if (dev->fd < 0)
 112                 return 0;
 113
 114         // dynamic axis tuning, expect "1,-1,-1" format
 115         if (property_get("hal.sensors.axis.revert", prop, 0)) {
 116                 sscanf(prop, "%d,%d,%d", &x, &y, &z);
 117                 ALOGD("axis signs set to %d %d %d", x, y, z);
 118         } else {
 119                 x = y = z = 1;
 120         }
 121         signs.conv[ABS_X] = accelerometer.conv[ABS_X] * x;
 122         signs.conv[ABS_Y] = accelerometer.conv[ABS_Y] * y;
 123         signs.conv[ABS_Z] = accelerometer.conv[ABS_Z] * z;
 124
 125         ALOGV("axis convert set to %.6f %.6f %.6f",
 126                 signs.conv[ABS_X], signs.conv[ABS_Y] ,signs.conv[ABS_Z]);
 127
 128         // dynamic axis swap, expect "0,1,2" format
 129         if (property_get("hal.sensors.axis.order", prop, 0)) {
 130                 sscanf(prop, "%d,%d,%d", &x, &y, &z);
 131                 ALOGD("axis order set to %c %c %c", 'x'+x, 'x'+y, 'x'+z);
 132         } else {
 133                 // use default values (accel_params)
 134                 x = accelerometer.swap[0];
 135                 y = accelerometer.swap[1];
 136                 z = accelerometer.swap[2];
 137         }
 138
 139         while (read(dev->fd, &event, sizeof(event)) > 0) {
 140
 141                 ALOGV("gsensor event %d - %d - %d", event.type, event.code, event.value);
 142
 143                 if (event.type == EV_ABS) {
 144                         switch (event.code) {
 145                         // Even though this mapping results in wrong results with some apps,
 146                         // it just means that these apps are broken, i.e. they rely on the
 147                         // fact that phone have portrait displays. Laptops/tablets however
 148                         // have landscape displays, and the axes are relative to the default
 149                         // screen orientation, not relative to portrait orientation.
 150                         // See the nVidia Tegra accelerometer docs if you want to know for sure.
 151                         case ABS_X: // 0x00
 152                         case ABS_Y: // 0x01
 153                         case ABS_Z: // 0x02
 154                                 val = signs.conv[event.code] * event.value;
 155                                 if (event.code == ABS_X)
 156                                         events_q[event_cnt].v[x] = val;
 157                                 else if (event.code == ABS_Y)
 158                                         events_q[event_cnt].v[y] = val;
 159                                 else if (event.code == ABS_Z)
 160                                         events_q[event_cnt].v[z] = val;
 161                                 break;
 162                         }
 163                 } else if (event.type == EV_SYN) {
 164                         int i;
 165                         data->timestamp = (int64_t) ((int64_t) event.time.tv_sec
 166                                         * 1000000000 + (int64_t) event.time.tv_usec * 1000);
 167                         // hdaps doesn't have z-axis, so simulate it by rotation matrix solution
 168                         if (signs.conv[2] == 0)
 169                                 events_q[event_cnt].z = COS_ASIN_2D(GRAVITY_EARTH, events_q[event_cnt].x, events_q[event_cnt].y);
 170                         memset(&data->acceleration, 0, sizeof(sensors_vec_t));
 171                         for (i = 0; i < accelerometer.avg_cnt; ++i) {
 172                                 data->acceleration.x += events_q[i].x;
 173                                 data->acceleration.y += events_q[i].y;
 174                                 data->acceleration.z += events_q[i].z;
 175                         }
 176                         data->sensor = ID_ACCELERATION;
 177                         data->type = SENSOR_TYPE_ACCELEROMETER;
 178                         data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 179                         if (++event_cnt >= accelerometer.avg_cnt)
 180                                 event_cnt = 0;
 181
 182                         // spare the CPU if desired
 183                         if (forced_delay)
 184                                 usleep(forced_delay);
 185                         return 1;
 186                 }
 187         }
 188
 189         return -errno;
 190 }
 191
 192 static int open_input_device(void) {
 193         char *filename;
 194         int fd = -1;
 195         DIR *dir;
 196         struct dirent *de;
 197         char name[80];
 198         char devname[256];
 199         dir = opendir(INPUT_DIR);
 200         if (dir == NULL)
 201                 return -1;
 202
 203         strcpy(devname, INPUT_DIR);
 204         filename = devname + strlen(devname);
 205         *filename++ = '/';
 206
 207         while ((de = readdir(dir))) {
 208                 int f;
 209                 size_t i;
 210                 if (de->d_name[0] == '.')
 211                         continue;
 212                 strcpy(filename, de->d_name);
 213                 f = open(devname, O_RDONLY);
 214                 if (f < 0) {
 215                         continue;
 216                 }
 217
 218                 if (ioctl(f, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
 219                         name[0] = '\0';
 220                 }
 221
 222                 ALOGV("%s name is %s", devname, name);
 223
 224                 for (i = 0; i < ARRAY_SIZE(accel_list); ++i) {
 225                         if (!strcmp(name, accel_list[i].name)) {
 226                                 int c = accel_list[i].avg_cnt;
 227                                 memcpy(&accelerometer, &accel_list[i], sizeof(accel_params));
 228                                 accelerometer.conv[0] = accel_list[i].conv[0] / c;
 229                                 accelerometer.conv[1] = accel_list[i].conv[1] / c;
 230                                 accelerometer.conv[2] = accel_list[i].conv[2] / c;
 231                                 events_q = calloc(c, sizeof(sensors_vec_t));
 232                                 ALOGI("found %s at %s", name, devname);
 233                                 break;
 234                         }
 235                 }
 236                 if (events_q) {
 237                         fd = f;
 238                         break;
 239                 }
 240                 close(f);
 241         }
 242         closedir(dir);
 243
 244         return fd;
 245 }
 246
 247 static const struct sensor_t sSensorList[] = {
 248         {       .name = "HDAPS accelerometer",
 249                 .vendor = "Linux kernel",
 250                 .version = 1,
 251                 .handle = ID_ACCELERATION,
 252                 .type = SENSOR_TYPE_ACCELEROMETER,
 253                 .maxRange = (GRAVITY_EARTH * 6.0f),
 254                 .resolution = (GRAVITY_EARTH * 6.0f) / 1024.0f,
 255                 .power = 0.84f,
 256                 .reserved = {},
 257         },
 258 };
 259
 260 static int open_sensors(const struct hw_module_t* module, const char* name,
 261                 struct hw_device_t** device);
 262
 263 static int sensors__get_sensors_list(struct sensors_module_t* module,
 264                 struct sensor_t const** list) {
 265         *list = sSensorList;
 266
 267         return ARRAY_SIZE(sSensorList);
 268 }
 269
 270 static struct hw_module_methods_t sensors_module_methods = {
 271         .open = open_sensors
 272 };
 273
 274 struct sensors_module_t HAL_MODULE_INFO_SYM = {
 275         .common = {
 276                 .tag = HARDWARE_MODULE_TAG,
 277                 .version_major = 2,
 278                 .version_minor = 0,
 279                 .id = SENSORS_HARDWARE_MODULE_ID,
 280                 .name = "hdaps accelerometer sensor",
 281                 .author = "Stefan Seidel",
 282                 .methods = &sensors_module_methods,
 283                 .dso = NULL,
 284                 .reserved = {},
 285         },
 286         .get_sensors_list = sensors__get_sensors_list
 287 };
 288
 289 static int open_sensors(const struct hw_module_t* module, const char* name,
 290                 struct hw_device_t** device) {
 291         int status = -EINVAL;
 292
 293         struct sensors_poll_context_t *dev = calloc(1,
 294                         sizeof(struct sensors_poll_context_t));
 295
 296         dev->device.common.tag = HARDWARE_DEVICE_TAG;
 297         dev->device.common.version = 0;
 298         dev->device.common.module = (struct hw_module_t*) module;
 299         dev->device.common.close = common__close;
 300         dev->device.activate = device__activate;
 301         dev->device.setDelay = device__set_delay;
 302         dev->device.poll = device__poll;
 303
 304         if ((dev->fd = open_input_device()) < 0) {
 305                 ALOGE("GSensor get class path error");
 306         } else {
 307                 *device = &dev->device.common;
 308                 status = 0;
 309         }
 310
 311         return status;
 312 }