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 "RotationVectorSensor.h" 26f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 27f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopiannamespace android { 28f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian// --------------------------------------------------------------------------- 29f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 30984826cc158193e61e3a00359ef4f6699c7d748aMathias AgopianRotationVectorSensor::RotationVectorSensor() 31f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian : mSensorDevice(SensorDevice::getInstance()), 32984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorFusion(SensorFusion::getInstance()) 33f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian{ 34f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 35f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 36f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopianbool RotationVectorSensor::process(sensors_event_t* outEvent, 37f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian const sensors_event_t& event) 38f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian{ 39f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian if (event.type == SENSOR_TYPE_ACCELEROMETER) { 40984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mSensorFusion.hasEstimate()) { 413301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec4_t q(mSensorFusion.getAttitude()); 42984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian *outEvent = event; 433301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[0] = q.x; 443301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[1] = q.y; 453301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[2] = q.z; 463301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[3] = q.w; 47984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->sensor = '_rov'; 48984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; 49984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return true; 50f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 51f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian } 52f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return false; 53f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 54f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 55f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopianstatus_t RotationVectorSensor::activate(void* ident, bool enabled) { 56984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mSensorFusion.activate(this, enabled); 57f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 58f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 59984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) { 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mSensorFusion.setDelay(this, ns); 61f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 62f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 63f001c92436b4a66eb7687286325ced7f10c9f917Mathias AgopianSensor RotationVectorSensor::getSensor() const { 64f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian sensor_t hwSensor; 65f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.name = "Rotation Vector Sensor"; 66f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.vendor = "Google Inc."; 673301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.version = 3; 68f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.handle = '_rov'; 69f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; 70f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.maxRange = 1; 71f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian hwSensor.resolution = 1.0f / (1<<24); 72984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.power = mSensorFusion.getPowerUsage(); 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian hwSensor.minDelay = mSensorFusion.getMinDelay(); 74f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian Sensor sensor(&hwSensor); 75f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian return sensor; 76f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian} 77f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 78f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian// --------------------------------------------------------------------------- 793301542828febc768e1df42892cfac4992c35474Mathias Agopian 803301542828febc768e1df42892cfac4992c35474Mathias AgopianGyroDriftSensor::GyroDriftSensor() 813301542828febc768e1df42892cfac4992c35474Mathias Agopian : mSensorDevice(SensorDevice::getInstance()), 823301542828febc768e1df42892cfac4992c35474Mathias Agopian mSensorFusion(SensorFusion::getInstance()) 833301542828febc768e1df42892cfac4992c35474Mathias Agopian{ 843301542828febc768e1df42892cfac4992c35474Mathias Agopian} 853301542828febc768e1df42892cfac4992c35474Mathias Agopian 863301542828febc768e1df42892cfac4992c35474Mathias Agopianbool GyroDriftSensor::process(sensors_event_t* outEvent, 873301542828febc768e1df42892cfac4992c35474Mathias Agopian const sensors_event_t& event) 883301542828febc768e1df42892cfac4992c35474Mathias Agopian{ 893301542828febc768e1df42892cfac4992c35474Mathias Agopian if (event.type == SENSOR_TYPE_ACCELEROMETER) { 903301542828febc768e1df42892cfac4992c35474Mathias Agopian if (mSensorFusion.hasEstimate()) { 913301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec3_t b(mSensorFusion.getGyroBias()); 923301542828febc768e1df42892cfac4992c35474Mathias Agopian *outEvent = event; 933301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[0] = b.x; 943301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[1] = b.y; 953301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->data[2] = b.z; 963301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->sensor = '_gbs'; 973301542828febc768e1df42892cfac4992c35474Mathias Agopian outEvent->type = SENSOR_TYPE_ACCELEROMETER; 983301542828febc768e1df42892cfac4992c35474Mathias Agopian return true; 993301542828febc768e1df42892cfac4992c35474Mathias Agopian } 1003301542828febc768e1df42892cfac4992c35474Mathias Agopian } 1013301542828febc768e1df42892cfac4992c35474Mathias Agopian return false; 1023301542828febc768e1df42892cfac4992c35474Mathias Agopian} 1033301542828febc768e1df42892cfac4992c35474Mathias Agopian 1043301542828febc768e1df42892cfac4992c35474Mathias Agopianstatus_t GyroDriftSensor::activate(void* ident, bool enabled) { 1053301542828febc768e1df42892cfac4992c35474Mathias Agopian return mSensorFusion.activate(this, enabled); 1063301542828febc768e1df42892cfac4992c35474Mathias Agopian} 1073301542828febc768e1df42892cfac4992c35474Mathias Agopian 1083301542828febc768e1df42892cfac4992c35474Mathias Agopianstatus_t GyroDriftSensor::setDelay(void* ident, int handle, int64_t ns) { 1093301542828febc768e1df42892cfac4992c35474Mathias Agopian return mSensorFusion.setDelay(this, ns); 1103301542828febc768e1df42892cfac4992c35474Mathias Agopian} 1113301542828febc768e1df42892cfac4992c35474Mathias Agopian 1123301542828febc768e1df42892cfac4992c35474Mathias AgopianSensor GyroDriftSensor::getSensor() const { 1133301542828febc768e1df42892cfac4992c35474Mathias Agopian sensor_t hwSensor; 1143301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.name = "Gyroscope Bias (debug)"; 1153301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.vendor = "Google Inc."; 1163301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.version = 1; 1173301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.handle = '_gbs'; 1183301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.type = SENSOR_TYPE_ACCELEROMETER; 1193301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.maxRange = 1; 1203301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.resolution = 1.0f / (1<<24); 1213301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.power = mSensorFusion.getPowerUsage(); 1223301542828febc768e1df42892cfac4992c35474Mathias Agopian hwSensor.minDelay = mSensorFusion.getMinDelay(); 1233301542828febc768e1df42892cfac4992c35474Mathias Agopian Sensor sensor(&hwSensor); 1243301542828febc768e1df42892cfac4992c35474Mathias Agopian return sensor; 1253301542828febc768e1df42892cfac4992c35474Mathias Agopian} 1263301542828febc768e1df42892cfac4992c35474Mathias Agopian 1273301542828febc768e1df42892cfac4992c35474Mathias Agopian// --------------------------------------------------------------------------- 128f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian}; // namespace android 129f001c92436b4a66eb7687286325ced7f10c9f917Mathias Agopian 130