frameworks/base/services/sensorservice/RotationVectorSensor.cpp

   1 /*
   2  * Copyright (C) 2010 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
  17 #include <stdint.h>
  18 #include <math.h>
  19 #include <sys/types.h>
  20
  21 #include <utils/Errors.h>
  22
  23 #include <hardware/sensors.h>
  24
  25 #include "RotationVectorSensor.h"
  26
  27 namespace android {
  28 // ---------------------------------------------------------------------------
  29
  30 template <typename T>
  31 static inline T clamp(T v) {
  32     return v < 0 ? 0 : v;
  33 }
  34
  35 RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count)
  36     : mSensorDevice(SensorDevice::getInstance()),
  37       mALowPass(M_SQRT1_2, 1.5f),
  38       mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass),
  39       mMLowPass(M_SQRT1_2, 1.5f),
  40       mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass)
  41 {
  42     for (size_t i=0 ; i<count ; i++) {
  43         if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
  44             mAcc = Sensor(list + i);
  45         }
  46         if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
  47             mMag = Sensor(list + i);
  48         }
  49     }
  50     memset(mMagData, 0, sizeof(mMagData));
  51 }
  52
  53 bool RotationVectorSensor::process(sensors_event_t* outEvent,
  54         const sensors_event_t& event)
  55 {
  56     const static double NS2S = 1.0 / 1000000000.0;
  57     if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
  58         const double now = event.timestamp * NS2S;
  59         if (mMagTime == 0) {
  60             mMagData[0] = mMX.init(event.magnetic.x);
  61             mMagData[1] = mMY.init(event.magnetic.y);
  62             mMagData[2] = mMZ.init(event.magnetic.z);
  63         } else {
  64             double dT = now - mMagTime;
  65             mMLowPass.setSamplingPeriod(dT);
  66             mMagData[0] = mMX(event.magnetic.x);
  67             mMagData[1] = mMY(event.magnetic.y);
  68             mMagData[2] = mMZ(event.magnetic.z);
  69         }
  70         mMagTime = now;
  71     }
  72     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
  73         const double now = event.timestamp * NS2S;
  74         float Ax, Ay, Az;
  75         if (mAccTime == 0) {
  76             Ax = mAX.init(event.acceleration.x);
  77             Ay = mAY.init(event.acceleration.y);
  78             Az = mAZ.init(event.acceleration.z);
  79         } else {
  80             double dT = now - mAccTime;
  81             mALowPass.setSamplingPeriod(dT);
  82             Ax = mAX(event.acceleration.x);
  83             Ay = mAY(event.acceleration.y);
  84             Az = mAZ(event.acceleration.z);
  85         }
  86         mAccTime = now;
  87         const float Ex = mMagData[0];
  88         const float Ey = mMagData[1];
  89         const float Ez = mMagData[2];
  90         float Hx = Ey*Az - Ez*Ay;
  91         float Hy = Ez*Ax - Ex*Az;
  92         float Hz = Ex*Ay - Ey*Ax;
  93         const float normH = sqrtf(Hx*Hx + Hy*Hy + Hz*Hz);
  94         if (normH < 0.1f) {
  95             // device is close to free fall (or in space?), or close to
  96             // magnetic north pole. Typical values are  > 100.
  97             return false;
  98         }
  99         const float invH = 1.0f / normH;
 100         const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az);
 101         Hx *= invH;
 102         Hy *= invH;
 103         Hz *= invH;
 104         Ax *= invA;
 105         Ay *= invA;
 106         Az *= invA;
 107         const float Mx = Ay*Hz - Az*Hy;
 108         const float My = Az*Hx - Ax*Hz;
 109         const float Mz = Ax*Hy - Ay*Hx;
 110
 111         // matrix to rotation vector (normalized quaternion)
 112         float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
 113         float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
 114         float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
 115         float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
 116         qx = copysignf(qx, Ay - Mz);
 117         qy = copysignf(qy, Hz - Ax);
 118         qz = copysignf(qz, Mx - Hy);
 119
 120         // this quaternion is guaranteed to be normalized, by construction
 121         // of the rotation matrix.
 122
 123         *outEvent = event;
 124         outEvent->data[0] = qx;
 125         outEvent->data[1] = qy;
 126         outEvent->data[2] = qz;
 127         outEvent->data[3] = qw;
 128         outEvent->sensor = '_rov';
 129         outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
 130         return true;
 131     }
 132     return false;
 133 }
 134
 135 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
 136     mSensorDevice.activate(this, mAcc.getHandle(), enabled);
 137     mSensorDevice.activate(this, mMag.getHandle(), enabled);
 138     if (enabled) {
 139         mMagTime = 0;
 140         mAccTime = 0;
 141     }
 142     return NO_ERROR;
 143 }
 144
 145 status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns)
 146 {
 147     mSensorDevice.setDelay(this, mAcc.getHandle(), ns);
 148     mSensorDevice.setDelay(this, mMag.getHandle(), ns);
 149     return NO_ERROR;
 150 }
 151
 152 Sensor RotationVectorSensor::getSensor() const {
 153     sensor_t hwSensor;
 154     hwSensor.name       = "Rotation Vector Sensor";
 155     hwSensor.vendor     = "Google Inc.";
 156     hwSensor.version    = 1;
 157     hwSensor.handle     = '_rov';
 158     hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
 159     hwSensor.maxRange   = 1;
 160     hwSensor.resolution = 1.0f / (1<<24);
 161     hwSensor.power      = mAcc.getPowerUsage() + mMag.getPowerUsage();
 162     hwSensor.minDelay   = mAcc.getMinDelay();
 163     Sensor sensor(&hwSensor);
 164     return sensor;
 165 }
 166
 167 // ---------------------------------------------------------------------------
 168 }; // namespace android
 169