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 <inttypes.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_1 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 device__batch(struct sensors_poll_device_1* dev, int sensor_handle,
 193                 int flags, int64_t sampling_period_ns, int64_t max_report_latency_ns) {
 194         ALOGD("%s: dev=%p sensor_handle=%d flags=%d sampling_period_ns=%" PRId64 " max_report_latency_ns=%" PRId64,
 195                         __FUNCTION__, dev, sensor_handle, flags, sampling_period_ns, max_report_latency_ns);
 196         forced_delay = sampling_period_ns / 1000;
 197         return EXIT_SUCCESS;
 198 }
 199
 200 static int device__flush(struct sensors_poll_device_1* dev, int sensor_handle) {
 201         ALOGD("%s: dev=%p sensor_handle=%d", __FUNCTION__, dev, sensor_handle);
 202         return EXIT_SUCCESS;
 203 }
 204
 205 static int open_input_device(void) {
 206         char *filename;
 207         int fd = -1;
 208         DIR *dir;
 209         struct dirent *de;
 210         char name[80];
 211         char devname[256];
 212         dir = opendir(INPUT_DIR);
 213         if (dir == NULL)
 214                 return -1;
 215
 216         strcpy(devname, INPUT_DIR);
 217         filename = devname + strlen(devname);
 218         *filename++ = '/';
 219
 220         while ((de = readdir(dir))) {
 221                 int f;
 222                 size_t i;
 223                 if (de->d_name[0] == '.')
 224                         continue;
 225                 strcpy(filename, de->d_name);
 226                 f = open(devname, O_RDONLY);
 227                 if (f < 0) {
 228                         continue;
 229                 }
 230
 231                 if (ioctl(f, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
 232                         name[0] = '\0';
 233                 }
 234
 235                 ALOGV("%s name is %s", devname, name);
 236
 237                 for (i = 0; i < ARRAY_SIZE(accel_list); ++i) {
 238                         if (!strcmp(name, accel_list[i].name)) {
 239                                 int c = accel_list[i].avg_cnt;
 240                                 memcpy(&accelerometer, &accel_list[i], sizeof(accel_params));
 241                                 accelerometer.conv[0] = accel_list[i].conv[0] / c;
 242                                 accelerometer.conv[1] = accel_list[i].conv[1] / c;
 243                                 accelerometer.conv[2] = accel_list[i].conv[2] / c;
 244                                 events_q = calloc(c, sizeof(sensors_vec_t));
 245                                 ALOGI("found %s at %s", name, devname);
 246                                 break;
 247                         }
 248                 }
 249                 if (events_q) {
 250                         fd = f;
 251                         break;
 252                 }
 253                 close(f);
 254         }
 255         closedir(dir);
 256
 257         return fd;
 258 }
 259
 260 static const struct sensor_t sSensorList[] = {
 261         {       .name = "HDAPS accelerometer",
 262                 .vendor = "Linux kernel",
 263                 .version = 1,
 264                 .handle = ID_ACCELERATION,
 265                 .type = SENSOR_TYPE_ACCELEROMETER,
 266                 .maxRange = (GRAVITY_EARTH * 6.0f),
 267                 .resolution = (GRAVITY_EARTH * 6.0f) / 1024.0f,
 268                 .power = 0.84f,
 269                 .reserved = {},
 270         },
 271 };
 272
 273 static int open_sensors(const struct hw_module_t* module, const char* name,
 274                 struct hw_device_t** device);
 275
 276 static int sensors__get_sensors_list(struct sensors_module_t* module,
 277                 struct sensor_t const** list) {
 278         *list = sSensorList;
 279
 280         return ARRAY_SIZE(sSensorList);
 281 }
 282
 283 static struct hw_module_methods_t sensors_module_methods = {
 284         .open = open_sensors
 285 };
 286
 287 struct sensors_module_t HAL_MODULE_INFO_SYM = {
 288         .common = {
 289                 .tag = HARDWARE_MODULE_TAG,
 290                 .module_api_version = 2,
 291                 .hal_api_version = 0,
 292                 .id = SENSORS_HARDWARE_MODULE_ID,
 293                 .name = "hdaps accelerometer sensor",
 294                 .author = "Stefan Seidel",
 295                 .methods = &sensors_module_methods,
 296                 .dso = NULL,
 297                 .reserved = {},
 298         },
 299         .get_sensors_list = sensors__get_sensors_list
 300 };
 301
 302 static int open_sensors(const struct hw_module_t* module, const char* name,
 303                 struct hw_device_t** device) {
 304         int status = -EINVAL;
 305
 306         struct sensors_poll_context_t *dev = calloc(1,
 307                         sizeof(struct sensors_poll_context_t));
 308
 309         dev->device.common.tag = HARDWARE_DEVICE_TAG;
 310         dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_3;
 311         dev->device.common.module = (struct hw_module_t*) module;
 312         dev->device.common.close = common__close;
 313         dev->device.activate = device__activate;
 314         dev->device.setDelay = device__set_delay;
 315         dev->device.poll = device__poll;
 316         dev->device.batch = device__batch;
 317         dev->device.flush = device__flush;
 318
 319         if ((dev->fd = open_input_device()) < 0) {
 320                 ALOGE("GSensor get class path error");
 321         } else {
 322                 *device = &dev->device.common;
 323                 status = 0;
 324         }
 325
 326         return status;
 327 }