1 /* 2 * Copyright (C) 2010 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 "RotationVectorSensor.h" 26 27 namespace android { 28 // --------------------------------------------------------------------------- 29 30 template <typename T> 31 static inline T clamp(T v) { 32 return v < 0 ? 0 : v; 33 } 34 35 RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count) 36 : mSensorDevice(SensorDevice::getInstance()), 37 mEnabled(false), 38 mALowPass(M_SQRT1_2, 5.0f), 39 mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass), 40 mMLowPass(M_SQRT1_2, 2.5f), 41 mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass) 42 { 43 for (size_t i=0 ; i<count ; i++) { 44 if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 45 mAcc = Sensor(list + i); 46 } 47 if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { 48 mMag = Sensor(list + i); 49 } 50 } 51 memset(mMagData, 0, sizeof(mMagData)); 52 } 53 54 bool RotationVectorSensor::process(sensors_event_t* outEvent, 55 const sensors_event_t& event) 56 { 57 const static double NS2S = 1.0 / 1000000000.0; 58 if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { 59 const double now = event.timestamp * NS2S; 60 if (mMagTime == 0) { 61 mMagData[0] = mMX.init(event.magnetic.x); 62 mMagData[1] = mMY.init(event.magnetic.y); 63 mMagData[2] = mMZ.init(event.magnetic.z); 64 } else { 65 double dT = now - mMagTime; 66 mMLowPass.setSamplingPeriod(dT); 67 mMagData[0] = mMX(event.magnetic.x); 68 mMagData[1] = mMY(event.magnetic.y); 69 mMagData[2] = mMZ(event.magnetic.z); 70 } 71 mMagTime = now; 72 } 73 if (event.type == SENSOR_TYPE_ACCELEROMETER) { 74 const double now = event.timestamp * NS2S; 75 float Ax, Ay, Az; 76 if (mAccTime == 0) { 77 Ax = mAX.init(event.acceleration.x); 78 Ay = mAY.init(event.acceleration.y); 79 Az = mAZ.init(event.acceleration.z); 80 } else { 81 double dT = now - mAccTime; 82 mALowPass.setSamplingPeriod(dT); 83 Ax = mAX(event.acceleration.x); 84 Ay = mAY(event.acceleration.y); 85 Az = mAZ(event.acceleration.z); 86 } 87 mAccTime = now; 88 const float Ex = mMagData[0]; 89 const float Ey = mMagData[1]; 90 const float Ez = mMagData[2]; 91 float Hx = Ey*Az - Ez*Ay; 92 float Hy = Ez*Ax - Ex*Az; 93 float Hz = Ex*Ay - Ey*Ax; 94 const float normH = sqrtf(Hx*Hx + Hy*Hy + Hz*Hz); 95 if (normH < 0.1f) { 96 // device is close to free fall (or in space?), or close to 97 // magnetic north pole. Typical values are > 100. 98 return false; 99 } 100 const float invH = 1.0f / normH; 101 const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az); 102 Hx *= invH; 103 Hy *= invH; 104 Hz *= invH; 105 Ax *= invA; 106 Ay *= invA; 107 Az *= invA; 108 const float Mx = Ay*Hz - Az*Hy; 109 const float My = Az*Hx - Ax*Hz; 110 const float Mz = Ax*Hy - Ay*Hx; 111 112 // matrix to rotation vector (normalized quaternion) 113 float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); 114 float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); 115 float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); 116 float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); 117 const float n = 1.0f / (qw*qw + qx*qx + qy*qy + qz*qz); 118 qx = copysignf(qx, Ay - Mz) * n; 119 qy = copysignf(qy, Hz - Ax) * n; 120 qz = copysignf(qz, Mx - Hy) * n; 121 122 *outEvent = event; 123 outEvent->data[0] = qx; 124 outEvent->data[1] = qy; 125 outEvent->data[2] = qz; 126 outEvent->sensor = '_rov'; 127 outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; 128 return true; 129 } 130 return false; 131 } 132 133 bool RotationVectorSensor::isEnabled() const { 134 return mEnabled; 135 } 136 137 status_t RotationVectorSensor::activate(void* ident, bool enabled) { 138 if (mEnabled != enabled) { 139 mSensorDevice.activate(this, mAcc.getHandle(), enabled); 140 mSensorDevice.activate(this, mMag.getHandle(), enabled); 141 mEnabled = enabled; 142 if (enabled) { 143 mMagTime = 0; 144 mAccTime = 0; 145 } 146 } 147 return NO_ERROR; 148 } 149 150 status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) 151 { 152 mSensorDevice.setDelay(this, mAcc.getHandle(), ns); 153 mSensorDevice.setDelay(this, mMag.getHandle(), ns); 154 return NO_ERROR; 155 } 156 157 Sensor RotationVectorSensor::getSensor() const { 158 sensor_t hwSensor; 159 hwSensor.name = "Rotation Vector Sensor"; 160 hwSensor.vendor = "Google Inc."; 161 hwSensor.version = 1; 162 hwSensor.handle = '_rov'; 163 hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; 164 hwSensor.maxRange = 1; 165 hwSensor.resolution = 1.0f / (1<<24); 166 hwSensor.power = mAcc.getPowerUsage() + mMag.getPowerUsage(); 167 hwSensor.minDelay = mAcc.getMinDelay(); 168 Sensor sensor(&hwSensor); 169 return sensor; 170 } 171 172 // --------------------------------------------------------------------------- 173 }; // namespace android 174 175
