transform.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <stdlib.h>
   6 #include <math.h>
   7 #include <utils/Log.h>
   8 #include <cutils/properties.h>
   9 #include <hardware/sensors.h>
  10 #include "calibration.h"
  11 #include "common.h"
  12 #include "description.h"
  13 #include "transform.h"
  14 #include "utils.h"
  15 #include "filtering.h"
  16
  17 /*----------------------------------------------------------------------------*/
  18
  19 /* Macros related to Intel Sensor Hub */
  20
  21 #define GRAVITY 9.80665f
  22
  23 /* 720 LSG = 1G */
  24 #define LSG                         (1024.0)
  25 #define NUMOFACCDATA                (8.0)
  26
  27 /* conversion of acceleration data to SI units (m/s^2) */
  28 #define CONVERT_A                   (GRAVITY_EARTH / LSG / NUMOFACCDATA)
  29 #define CONVERT_A_X(x)              ((float(x)/1000) * (GRAVITY * -1.0))
  30 #define CONVERT_A_Y(x)              ((float(x)/1000) * (GRAVITY * 1.0))
  31 #define CONVERT_A_Z(x)              ((float(x)/1000) * (GRAVITY * 1.0))
  32
  33 /* conversion of magnetic data to uT units */
  34 #define CONVERT_M                   (1.0/6.6)
  35 #define CONVERT_M_X                 (-CONVERT_M)
  36 #define CONVERT_M_Y                 (-CONVERT_M)
  37 #define CONVERT_M_Z                 (CONVERT_M)
  38
  39 #define CONVERT_GAUSS_TO_MICROTESLA(x)        ((x) * 100 )
  40
  41 /* conversion of orientation data to degree units */
  42 #define CONVERT_O                   (1.0/64)
  43 #define CONVERT_O_A                 (CONVERT_O)
  44 #define CONVERT_O_P                 (CONVERT_O)
  45 #define CONVERT_O_R                 (-CONVERT_O)
  46
  47 /*conversion of gyro data to SI units (radian/sec) */
  48 #define CONVERT_GYRO                (2000.0/32767*M_PI/180)
  49 #define CONVERT_GYRO_X              (-CONVERT_GYRO)
  50 #define CONVERT_GYRO_Y              (-CONVERT_GYRO)
  51 #define CONVERT_GYRO_Z              (CONVERT_GYRO)
  52
  53 #define BIT(x) (1 << (x))
  54
  55 inline unsigned int set_bit_range(int start, int end)
  56 {
  57     int i;
  58     unsigned int value = 0;
  59
  60     for (i = start; i < end; ++i)
  61         value |= BIT(i);
  62     return value;
  63 }
  64
  65 inline float convert_from_vtf_format(int size, int exponent, unsigned int value)
  66 {
  67     int divider=1;
  68     int i;
  69     float sample;
  70     int mul = 1.0;
  71
  72     value = value & set_bit_range(0, size*8);
  73     if (value & BIT(size*8-1)) {
  74         value =  ((1LL << (size*8)) - value);
  75         mul = -1.0;
  76     }
  77     sample = value * 1.0;
  78     if (exponent < 0) {
  79         exponent = abs(exponent);
  80         for (i = 0; i < exponent; ++i) {
  81             divider = divider*10;
  82         }
  83         return mul * sample/divider;
  84     } else {
  85         return mul * sample * pow(10.0, exponent);
  86     }
  87 }
  88
  89 /* Platform sensor orientation */
  90 #define DEF_ORIENT_ACCEL_X                   -1
  91 #define DEF_ORIENT_ACCEL_Y                   -1
  92 #define DEF_ORIENT_ACCEL_Z                   -1
  93
  94 #define DEF_ORIENT_GYRO_X                   1
  95 #define DEF_ORIENT_GYRO_Y                   1
  96 #define DEF_ORIENT_GYRO_Z                   1
  97
  98 /* G to m/s2 */
  99 #define CONVERT_FROM_VTF16(s,d,x)      (convert_from_vtf_format(s,d,x))
 100 #define CONVERT_A_G_VTF16E14_X(s,d,x)  (DEF_ORIENT_ACCEL_X *\
 101                                         convert_from_vtf_format(s,d,x)*GRAVITY)
 102 #define CONVERT_A_G_VTF16E14_Y(s,d,x)  (DEF_ORIENT_ACCEL_Y *\
 103                                         convert_from_vtf_format(s,d,x)*GRAVITY)
 104 #define CONVERT_A_G_VTF16E14_Z(s,d,x)  (DEF_ORIENT_ACCEL_Z *\
 105                                         convert_from_vtf_format(s,d,x)*GRAVITY)
 106
 107 /* Degree/sec to radian/sec */
 108 #define CONVERT_G_D_VTF16E14_X(s,d,x)  (DEF_ORIENT_GYRO_X *\
 109                                         convert_from_vtf_format(s,d,x) * \
 110                                         M_PI/180)
 111 #define CONVERT_G_D_VTF16E14_Y(s,d,x)  (DEF_ORIENT_GYRO_Y *\
 112                                         convert_from_vtf_format(s,d,x) * \
 113                                         M_PI/180)
 114 #define CONVERT_G_D_VTF16E14_Z(s,d,x)  (DEF_ORIENT_GYRO_Z *\
 115                                         convert_from_vtf_format(s,d,x) * \
 116                                         M_PI/180)
 117
 118 /* Milli gauss to micro tesla */
 119 #define CONVERT_M_MG_VTF16E14_X(s,d,x) (convert_from_vtf_format(s,d,x)/10)
 120 #define CONVERT_M_MG_VTF16E14_Y(s,d,x) (convert_from_vtf_format(s,d,x)/10)
 121 #define CONVERT_M_MG_VTF16E14_Z(s,d,x) (convert_from_vtf_format(s,d,x)/10)
 122
 123
 124 /*----------------------------------------------------------------------------*/
 125
 126
 127 static int64_t sample_as_int64 (unsigned char* sample, datum_info_t* type)
 128 {
 129         uint64_t u64;
 130         int i;
 131         int zeroed_bits = type->storagebits - type->realbits;
 132         uint64_t sign_mask;
 133         uint64_t value_mask;
 134
 135         u64 = 0;
 136
 137         if (type->endianness == 'b')
 138                 for (i=0; i<type->storagebits/8; i++)
 139                         u64 = (u64 << 8) | sample[i];
 140         else
 141                 for (i=type->storagebits/8 - 1; i>=0; i--)
 142                         u64 = (u64 << 8) | sample[i];
 143
 144         u64 = (u64 >> type->shift) & (~0ULL >> zeroed_bits);
 145
 146         if (type->sign == 'u')
 147                 return (int64_t) u64; /* We don't handle unsigned 64 bits int */
 148
 149         /* Signed integer */
 150
 151         switch (type->realbits) {
 152                 case 0 ... 1:
 153                         return 0;
 154
 155                 case 8:
 156                         return (int64_t) (int8_t) u64;
 157
 158                 case 16:
 159                         return (int64_t) (int16_t) u64;
 160
 161                 case 32:
 162                         return (int64_t) (int32_t) u64;
 163
 164                 case 64:
 165                         return (int64_t) u64;
 166
 167                 default:
 168                         sign_mask = 1 << (type->realbits-1);
 169                         value_mask = sign_mask - 1;
 170
 171                         if (u64 & sign_mask)
 172                                 /* Negative value: return 2-complement */
 173                                 return - ((~u64 & value_mask) + 1);
 174                         else
 175                                 return (int64_t) u64; /* Positive value */
 176         }
 177 }
 178
 179
 180 static void reorder_fields (float* data, unsigned char map[MAX_CHANNELS])
 181 {
 182         int i;
 183         float temp[MAX_CHANNELS];
 184
 185         for (i=0; i<MAX_CHANNELS; i++)
 186                 temp[i] = data[map[i]];
 187
 188         for (i=0; i<MAX_CHANNELS; i++)
 189                 data[i] = temp[i];
 190 }
 191
 192
 193 static void clamp_gyro_readings_to_zero (int s, sensors_event_t* data)
 194 {
 195         float x, y, z;
 196         float near_zero;
 197
 198         x = data->data[0];
 199         y = data->data[1];
 200         z = data->data[2];
 201
 202
 203         /* If we're calibrated, don't filter out as much */
 204         if (sensor[s].cal_level > 0)
 205                 near_zero = 0.02; /* rad/s */
 206         else
 207                 near_zero = 0.1;
 208
 209         /* If motion on all axes is small enough */
 210         if (fabs(x) < near_zero && fabs(y) < near_zero && fabs(z) < near_zero) {
 211
 212                 /*
 213                  * Report that we're not moving at all... but not exactly zero
 214                  * as composite sensors (orientation, rotation vector) don't
 215                  * seem to react very well to it.
 216                  */
 217
 218                 data->data[0] *= 0.000001;
 219                 data->data[1] *= 0.000001;
 220                 data->data[2] *= 0.000001;
 221         }
 222 }
 223
 224
 225 static void process_event_gyro_uncal (int s, int i, sensors_event_t* data)
 226 {
 227         gyro_cal_t* gyro_data;
 228
 229         if (sensor[s].type == SENSOR_TYPE_GYROSCOPE) {
 230                 gyro_data = (gyro_cal_t*) sensor[s].cal_data;
 231
 232                 memcpy(&sensor[i].sample, data, sizeof(sensors_event_t));
 233
 234                 sensor[i].sample.type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
 235                 sensor[i].sample.sensor = s;
 236
 237                 sensor[i].sample.data[0] = data->data[0] + gyro_data->bias_x;
 238                 sensor[i].sample.data[1] = data->data[1] + gyro_data->bias_y;
 239                 sensor[i].sample.data[2] = data->data[2] + gyro_data->bias_z;
 240
 241                 sensor[i].sample.uncalibrated_gyro.bias[0] = gyro_data->bias_x;
 242                 sensor[i].sample.uncalibrated_gyro.bias[1] = gyro_data->bias_y;
 243                 sensor[i].sample.uncalibrated_gyro.bias[2] = gyro_data->bias_z;
 244
 245                 sensor[i].report_pending = 1;
 246         }
 247 }
 248
 249
 250 static void process_event (int s, sensors_event_t* data)
 251 {
 252         /*
 253          * This gets the real event (post process - calibration, filtering & co.)
 254          * and makes it into a virtual one.
 255          * The specific processing function for each sensor will populate the
 256          * necessary fields and set up the report pending flag.
 257          */
 258
 259          int i;
 260
 261          /* Go through out virtual sensors and check if we can use this event */
 262          for (i = 0; i < sensor_count; i++) {
 263                 switch (sensor[i].type) {
 264                         case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 265                                 process_event_gyro_uncal(s, i, data);
 266                         break;
 267
 268                         default:
 269                         break;
 270                 }
 271         }
 272 }
 273
 274
 275 static int finalize_sample_default (int s, sensors_event_t* data)
 276 {
 277         /* Swap fields if we have a custom channel ordering on this sensor */
 278         if (sensor[s].quirks & QUIRK_FIELD_ORDERING)
 279                 reorder_fields(data->data, sensor[s].order);
 280
 281         sensor[s].event_count++;
 282         switch (sensor[s].type) {
 283                 case SENSOR_TYPE_ACCELEROMETER:
 284                         /* Always consider the accelerometer accurate */
 285                         data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 286                         if (sensor[s].quirks & QUIRK_NOISY)
 287                                 denoise(s, data);
 288                         break;
 289
 290                 case SENSOR_TYPE_MAGNETIC_FIELD:
 291                         calibrate_compass (data, &sensor[s]);
 292                         if (sensor[s].quirks & QUIRK_NOISY)
 293                                 denoise(s, data);
 294                         break;
 295
 296                 case SENSOR_TYPE_GYROSCOPE:
 297
 298                         /*
 299                          * Report medium accuracy by default ; higher accuracy
 300                          * levels will be reported once, and if, we achieve
 301                          * calibration.
 302                          */
 303                         data->gyro.status = SENSOR_STATUS_ACCURACY_MEDIUM;
 304
 305                         /*
 306                          * We're only trying to calibrate data from continuously
 307                          * firing gyroscope drivers, as motion based ones use
 308                          * movement thresholds that may lead us to incorrectly
 309                          * estimate bias.
 310                          */
 311                         if (sensor[s].selected_trigger !=
 312                                 sensor[s].motion_trigger_name)
 313                                         calibrate_gyro(data, &sensor[s]);
 314
 315                         /*
 316                          * For noisy sensors drop a few samples to make sure we
 317                          * have at least GYRO_MIN_SAMPLES events in the
 318                          * filtering queue. This improves mean and std dev.
 319                          */
 320                         if (sensor[s].quirks & QUIRK_NOISY) {
 321                                 if (sensor[s].selected_trigger !=
 322                                     sensor[s].motion_trigger_name &&
 323                                     sensor[s].event_count<GYRO_MIN_SAMPLES)
 324                                                 return 0;
 325
 326                                 denoise(s, data);
 327                         }
 328
 329                         /* Clamp near zero moves to (0,0,0) if appropriate */
 330                         clamp_gyro_readings_to_zero(s, data);
 331                         break;
 332
 333                 case SENSOR_TYPE_LIGHT:
 334                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:
 335                 case SENSOR_TYPE_TEMPERATURE:
 336                         /* Only keep two decimals for these readings */
 337                         data->data[0] = 0.01 * ((int) (data->data[0] * 100));
 338
 339                         /* ... fall through ... */
 340
 341                 case SENSOR_TYPE_PROXIMITY:
 342                         /*
 343                          * These are on change sensors ; drop the sample if it
 344                          * has the same value as the previously reported one.
 345                          */
 346                         if (data->data[0] == sensor[s].prev_val)
 347                                 return 0;
 348
 349                         sensor[s].prev_val = data->data[0];
 350                         break;
 351         }
 352         /* If there are active virtual sensors depending on this one - process the event */
 353         if (sensor[s].ref_count)
 354                 process_event(s, data);
 355         /* We will drop samples if the sensor is not directly enabled */
 356         if (!sensor[s].directly_enabled)
 357                 return 0;
 358
 359         return 1; /* Return sample to Android */
 360 }
 361
 362
 363 static float transform_sample_default(int s, int c, unsigned char* sample_data)
 364 {
 365         datum_info_t* sample_type = &sensor[s].channel[c].type_info;
 366         int64_t s64 = sample_as_int64(sample_data, sample_type);
 367         float scale = sensor[s].scale ?
 368                         sensor[s].scale : sensor[s].channel[c].scale;
 369
 370         /* In case correction has been requested using properties, apply it */
 371         scale *= sensor[s].channel[c].opt_scale;
 372
 373         /* Apply default scaling rules */
 374         return (sensor[s].offset + s64) * scale;
 375 }
 376
 377
 378 static int finalize_sample_ISH (int s, sensors_event_t* data)
 379 {
 380         float pitch, roll, yaw;
 381
 382         /* Swap fields if we have a custom channel ordering on this sensor */
 383         if (sensor[s].quirks & QUIRK_FIELD_ORDERING)
 384                 reorder_fields(data->data, sensor[s].order);
 385
 386         if (sensor[s].type == SENSOR_TYPE_ORIENTATION) {
 387
 388                 pitch = data->data[0];
 389                 roll = data->data[1];
 390                 yaw = data->data[2];
 391
 392                 data->data[0] = 360.0 - yaw;
 393                 data->data[1] = -pitch;
 394                 data->data[2] = -roll;
 395         }
 396
 397         /* Add this event to our global records, for filtering purposes */
 398         record_sample(s, data);
 399
 400         return 1; /* Return sample to Android */
 401 }
 402
 403
 404 static float transform_sample_ISH (int s, int c, unsigned char* sample_data)
 405 {
 406         datum_info_t* sample_type = &sensor[s].channel[c].type_info;
 407         int val         = (int) sample_as_int64(sample_data, sample_type);
 408         float correction;
 409         int data_bytes  = (sample_type->realbits)/8;
 410         int exponent    = sensor[s].offset;
 411
 412         /* In case correction has been requested using properties, apply it */
 413         correction = sensor[s].channel[c].opt_scale;
 414
 415         switch (sensor[s].type) {
 416                 case SENSOR_TYPE_ACCELEROMETER:
 417                         switch (c) {
 418                                 case 0:
 419                                         return  correction *
 420                                                 CONVERT_A_G_VTF16E14_X(
 421                                                 data_bytes, exponent, val);
 422
 423                                 case 1:
 424                                         return  correction *
 425                                                 CONVERT_A_G_VTF16E14_Y(
 426                                                 data_bytes, exponent, val);
 427
 428                                 case 2:
 429                                         return  correction *
 430                                                 CONVERT_A_G_VTF16E14_Z(
 431                                                 data_bytes, exponent, val);
 432                         }
 433                         break;
 434
 435
 436                 case SENSOR_TYPE_GYROSCOPE:
 437                         switch (c) {
 438                                 case 0:
 439                                         return  correction *
 440                                                 CONVERT_G_D_VTF16E14_X(
 441                                                 data_bytes, exponent, val);
 442
 443                                 case 1:
 444                                         return  correction *
 445                                                 CONVERT_G_D_VTF16E14_Y(
 446                                                 data_bytes, exponent, val);
 447
 448                                 case 2:
 449                                         return  correction *
 450                                                 CONVERT_G_D_VTF16E14_Z(
 451                                                 data_bytes, exponent, val);
 452                         }
 453                         break;
 454
 455                 case SENSOR_TYPE_MAGNETIC_FIELD:
 456                         switch (c) {
 457                                 case 0:
 458                                         return  correction *
 459                                                 CONVERT_M_MG_VTF16E14_X(
 460                                                 data_bytes, exponent, val);
 461
 462                                 case 1:
 463                                         return  correction *
 464                                                 CONVERT_M_MG_VTF16E14_Y(
 465                                                 data_bytes, exponent, val);
 466
 467                                 case 2:
 468                                         return  correction *
 469                                                 CONVERT_M_MG_VTF16E14_Z(
 470                                                 data_bytes, exponent, val);
 471                         }
 472                         break;
 473
 474                 case SENSOR_TYPE_LIGHT:
 475                                 return (float) val;
 476
 477                 case SENSOR_TYPE_ORIENTATION:
 478                         return  correction * convert_from_vtf_format(
 479                                                 data_bytes, exponent, val);
 480
 481                 case SENSOR_TYPE_ROTATION_VECTOR:
 482                         return  correction * convert_from_vtf_format(
 483                                                 data_bytes, exponent, val);
 484         }
 485
 486         return 0;
 487 }
 488
 489
 490 void select_transform (int s)
 491 {
 492         char prop_name[PROP_NAME_MAX];
 493         char prop_val[PROP_VALUE_MAX];
 494         int i                   = sensor[s].catalog_index;
 495         const char *prefix      = sensor_catalog[i].tag;
 496
 497         sprintf(prop_name, PROP_BASE, prefix, "transform");
 498
 499         if (property_get(prop_name, prop_val, "")) {
 500                 if (!strcmp(prop_val, "ISH")) {
 501                         ALOGI(  "Using Intel Sensor Hub semantics on %s\n",
 502                                 sensor[s].friendly_name);
 503
 504                         sensor[s].ops.transform = transform_sample_ISH;
 505                         sensor[s].ops.finalize = finalize_sample_ISH;
 506                         return;
 507                 }
 508         }
 509
 510         sensor[s].ops.transform = transform_sample_default;
 511         sensor[s].ops.finalize = finalize_sample_default;
 512 }
 513
 514
 515 float acquire_immediate_value(int s, int c)
 516 {
 517         char sysfs_path[PATH_MAX];
 518         float val;
 519         int ret;
 520         int dev_num = sensor[s].dev_num;
 521         int i = sensor[s].catalog_index;
 522         const char* raw_path = sensor_catalog[i].channel[c].raw_path;
 523         const char* input_path = sensor_catalog[i].channel[c].input_path;
 524         float scale = sensor[s].scale ?
 525                         sensor[s].scale : sensor[s].channel[c].scale;
 526         float offset = sensor[s].offset;
 527         int sensor_type = sensor_catalog[i].type;
 528         float correction;
 529
 530         /* In case correction has been requested using properties, apply it */
 531         correction = sensor[s].channel[c].opt_scale;
 532
 533         /* Acquire a sample value for sensor s / channel c through sysfs */
 534
 535         if (input_path[0]) {
 536                 sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path);
 537                 ret = sysfs_read_float(sysfs_path, &val);
 538
 539                 if (!ret) {
 540                         return val * correction;
 541                 }
 542         };
 543
 544         if (!raw_path[0])
 545                 return 0;
 546
 547         sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path);
 548         ret = sysfs_read_float(sysfs_path, &val);
 549
 550         if (ret == -1)
 551                 return 0;
 552
 553         /*
 554          * There is no transform ops defined yet for raw sysfs values.
 555          * Use this function to perform transformation as well.
 556          */
 557         if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
 558                 return  CONVERT_GAUSS_TO_MICROTESLA ((val + offset) * scale) *
 559                         correction;
 560
 561         return (val + offset) * scale * correction;
 562 }