1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
|
/*
* Copyright (C) 2010 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <stdint.h>
#include <math.h>
#include <sys/types.h>
#include <utils/Errors.h>
#include <hardware/sensors.h>
#include "RotationVectorSensor.h"
namespace android {
// ---------------------------------------------------------------------------
template <typename T>
static inline T clamp(T v) {
return v < 0 ? 0 : v;
}
RotationVectorSensor::RotationVectorSensor()
: mSensorDevice(SensorDevice::getInstance()),
mSensorFusion(SensorFusion::getInstance())
{
}
bool RotationVectorSensor::process(sensors_event_t* outEvent,
const sensors_event_t& event)
{
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
if (mSensorFusion.hasEstimate()) {
const mat33_t R(mSensorFusion.getRotationMatrix());
// matrix to rotation vector (normalized quaternion)
const float Hx = R[0].x;
const float My = R[1].y;
const float Az = R[2].z;
float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
qx = copysignf(qx, R[2].y - R[1].z);
qy = copysignf(qy, R[0].z - R[2].x);
qz = copysignf(qz, R[1].x - R[0].y);
// this quaternion is guaranteed to be normalized, by construction
// of the rotation matrix.
*outEvent = event;
outEvent->data[0] = qx;
outEvent->data[1] = qy;
outEvent->data[2] = qz;
outEvent->data[3] = qw;
outEvent->sensor = '_rov';
outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
return true;
}
}
return false;
}
status_t RotationVectorSensor::activate(void* ident, bool enabled) {
return mSensorFusion.activate(this, enabled);
}
status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) {
return mSensorFusion.setDelay(this, ns);
}
Sensor RotationVectorSensor::getSensor() const {
sensor_t hwSensor;
hwSensor.name = "Rotation Vector Sensor";
hwSensor.vendor = "Google Inc.";
hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2;
hwSensor.handle = '_rov';
hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
hwSensor.maxRange = 1;
hwSensor.resolution = 1.0f / (1<<24);
hwSensor.power = mSensorFusion.getPowerUsage();
hwSensor.minDelay = mSensorFusion.getMinDelay();
Sensor sensor(&hwSensor);
return sensor;
}
// ---------------------------------------------------------------------------
}; // namespace android
|
