173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian/* 273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Copyright (C) 2011 The Android Open Source Project 373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * you may not use this file except in compliance with the License. 673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * You may obtain a copy of the License at 773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 1073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Unless required by applicable law or agreed to in writing, software 1173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 1273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * See the License for the specific language governing permissions and 1473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * limitations under the License. 1573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian */ 1673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 1773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <stdint.h> 1873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <math.h> 1973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <sys/types.h> 2073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <utils/Errors.h> 2273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <hardware/sensors.h> 2473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "CorrectedGyroSensor.h" 2673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "SensorDevice.h" 2773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "SensorFusion.h" 2873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiannamespace android { 3073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// --------------------------------------------------------------------------- 3173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 3273e0bc805a143d8cc2202fccb73230459edc6869Mathias AgopianCorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count) 3373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian : mSensorDevice(SensorDevice::getInstance()), 3473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSensorFusion(SensorFusion::getInstance()) 3573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian{ 3673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian for (size_t i=0 ; i<count ; i++) { 3773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 3873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mGyro = Sensor(list + i); 3973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian break; 4073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 4173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 4273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 4373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 4473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianbool CorrectedGyroSensor::process(sensors_event_t* outEvent, 4573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const sensors_event_t& event) 4673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian{ 4773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (event.type == SENSOR_TYPE_GYROSCOPE) { 486043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec3_t bias(mSensorFusion.getGyroBias()); 4973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian *outEvent = event; 5073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian outEvent->data[0] -= bias.x; 5173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian outEvent->data[1] -= bias.y; 5273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian outEvent->data[2] -= bias.z; 5373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian outEvent->sensor = '_cgy'; 5473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return true; 5573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 5673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return false; 5773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 5873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 5973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatus_t CorrectedGyroSensor::activate(void* ident, bool enabled) { 6073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSensorDevice.activate(this, mGyro.getHandle(), enabled); 6173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return mSensorFusion.activate(this, enabled); 6273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 6373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 6473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatus_t CorrectedGyroSensor::setDelay(void* ident, int handle, int64_t ns) { 6573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSensorDevice.setDelay(this, mGyro.getHandle(), ns); 6673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return mSensorFusion.setDelay(this, ns); 6773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 6873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 6973e0bc805a143d8cc2202fccb73230459edc6869Mathias AgopianSensor CorrectedGyroSensor::getSensor() const { 7073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian sensor_t hwSensor; 7173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.name = "Corrected Gyroscope Sensor"; 7273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.vendor = "Google Inc."; 7373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.version = 1; 7473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.handle = '_cgy'; 7573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.type = SENSOR_TYPE_GYROSCOPE; 7673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.maxRange = mGyro.getMaxValue(); 7773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.resolution = mGyro.getResolution(); 7873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.power = mSensorFusion.getPowerUsage(); 7973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian hwSensor.minDelay = mGyro.getMinDelay(); 8073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Sensor sensor(&hwSensor); 8173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return sensor; 8273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 8373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 8473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// --------------------------------------------------------------------------- 8573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian}; // namespace android 8673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 87