Home | History | Annotate | Download | only in sensorservice 
      1 /*
      2  * Copyright (C) 2011 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 "SensorDevice.h"
     18 #include "SensorFusion.h"
     19 #include "SensorService.h"
     20 
     21 namespace android {
     22 // ---------------------------------------------------------------------------
     23 
     24 ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
     25 
     26 SensorFusion::SensorFusion()
     27     : mSensorDevice(SensorDevice::getInstance()),
     28       mEnabled(false), mGyroTime(0)
     29 {
     30     sensor_t const* list;
     31     ssize_t count = mSensorDevice.getSensorList(&list);
     32     if (count > 0) {
     33         for (size_t i=0 ; i<size_t(count) ; i++) {
     34             if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
     35                 mAcc = Sensor(list + i);
     36             }
     37             if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
     38                 mMag = Sensor(list + i);
     39             }
     40             if (list[i].type == SENSOR_TYPE_GYROSCOPE) {
     41                 mGyro = Sensor(list + i);
     42                 // 200 Hz for gyro events is a good compromise between precision
     43                 // and power/cpu usage.
     44                 mGyroRate = 200;
     45                 mTargetDelayNs = 1000000000LL/mGyroRate;
     46             }
     47         }
     48         mFusion.init();
     49     }
     50 }
     51 
     52 void SensorFusion::process(const sensors_event_t& event) {
     53     if (event.type == SENSOR_TYPE_GYROSCOPE) {
     54         if (mGyroTime != 0) {
     55             const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
     56             const float freq = 1 / dT;
     57             if (freq >= 100 && freq<1000) { // filter values obviously wrong
     58                 const float alpha = 1 / (1 + dT); // 1s time-constant
     59                 mGyroRate = freq + (mGyroRate - freq)*alpha;
     60             }
     61         }
     62         mGyroTime = event.timestamp;
     63         mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);
     64     } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
     65         const vec3_t mag(event.data);
     66         mFusion.handleMag(mag);
     67     } else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
     68         const vec3_t acc(event.data);
     69         mFusion.handleAcc(acc);
     70         mAttitude = mFusion.getAttitude();
     71     }
     72 }
     73 
     74 template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
     75 template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
     76 
     77 status_t SensorFusion::activate(void* ident, bool enabled) {
     78 
     79     ALOGD_IF(DEBUG_CONNECTIONS,
     80             "SensorFusion::activate(ident=%p, enabled=%d)",
     81             ident, enabled);
     82 
     83     const ssize_t idx = mClients.indexOf(ident);
     84     if (enabled) {
     85         if (idx < 0) {
     86             mClients.add(ident);
     87         }
     88     } else {
     89         if (idx >= 0) {
     90             mClients.removeItemsAt(idx);
     91         }
     92     }
     93 
     94     mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
     95     mSensorDevice.activate(ident, mMag.getHandle(), enabled);
     96     mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
     97 
     98     const bool newState = mClients.size() != 0;
     99     if (newState != mEnabled) {
    100         mEnabled = newState;
    101         if (newState) {
    102             mFusion.init();
    103             mGyroTime = 0;
    104         }
    105     }
    106     return NO_ERROR;
    107 }
    108 
    109 status_t SensorFusion::setDelay(void* ident, int64_t ns) {
    110     mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
    111     mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
    112     mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs);
    113     return NO_ERROR;
    114 }
    115 
    116 
    117 float SensorFusion::getPowerUsage() const {
    118     float power =   mAcc.getPowerUsage() +
    119                     mMag.getPowerUsage() +
    120                     mGyro.getPowerUsage();
    121     return power;
    122 }
    123 
    124 int32_t SensorFusion::getMinDelay() const {
    125     return mAcc.getMinDelay();
    126 }
    127 
    128 void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
    129     const Fusion& fusion(mFusion);
    130     snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, "
    131             "q=< %g, %g, %g, %g > (%g), "
    132             "b=< %g, %g, %g >\n",
    133             mEnabled ? "enabled" : "disabled",
    134             mClients.size(),
    135             mGyroRate,
    136             fusion.getAttitude().x,
    137             fusion.getAttitude().y,
    138             fusion.getAttitude().z,
    139             fusion.getAttitude().w,
    140             length(fusion.getAttitude()),
    141             fusion.getBias().x,
    142             fusion.getBias().y,
    143             fusion.getBias().z);
    144     result.append(buffer);
    145 }
    146 
    147 // ---------------------------------------------------------------------------
    148 }; // namespace android
    149