1 /* 2 * Copyright (C) 2011 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include <stdint.h> 18 #include <math.h> 19 #include <sys/types.h> 20 21 #include <utils/Errors.h> 22 23 #include <hardware/sensors.h> 24 25 #include "CorrectedGyroSensor.h" 26 #include "SensorDevice.h" 27 #include "SensorFusion.h" 28 29 namespace android { 30 // --------------------------------------------------------------------------- 31 32 CorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count) 33 : mSensorDevice(SensorDevice::getInstance()), 34 mSensorFusion(SensorFusion::getInstance()) 35 { 36 for (size_t i=0 ; i<count ; i++) { 37 if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 38 mGyro = Sensor(list + i); 39 break; 40 } 41 } 42 } 43 44 bool CorrectedGyroSensor::process(sensors_event_t* outEvent, 45 const sensors_event_t& event) 46 { 47 if (event.type == SENSOR_TYPE_GYROSCOPE) { 48 const vec3_t bias(mSensorFusion.getGyroBias()); 49 *outEvent = event; 50 outEvent->data[0] -= bias.x; 51 outEvent->data[1] -= bias.y; 52 outEvent->data[2] -= bias.z; 53 outEvent->sensor = '_cgy'; 54 return true; 55 } 56 return false; 57 } 58 59 status_t CorrectedGyroSensor::activate(void* ident, bool enabled) { 60 mSensorDevice.activate(this, mGyro.getHandle(), enabled); 61 return mSensorFusion.activate(this, enabled); 62 } 63 64 status_t CorrectedGyroSensor::setDelay(void* ident, int handle, int64_t ns) { 65 mSensorDevice.setDelay(this, mGyro.getHandle(), ns); 66 return mSensorFusion.setDelay(this, ns); 67 } 68 69 Sensor CorrectedGyroSensor::getSensor() const { 70 sensor_t hwSensor; 71 hwSensor.name = "Corrected Gyroscope Sensor"; 72 hwSensor.vendor = "Google Inc."; 73 hwSensor.version = 1; 74 hwSensor.handle = '_cgy'; 75 hwSensor.type = SENSOR_TYPE_GYROSCOPE; 76 hwSensor.maxRange = mGyro.getMaxValue(); 77 hwSensor.resolution = mGyro.getResolution(); 78 hwSensor.power = mSensorFusion.getPowerUsage(); 79 hwSensor.minDelay = mGyro.getMinDelay(); 80 Sensor sensor(&hwSensor); 81 return sensor; 82 } 83 84 // --------------------------------------------------------------------------- 85 }; // namespace android 86 87