1984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian/* 2984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Copyright (C) 2011 The Android Open Source Project 3984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 4984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 5984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * you may not use this file except in compliance with the License. 6984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * You may obtain a copy of the License at 7984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 8984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 9984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 10984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Unless required by applicable law or agreed to in writing, software 11984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 12984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * See the License for the specific language governing permissions and 14984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * limitations under the License. 15984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 16984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 17984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <stdint.h> 18984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <math.h> 19984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <sys/types.h> 20984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 21984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <utils/Errors.h> 22984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 23984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <hardware/sensors.h> 24984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 25984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "CorrectedGyroSensor.h" 26984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorDevice.h" 27984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorFusion.h" 28984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 29984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiannamespace android { 30984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 31984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 32984826cc158193e61e3a00359ef4f6699c7d748aMathias AgopianCorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count) 33755c451c7861a029e26e5f16e319b629169e656dPeng Xu : VirtualSensor() { 34984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t i=0 ; i<count ; i++) { 35984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 36984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyro = Sensor(list + i); 37984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian break; 38984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 39984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 400cc8f809924706c7d683da30605f432635dd5bb6Peng Xu 41755c451c7861a029e26e5f16e319b629169e656dPeng Xu const sensor_t sensor = { 42755c451c7861a029e26e5f16e319b629169e656dPeng Xu .name = "Corrected Gyroscope Sensor", 43755c451c7861a029e26e5f16e319b629169e656dPeng Xu .vendor = "AOSP", 44755c451c7861a029e26e5f16e319b629169e656dPeng Xu .version = 1, 45755c451c7861a029e26e5f16e319b629169e656dPeng Xu .handle = '_cgy', 46755c451c7861a029e26e5f16e319b629169e656dPeng Xu .type = SENSOR_TYPE_GYROSCOPE, 47755c451c7861a029e26e5f16e319b629169e656dPeng Xu .maxRange = mGyro.getMaxValue(), 48755c451c7861a029e26e5f16e319b629169e656dPeng Xu .resolution = mGyro.getResolution(), 49755c451c7861a029e26e5f16e319b629169e656dPeng Xu .power = mSensorFusion.getPowerUsage(), 50755c451c7861a029e26e5f16e319b629169e656dPeng Xu .minDelay = mGyro.getMinDelay(), 51755c451c7861a029e26e5f16e319b629169e656dPeng Xu }; 52755c451c7861a029e26e5f16e319b629169e656dPeng Xu mSensor = Sensor(&sensor); 53984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 54984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 55984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianbool CorrectedGyroSensor::process(sensors_event_t* outEvent, 56984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const sensors_event_t& event) 57984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian{ 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (event.type == SENSOR_TYPE_GYROSCOPE) { 593301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec3_t bias(mSensorFusion.getGyroBias()); 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian *outEvent = event; 61984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[0] -= bias.x; 62984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[1] -= bias.y; 63984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[2] -= bias.z; 64984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->sensor = '_cgy'; 65984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return true; 66984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 67984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return false; 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 69984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t CorrectedGyroSensor::activate(void* ident, bool enabled) { 71bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 72f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return mSensorFusion.activate(FUSION_9AXIS, ident, enabled); 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 74984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 7592dc3fc52cf097bd105460cf377779bdcf146d62Mark Salyzynstatus_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) { 76bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella mSensorDevice.setDelay(ident, mGyro.getHandle(), ns); 77f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns); 78984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 79984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 80984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 81984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 82984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 83