services/sensorservice/SensorFusion.cpp

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