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 RotationVectorSensor::RotationVectorSensor()
  31     : mSensorDevice(SensorDevice::getInstance()),
  32       mSensorFusion(SensorFusion::getInstance())
  33 {
  34 }
  35
  36 bool RotationVectorSensor::process(sensors_event_t* outEvent,
  37         const sensors_event_t& event)
  38 {
  39     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
  40         if (mSensorFusion.hasEstimate()) {
  41             const vec4_t q(mSensorFusion.getAttitude());
  42             *outEvent = event;
  43             outEvent->data[0] = q.x;
  44             outEvent->data[1] = q.y;
  45             outEvent->data[2] = q.z;
  46             outEvent->data[3] = q.w;
  47             outEvent->sensor = '_rov';
  48             outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
  49             return true;
  50         }
  51     }
  52     return false;
  53 }
  54
  55 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
  56     return mSensorFusion.activate(ident, enabled);
  57 }
  58
  59 status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
  60     return mSensorFusion.setDelay(ident, ns);
  61 }
  62
  63 Sensor RotationVectorSensor::getSensor() const {
  64     sensor_t hwSensor;
  65     hwSensor.name       = "Rotation Vector Sensor";
  66     hwSensor.vendor     = "AOSP";
  67     hwSensor.version    = 3;
  68     hwSensor.handle     = '_rov';
  69     hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
  70     hwSensor.maxRange   = 1;
  71     hwSensor.resolution = 1.0f / (1<<24);
  72     hwSensor.power      = mSensorFusion.getPowerUsage();
  73     hwSensor.minDelay   = mSensorFusion.getMinDelay();
  74     Sensor sensor(&hwSensor);
  75     return sensor;
  76 }
  77
  78 // ---------------------------------------------------------------------------
  79
  80 GyroDriftSensor::GyroDriftSensor()
  81     : mSensorDevice(SensorDevice::getInstance()),
  82       mSensorFusion(SensorFusion::getInstance())
  83 {
  84 }
  85
  86 bool GyroDriftSensor::process(sensors_event_t* outEvent,
  87         const sensors_event_t& event)
  88 {
  89     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
  90         if (mSensorFusion.hasEstimate()) {
  91             const vec3_t b(mSensorFusion.getGyroBias());
  92             *outEvent = event;
  93             outEvent->data[0] = b.x;
  94             outEvent->data[1] = b.y;
  95             outEvent->data[2] = b.z;
  96             outEvent->sensor = '_gbs';
  97             outEvent->type = SENSOR_TYPE_ACCELEROMETER;
  98             return true;
  99         }
 100     }
 101     return false;
 102 }
 103
 104 status_t GyroDriftSensor::activate(void* ident, bool enabled) {
 105     return mSensorFusion.activate(ident, enabled);
 106 }
 107
 108 status_t GyroDriftSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
 109     return mSensorFusion.setDelay(ident, ns);
 110 }
 111
 112 Sensor GyroDriftSensor::getSensor() const {
 113     sensor_t hwSensor;
 114     hwSensor.name       = "Gyroscope Bias (debug)";
 115     hwSensor.vendor     = "AOSP";
 116     hwSensor.version    = 1;
 117     hwSensor.handle     = '_gbs';
 118     hwSensor.type       = SENSOR_TYPE_ACCELEROMETER;
 119     hwSensor.maxRange   = 1;
 120     hwSensor.resolution = 1.0f / (1<<24);
 121     hwSensor.power      = mSensorFusion.getPowerUsage();
 122     hwSensor.minDelay   = mSensorFusion.getMinDelay();
 123     Sensor sensor(&hwSensor);
 124     return sensor;
 125 }
 126
 127 // ---------------------------------------------------------------------------
 128 }; // namespace android
 129