GravitySensor.cpp revision 984826cc158193e61e3a00359ef4f6699c7d748a
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 "GravitySensor.h" 26#include "SensorDevice.h" 27#include "SensorFusion.h" 28 29namespace android { 30// --------------------------------------------------------------------------- 31 32GravitySensor::GravitySensor(sensor_t const* list, size_t count) 33 : mSensorDevice(SensorDevice::getInstance()), 34 mSensorFusion(SensorFusion::getInstance()), 35 mAccTime(0), 36 mLowPass(M_SQRT1_2, 1.5f), 37 mX(mLowPass), mY(mLowPass), mZ(mLowPass) 38{ 39 for (size_t i=0 ; i<count ; i++) { 40 if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 41 mAccelerometer = Sensor(list + i); 42 break; 43 } 44 } 45} 46 47bool GravitySensor::process(sensors_event_t* outEvent, 48 const sensors_event_t& event) 49{ 50 const static double NS2S = 1.0 / 1000000000.0; 51 if (event.type == SENSOR_TYPE_ACCELEROMETER) { 52 vec3_t g; 53 if (mSensorFusion.hasGyro()) { 54 if (!mSensorFusion.hasEstimate()) 55 return false; 56 const mat33_t R(mSensorFusion.getRotationMatrix()); 57 // FIXME: we need to estimate the length of gravity because 58 // the accelerometer may have a small scaling error. This 59 // translates to an offset in the linear-acceleration sensor. 60 g = R[2] * GRAVITY_EARTH; 61 } else { 62 const double now = event.timestamp * NS2S; 63 if (mAccTime == 0) { 64 g.x = mX.init(event.acceleration.x); 65 g.y = mY.init(event.acceleration.y); 66 g.z = mZ.init(event.acceleration.z); 67 } else { 68 double dT = now - mAccTime; 69 mLowPass.setSamplingPeriod(dT); 70 g.x = mX(event.acceleration.x); 71 g.y = mY(event.acceleration.y); 72 g.z = mZ(event.acceleration.z); 73 } 74 g *= (GRAVITY_EARTH / length(g)); 75 mAccTime = now; 76 } 77 *outEvent = event; 78 outEvent->data[0] = g.x; 79 outEvent->data[1] = g.y; 80 outEvent->data[2] = g.z; 81 outEvent->sensor = '_grv'; 82 outEvent->type = SENSOR_TYPE_GRAVITY; 83 return true; 84 } 85 return false; 86} 87 88status_t GravitySensor::activate(void* ident, bool enabled) { 89 status_t err; 90 if (mSensorFusion.hasGyro()) { 91 err = mSensorFusion.activate(this, enabled); 92 } else { 93 err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); 94 if (err == NO_ERROR) { 95 if (enabled) { 96 mAccTime = 0; 97 } 98 } 99 } 100 return err; 101} 102 103status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) 104{ 105 if (mSensorFusion.hasGyro()) { 106 return mSensorFusion.setDelay(this, ns); 107 } else { 108 return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); 109 } 110} 111 112Sensor GravitySensor::getSensor() const { 113 sensor_t hwSensor; 114 hwSensor.name = "Gravity Sensor"; 115 hwSensor.vendor = "Google Inc."; 116 hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; 117 hwSensor.handle = '_grv'; 118 hwSensor.type = SENSOR_TYPE_GRAVITY; 119 hwSensor.maxRange = GRAVITY_EARTH * 2; 120 hwSensor.resolution = mAccelerometer.getResolution(); 121 hwSensor.power = mSensorFusion.hasGyro() ? 122 mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage(); 123 hwSensor.minDelay = mSensorFusion.hasGyro() ? 124 mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay(); 125 Sensor sensor(&hwSensor); 126 return sensor; 127} 128 129// --------------------------------------------------------------------------- 130}; // namespace android 131 132