Home | History | Annotate | Download | only in sensorservice 
      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(int mode) :
     31       mMode(mode) {
     32     const sensor_t sensor = {
     33         .name       = getSensorName(),
     34         .vendor     = "AOSP",
     35         .version    = 3,
     36         .handle     = getSensorToken(),
     37         .type       = getSensorType(),
     38         .maxRange   = 1,
     39         .resolution = 1.0f / (1<<24),
     40         .power      = mSensorFusion.getPowerUsage(),
     41         .minDelay   = mSensorFusion.getMinDelay(),
     42     };
     43     mSensor = Sensor(&sensor);
     44 }
     45 
     46 bool RotationVectorSensor::process(sensors_event_t* outEvent,
     47         const sensors_event_t& event)
     48 {
     49     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
     50         if (mSensorFusion.hasEstimate(mMode)) {
     51             const vec4_t q(mSensorFusion.getAttitude(mMode));
     52             *outEvent = event;
     53             outEvent->data[0] = q.x;
     54             outEvent->data[1] = q.y;
     55             outEvent->data[2] = q.z;
     56             outEvent->data[3] = q.w;
     57             outEvent->sensor = getSensorToken();
     58             outEvent->type = getSensorType();
     59             return true;
     60         }
     61     }
     62     return false;
     63 }
     64 
     65 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
     66     return mSensorFusion.activate(mMode, ident, enabled);
     67 }
     68 
     69 status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
     70     return mSensorFusion.setDelay(mMode, ident, ns);
     71 }
     72 
     73 int RotationVectorSensor::getSensorType() const {
     74     switch(mMode) {
     75         case FUSION_9AXIS:
     76             return SENSOR_TYPE_ROTATION_VECTOR;
     77         case FUSION_NOMAG:
     78             return SENSOR_TYPE_GAME_ROTATION_VECTOR;
     79         case FUSION_NOGYRO:
     80             return SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
     81         default:
     82             assert(0);
     83             return 0;
     84     }
     85 }
     86 
     87 const char* RotationVectorSensor::getSensorName() const {
     88     switch(mMode) {
     89         case FUSION_9AXIS:
     90             return "Rotation Vector Sensor";
     91         case FUSION_NOMAG:
     92             return "Game Rotation Vector Sensor";
     93         case FUSION_NOGYRO:
     94             return "GeoMag Rotation Vector Sensor";
     95         default:
     96             assert(0);
     97             return NULL;
     98     }
     99 }
    100 
    101 int RotationVectorSensor::getSensorToken() const {
    102     switch(mMode) {
    103         case FUSION_9AXIS:
    104             return '_rov';
    105         case FUSION_NOMAG:
    106             return '_gar';
    107         case FUSION_NOGYRO:
    108             return '_geo';
    109         default:
    110             assert(0);
    111             return 0;
    112     }
    113 }
    114 
    115 // ---------------------------------------------------------------------------
    116 
    117 GyroDriftSensor::GyroDriftSensor() {
    118     const sensor_t sensor = {
    119         .name       = "Gyroscope Bias (debug)",
    120         .vendor     = "AOSP",
    121         .version    = 1,
    122         .handle     = '_gbs',
    123         .type       = SENSOR_TYPE_ACCELEROMETER,
    124         .maxRange   = 1,
    125         .resolution = 1.0f / (1<<24),
    126         .power      = mSensorFusion.getPowerUsage(),
    127         .minDelay   = mSensorFusion.getMinDelay(),
    128     };
    129     mSensor = Sensor(&sensor);
    130 }
    131 
    132 bool GyroDriftSensor::process(sensors_event_t* outEvent,
    133         const sensors_event_t& event)
    134 {
    135     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
    136         if (mSensorFusion.hasEstimate()) {
    137             const vec3_t b(mSensorFusion.getGyroBias());
    138             *outEvent = event;
    139             outEvent->data[0] = b.x;
    140             outEvent->data[1] = b.y;
    141             outEvent->data[2] = b.z;
    142             outEvent->sensor = '_gbs';
    143             outEvent->type = SENSOR_TYPE_ACCELEROMETER;
    144             return true;
    145         }
    146     }
    147     return false;
    148 }
    149 
    150 status_t GyroDriftSensor::activate(void* ident, bool enabled) {
    151     return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
    152 }
    153 
    154 status_t GyroDriftSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
    155     return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
    156 }
    157 
    158 // ---------------------------------------------------------------------------
    159 }; // namespace android
    160 
    161