1f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian/* 2f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * Copyright (C) 2010 The Android Open Source Project 3f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * 4f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 5f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * you may not use this file except in compliance with the License. 6f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * You may obtain a copy of the License at 7f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * 8f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 9f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * 10f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * Unless required by applicable law or agreed to in writing, software 11f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 12f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * See the License for the specific language governing permissions and 14f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian * limitations under the License. 15f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian */ 16f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 17f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian#include <stdint.h> 18f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian#include <math.h> 19f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian#include <sys/types.h> 20f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 21f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian#include <utils/Errors.h> 22f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 23f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian#include <hardware/sensors.h> 24f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 25f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian#include "GravitySensor.h" 26984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorDevice.h" 27984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorFusion.h" 28f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 29f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopiannamespace android { 30f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian// --------------------------------------------------------------------------- 31f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 32f001c92436b4a66eb7687286325ced7f10c9f917Mathias AgopianGravitySensor::GravitySensor(sensor_t const* list, size_t count) 33f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian : mSensorDevice(SensorDevice::getInstance()), 343301542828febc768e1df42892cfac4992c35474Mathias Agopian mSensorFusion(SensorFusion::getInstance()) 35f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian{ 36f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian for (size_t i=0 ; i<count ; i++) { 37f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 38f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian mAccelerometer = Sensor(list + i); 39f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian break; 40f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 41f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 42f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 43f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 44f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopianbool GravitySensor::process(sensors_event_t* outEvent, 45f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian const sensors_event_t& event) 46f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian{ 47f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian const static double NS2S = 1.0 / 1000000000.0; 48f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian if (event.type == SENSOR_TYPE_ACCELEROMETER) { 49984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t g; 503301542828febc768e1df42892cfac4992c35474Mathias Agopian if (!mSensorFusion.hasEstimate()) 513301542828febc768e1df42892cfac4992c35474Mathias Agopian return false; 523301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t R(mSensorFusion.getRotationMatrix()); 533301542828febc768e1df42892cfac4992c35474Mathias Agopian // FIXME: we need to estimate the length of gravity because 543301542828febc768e1df42892cfac4992c35474Mathias Agopian // the accelerometer may have a small scaling error. This 553301542828febc768e1df42892cfac4992c35474Mathias Agopian // translates to an offset in the linear-acceleration sensor. 563301542828febc768e1df42892cfac4992c35474Mathias Agopian g = R[2] * GRAVITY_EARTH; 573301542828febc768e1df42892cfac4992c35474Mathias Agopian 58f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian *outEvent = event; 59984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[0] = g.x; 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[1] = g.y; 61984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[2] = g.z; 62f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian outEvent->sensor = '_grv'; 63f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian outEvent->type = SENSOR_TYPE_GRAVITY; 64f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return true; 65f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 66f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return false; 67f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 69f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopianstatus_t GravitySensor::activate(void* ident, bool enabled) { 70bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella return mSensorFusion.activate(ident, enabled); 71f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 72f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 733301542828febc768e1df42892cfac4992c35474Mathias Agopianstatus_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) { 74bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella return mSensorFusion.setDelay(ident, ns); 75f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 76f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 77f001c92436b4a66eb7687286325ced7f10c9f917Mathias AgopianSensor GravitySensor::getSensor() const { 78f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian sensor_t hwSensor; 79f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.name = "Gravity Sensor"; 800319306670b0344da99efa606b6f172dde575a39Mathias Agopian hwSensor.vendor = "AOSP"; 813301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.version = 3; 82f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.handle = '_grv'; 83f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.type = SENSOR_TYPE_GRAVITY; 84984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.maxRange = GRAVITY_EARTH * 2; 85f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.resolution = mAccelerometer.getResolution(); 863301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.power = mSensorFusion.getPowerUsage(); 873301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.minDelay = mSensorFusion.getMinDelay(); 88f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian Sensor sensor(&hwSensor); 89f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return sensor; 90f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 91f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 92f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian// --------------------------------------------------------------------------- 93f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian}; // namespace android 94f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 95