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 32755c451c7861a029e26e5f16e319b629169e656dPeng XuGravitySensor::GravitySensor(sensor_t const* list, size_t count) { 33f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian for (size_t i=0 ; i<count ; i++) { 34f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 35f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian mAccelerometer = Sensor(list + i); 36f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian break; 37f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 38f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 390cc8f809924706c7d683da30605f432635dd5bb6Peng Xu 40755c451c7861a029e26e5f16e319b629169e656dPeng Xu const sensor_t sensor = { 41755c451c7861a029e26e5f16e319b629169e656dPeng Xu .name = "Gravity Sensor", 42755c451c7861a029e26e5f16e319b629169e656dPeng Xu .vendor = "AOSP", 43755c451c7861a029e26e5f16e319b629169e656dPeng Xu .version = 3, 44755c451c7861a029e26e5f16e319b629169e656dPeng Xu .handle = '_grv', 45755c451c7861a029e26e5f16e319b629169e656dPeng Xu .type = SENSOR_TYPE_GRAVITY, 46755c451c7861a029e26e5f16e319b629169e656dPeng Xu .maxRange = GRAVITY_EARTH * 2, 47755c451c7861a029e26e5f16e319b629169e656dPeng Xu .resolution = mAccelerometer.getResolution(), 48755c451c7861a029e26e5f16e319b629169e656dPeng Xu .power = mSensorFusion.getPowerUsage(), 49755c451c7861a029e26e5f16e319b629169e656dPeng Xu .minDelay = mSensorFusion.getMinDelay(), 50755c451c7861a029e26e5f16e319b629169e656dPeng Xu }; 51755c451c7861a029e26e5f16e319b629169e656dPeng Xu mSensor = Sensor(&sensor); 52f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 53f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 54f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopianbool GravitySensor::process(sensors_event_t* outEvent, 55f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian const sensors_event_t& event) 56f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian{ 57f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian if (event.type == SENSOR_TYPE_ACCELEROMETER) { 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t g; 59f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (!mSensorFusion.hasEstimate(FUSION_NOMAG)) 603301542828febc768e1df42892cfac4992c35474Mathias Agopian return false; 61f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const mat33_t R(mSensorFusion.getRotationMatrix(FUSION_NOMAG)); 623301542828febc768e1df42892cfac4992c35474Mathias Agopian // FIXME: we need to estimate the length of gravity because 633301542828febc768e1df42892cfac4992c35474Mathias Agopian // the accelerometer may have a small scaling error. This 643301542828febc768e1df42892cfac4992c35474Mathias Agopian // translates to an offset in the linear-acceleration sensor. 653301542828febc768e1df42892cfac4992c35474Mathias Agopian g = R[2] * GRAVITY_EARTH; 663301542828febc768e1df42892cfac4992c35474Mathias Agopian 67f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian *outEvent = event; 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[0] = g.x; 69984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[1] = g.y; 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[2] = g.z; 71f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian outEvent->sensor = '_grv'; 72f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian outEvent->type = SENSOR_TYPE_GRAVITY; 73f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return true; 74f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 75f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return false; 76f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 77984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 78f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopianstatus_t GravitySensor::activate(void* ident, bool enabled) { 79f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return mSensorFusion.activate(FUSION_NOMAG, ident, enabled); 80f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 81f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 8292dc3fc52cf097bd105460cf377779bdcf146d62Mark Salyzynstatus_t GravitySensor::setDelay(void* ident, int /*handle*/, int64_t ns) { 83f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return mSensorFusion.setDelay(FUSION_NOMAG, ident, ns); 84f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 85f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 86f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian// --------------------------------------------------------------------------- 87f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian}; // namespace android 88f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 89