14da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek/* 24da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * Copyright (C) 2011 The Android Open Source Project 34da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * 44da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * Licensed under the Apache License, Version 2.0 (the "License"); 54da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * you may not use this file except in compliance with the License. 64da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * You may obtain a copy of the License at 74da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * 84da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * http://www.apache.org/licenses/LICENSE-2.0 94da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * 104da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * Unless required by applicable law or agreed to in writing, software 114da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * distributed under the License is distributed on an "AS IS" BASIS, 124da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 134da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * See the License for the specific language governing permissions and 144da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek * limitations under the License. 154da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek */ 164da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 172dc75ed2ef2fa3cbfd8b2ed88eb2369f3fcd98a9Daniel Jasper#include <stdint.h> 184da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include <math.h> 194da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include <sys/types.h> 204da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 214da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include <utils/Errors.h> 224da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 234da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include <hardware/sensors.h> 244da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 254da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include "OrientationSensor.h" 264da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include "SensorDevice.h" 274da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek#include "SensorFusion.h" 282dc75ed2ef2fa3cbfd8b2ed88eb2369f3fcd98a9Daniel Jasper 294da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimeknamespace android { 304da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek// --------------------------------------------------------------------------- 314da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 324da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel KlimekOrientationSensor::OrientationSensor() 334da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek : mSensorDevice(SensorDevice::getInstance()), 344da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek mSensorFusion(SensorFusion::getInstance()) 354da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek{ 364da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek // FIXME: instead of using the SensorFusion code, we should use 374da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek // the SENSOR_TYPE_ROTATION_VECTOR instead. This way we could use the 384da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek // HAL's implementation. 394da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek} 404da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 414da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimekbool OrientationSensor::process(sensors_event_t* outEvent, 424da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek const sensors_event_t& event) 434da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek{ 444da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek if (event.type == SENSOR_TYPE_ACCELEROMETER) { 454da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek if (mSensorFusion.hasEstimate()) { 464da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek vec3_t g; 474da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek const float rad2deg = 180 / M_PI; 48e0e6b9e79a0c4edae92abd3928263875c78e23aaDaniel Jasper const mat33_t R(mSensorFusion.getRotationMatrix()); 494da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; 504da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; 514da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek g[2] = asinf ( R[2][0]) * rad2deg; 52e0e6b9e79a0c4edae92abd3928263875c78e23aaDaniel Jasper if (g[0] < 0) 53425f41b370a8795fbca9ffc3b35e9b0ccbb15d97Sam Panzer g[0] += 360; 544da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek 554da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek *outEvent = event; 564da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek outEvent->orientation.azimuth = g.x; 574da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek outEvent->orientation.pitch = g.y; 584da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek outEvent->orientation.roll = g.z; 594da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek outEvent->orientation.status = SENSOR_STATUS_ACCURACY_HIGH; 6025466496a2ace2aa533abc7bac1e92a4f425e2b1Manuel Klimek outEvent->sensor = '_ypr'; 6125466496a2ace2aa533abc7bac1e92a4f425e2b1Manuel Klimek outEvent->type = SENSOR_TYPE_ORIENTATION; 624da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek return true; 6325466496a2ace2aa533abc7bac1e92a4f425e2b1Manuel Klimek } 644da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek } 654da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek return false; 664da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek} 6725466496a2ace2aa533abc7bac1e92a4f425e2b1Manuel Klimek 68a78d0d6203a990b88c9c3e4c4f2a277001e8bd46Manuel Klimekstatus_t OrientationSensor::activate(void* ident, bool enabled) { 6925466496a2ace2aa533abc7bac1e92a4f425e2b1Manuel Klimek return mSensorFusion.activate(ident, enabled); 704da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek} 71ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek 72a78d0d6203a990b88c9c3e4c4f2a277001e8bd46Manuel Klimekstatus_t OrientationSensor::setDelay(void* ident, int /*handle*/, int64_t ns) { 73ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek return mSensorFusion.setDelay(ident, ns); 74ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek} 75ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek 76ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel KlimekSensor OrientationSensor::getSensor() const { 774da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek sensor_t hwSensor; 784da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.name = "Orientation Sensor"; 794da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.vendor = "AOSP"; 80a78d0d6203a990b88c9c3e4c4f2a277001e8bd46Manuel Klimek hwSensor.version = 1; 814da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.handle = '_ypr'; 824da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.type = SENSOR_TYPE_ORIENTATION; 834da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.maxRange = 360.0f; 84a78d0d6203a990b88c9c3e4c4f2a277001e8bd46Manuel Klimek hwSensor.resolution = 1.0f/256.0f; // FIXME: real value here 854da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.power = mSensorFusion.getPowerUsage(); 864da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek hwSensor.minDelay = mSensorFusion.getMinDelay(); 874da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek Sensor sensor(&hwSensor); 884da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek return sensor; 894da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek} 90ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek 91ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek// --------------------------------------------------------------------------- 924da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek}; // namespace android 93ec2a396c6f11b4017e30f1865f7b62c5a42425b8Manuel Klimek 944da216637fa1ad4bdfd31bc265edb57ca35c2c12Manuel Klimek