RotVecSensor.cpp

   1 /*
   2  * Copyright (C) 2013 The Android Open Source Project
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16 #include <cutils/log.h>
  17 #include "common.h"
  18 #include "SensorConfig.h"
  19 #include "SynthCompassSensor.h"
  20 #include "RotVecSensor.h"
  21
  22 const struct sensor_t RotVecSensor::sSensorInfo_rotvec = {
  23     .name       = "HID_SENSOR Rotation Vector",
  24     .vendor     = "Intel",
  25     .version    = 1,
  26     .handle     = SENSORS_ROT_VEC_HANDLE,
  27     .type       = SENSOR_TYPE_ROTATION_VECTOR,
  28     .maxRange   = 1.0,
  29     .resolution = 1./512, // Advertise as 9 bits, no idea about reality
  30     .power      = 0.1f,
  31     .minDelay   = 0,
  32     .fifoReservedEventCount = 0,
  33     .fifoMaxEventCount      = 0,
  34     .reserved   = {},
  35 };
  36
  37 RotVecSensor::RotVecSensor()
  38     : SensorIIODev("magn_3d",       // name
  39                    "in_magn_scale", // units sysfs node
  40                    "in_magn_offset",// exponent sysfs node
  41                    "in_rot_",       // channel_prefix
  42                    10)              // retry count
  43 {
  44     mPendingEvent.version = sizeof(sensors_event_t);
  45     mPendingEvent.sensor = ID_R;
  46     mPendingEvent.type = SENSOR_TYPE_ROTATION_VECTOR;
  47     memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data));
  48
  49     sample_delay_min_ms = 50; // 20Hz default
  50
  51     mSynthCompass = NULL;
  52 }
  53
  54 int RotVecSensor::processEvent(unsigned char *data, size_t len)
  55 {
  56     if (IsDeviceInitialized() == false) {
  57         ALOGE("Device was not initialized \n");
  58         return -1;
  59     }
  60
  61     // The Intel Sensor Hub emits a normalized x/y/z/w quaternion
  62     // which acts to rotate points in the device coordinate system
  63     // (left/up/out) to the world coordinate system (north/east/down).
  64     // This is pleasingly identical to the Android convention, so just
  65     // copy out the raw data.
  66
  67     unsigned int *sample = (unsigned int*)data;
  68     float sc = GetScaleValue();
  69     for (int i=0; i < (len / sizeof(*sample)); i++) {
  70         int sz = GetChannelBytesUsedSize(i);
  71         mPendingEvent.data[i] = convert_from_vtf_format(sz, sc, sample[i]);
  72     }
  73
  74     if (mSynthCompass)
  75         mSynthCompass->setQuaternion(mPendingEvent.data);
  76
  77     return 0;
  78 }