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) 33984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian : mSensorDevice(SensorDevice::getInstance()), 34984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorFusion(SensorFusion::getInstance()) 35984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian{ 36984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t i=0 ; i<count ; i++) { 37984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 38984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyro = Sensor(list + i); 39984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian break; 40984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 41984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 42984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 43984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 44984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianbool CorrectedGyroSensor::process(sensors_event_t* outEvent, 45984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const sensors_event_t& event) 46984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian{ 47984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (event.type == SENSOR_TYPE_GYROSCOPE) { 483301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec3_t bias(mSensorFusion.getGyroBias()); 49984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian *outEvent = event; 50984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[0] -= bias.x; 51984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[1] -= bias.y; 52984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->data[2] -= bias.z; 53984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->sensor = '_cgy'; 54984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return true; 55984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 56984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return false; 57984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 59984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t CorrectedGyroSensor::activate(void* ident, bool enabled) { 60bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 61bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella return mSensorFusion.activate(ident, enabled); 62984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 63984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 6492dc3fc52cf097bd105460cf377779bdcf146d62Mark Salyzynstatus_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) { 65bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella mSensorDevice.setDelay(ident, mGyro.getHandle(), ns); 66bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella return mSensorFusion.setDelay(ident, ns); 67984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 69984826cc158193e61e3a00359ef4f6699c7d748aMathias AgopianSensor CorrectedGyroSensor::getSensor() const { 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian sensor_t hwSensor; 71984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.name = "Corrected Gyroscope Sensor"; 720319306670b0344da99efa606b6f172dde575a39Mathias Agopian hwSensor.vendor = "AOSP"; 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.version = 1; 74984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.handle = '_cgy'; 75984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.type = SENSOR_TYPE_GYROSCOPE; 76984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.maxRange = mGyro.getMaxValue(); 77984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.resolution = mGyro.getResolution(); 78984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.power = mSensorFusion.getPowerUsage(); 79984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.minDelay = mGyro.getMinDelay(); 80984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Sensor sensor(&hwSensor); 81984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return sensor; 82984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 83984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 84984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 85984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 86984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 87