OLD | NEW |
---|---|
(Empty) | |
1 // Copyright 2017 The Chromium Authors. All rights reserved. | |
2 // Use of this source code is governed by a BSD-style license that can be | |
3 // found in the LICENSE file. | |
4 | |
5 #include "device/generic_sensor/platform_sensor_relative_orientation_using_accel erometer.h" | |
6 | |
7 #include <stdint.h> | |
8 | |
9 #include <cmath> | |
10 | |
11 #include "base/bind.h" | |
12 #include "base/memory/ptr_util.h" | |
13 #include "device/generic_sensor/generic_sensor_consts.h" | |
14 #include "device/generic_sensor/platform_sensor_provider.h" | |
15 #include "device/generic_sensor/public/cpp/sensor_shared_buffer_reader.h" | |
16 | |
17 namespace { | |
18 | |
19 constexpr double kQuaternionThreshold = 0.01; | |
20 | |
21 bool IsSignificantlyDifferent(const device::SensorReading& reading1, | |
22 const device::SensorReading& reading2) { | |
23 return (std::fabs(reading1.values[0] - reading2.values[0]) >= | |
24 kQuaternionThreshold) || | |
25 (std::fabs(reading1.values[1] - reading2.values[1]) >= | |
26 kQuaternionThreshold) || | |
27 (std::fabs(reading1.values[2] - reading2.values[2]) >= | |
28 kQuaternionThreshold); | |
29 } | |
30 | |
31 void ComputeQuaternionFromEulerAngles(double alpha, | |
32 double beta, | |
33 double gamma, | |
34 double* x, | |
35 double* y, | |
36 double* z, | |
37 double* w) { | |
38 double cx = std::cos(beta / 2); | |
39 double cy = std::cos(gamma / 2); | |
40 double cz = std::cos(alpha / 2); | |
41 double sx = std::sin(beta / 2); | |
42 double sy = std::sin(gamma / 2); | |
43 double sz = std::sin(alpha / 2); | |
44 | |
45 *x = sx * cy * cz - cx * sy * sz; | |
46 *y = cx * sy * cz + sx * cy * sz; | |
47 *z = cx * cy * sz + sx * sy * cz; | |
48 *w = cx * cy * cz - sx * sy * sz; | |
49 } | |
50 | |
51 } // namespace | |
52 | |
53 namespace device { | |
54 | |
55 PlatformSensorRelativeOrientationUsingAccelerometer:: | |
56 PlatformSensorRelativeOrientationUsingAccelerometer( | |
57 mojo::ScopedSharedBufferMapping mapping, | |
58 PlatformSensorProvider* provider) | |
59 : PlatformSensor(mojom::SensorType::RELATIVE_ORIENTATION, | |
shalamov
2017/06/14 07:22:45
Would it be better to create 'fusion' (PlatformSen
juncai
2017/06/19 23:23:25
Thanks. For this CL, the relative orientation fusi
| |
60 std::move(mapping), | |
61 provider) { | |
62 provider->CreateSensor( | |
63 mojom::SensorType::ACCELEROMETER, | |
64 base::Bind(&PlatformSensorRelativeOrientationUsingAccelerometer:: | |
65 CreateAccelerometerCallback, | |
66 base::Unretained(this))); | |
67 } | |
68 | |
69 PlatformSensorRelativeOrientationUsingAccelerometer:: | |
70 ~PlatformSensorRelativeOrientationUsingAccelerometer() = default; | |
71 | |
72 mojom::ReportingMode | |
73 PlatformSensorRelativeOrientationUsingAccelerometer::GetReportingMode() { | |
74 return mojom::ReportingMode::ON_CHANGE; | |
Reilly Grant (use Gerrit)
2017/06/14 00:59:56
This should return accelerometer_->GetReportingMod
juncai
2017/06/19 23:23:25
Done.
| |
75 } | |
76 | |
77 PlatformSensorConfiguration | |
78 PlatformSensorRelativeOrientationUsingAccelerometer::GetDefaultConfiguration() { | |
79 return PlatformSensorConfiguration(kDefaultAccelerometerFrequencyHz); | |
80 } | |
81 | |
82 bool PlatformSensorRelativeOrientationUsingAccelerometer::StartSensor( | |
83 const PlatformSensorConfiguration& configuration) { | |
84 if (!accelerometer_ || | |
85 !accelerometer_->StartSensor( | |
86 PlatformSensorConfiguration(kDefaultAccelerometerFrequencyHz))) { | |
87 return false; | |
88 } | |
89 | |
90 accelerometer_reader_ = base::MakeUnique<SensorSharedBufferReader>( | |
91 static_cast<const SensorReadingSharedBuffer*>( | |
92 accelerometer_->shared_buffer_mapping().get())); | |
93 | |
94 timer_.Start( | |
Reilly Grant (use Gerrit)
2017/06/14 00:59:56
|accelerometer_| already has a timer and can notif
juncai
2017/06/19 23:23:25
Done.
| |
95 FROM_HERE, | |
96 base::TimeDelta::FromMicroseconds(base::Time::kMicrosecondsPerSecond / | |
97 configuration.frequency()), | |
98 this, &PlatformSensorRelativeOrientationUsingAccelerometer::PollForData); | |
99 | |
100 return true; | |
101 } | |
102 | |
103 void PlatformSensorRelativeOrientationUsingAccelerometer::StopSensor() { | |
104 timer_.Stop(); | |
105 if (accelerometer_) | |
106 accelerometer_->StopSensor(); | |
107 } | |
108 | |
109 bool PlatformSensorRelativeOrientationUsingAccelerometer:: | |
110 CheckSensorConfiguration(const PlatformSensorConfiguration& configuration) { | |
111 return configuration.frequency() > 0 && | |
112 configuration.frequency() <= | |
113 mojom::SensorConfiguration::kMaxAllowedFrequency; | |
114 } | |
115 | |
116 void PlatformSensorRelativeOrientationUsingAccelerometer:: | |
117 CreateAccelerometerCallback(scoped_refptr<PlatformSensor> accelerometer) { | |
118 accelerometer_ = accelerometer; | |
119 } | |
120 | |
121 void PlatformSensorRelativeOrientationUsingAccelerometer::PollForData() { | |
shalamov
2017/06/14 07:22:45
Wouldn't it be better to move all algorithms outsi
juncai
2017/06/19 23:23:25
Done.
| |
122 SensorReading reading; | |
123 reading.timestamp = (base::TimeTicks::Now() - base::TimeTicks()).InSecondsF(); | |
124 | |
125 if (!accelerometer_reader_->GetSensorReading(&reading)) | |
126 return; | |
127 | |
128 // Transform the accelerometer values to W3C draft angles. | |
129 // | |
130 // Accelerometer values are just dot products of the sensor axes | |
131 // by the gravity vector 'g' with the result for the z axis inverted. | |
132 // | |
133 // To understand this transformation calculate the 3rd row of the z-x-y | |
134 // Euler angles rotation matrix (because of the 'g' vector, only 3rd row | |
135 // affects to the result). Note that z-x-y matrix means R = Ry * Rx * Rz. | |
136 // Then, assume alpha = 0 and you get this: | |
137 // | |
138 // x_acc = sin(gamma) | |
139 // y_acc = - cos(gamma) * sin(beta) | |
140 // z_acc = cos(beta) * cos(gamma) | |
141 // | |
142 // After that the rest is just a bit of trigonometry. | |
143 // | |
144 // Also note that alpha can't be provided but it's assumed to be always zero. | |
145 // This is necessary in order to provide enough information to solve | |
146 // the equations. | |
147 // | |
148 double alpha = 0.0; | |
149 double beta = std::atan2(-reading.values[1], reading.values[2]); | |
shalamov
2017/06/14 07:22:44
Do we need to filter (low-pass) accelerometer read
juncai
2017/06/19 23:23:25
The main purpose of this CL is to add relative ori
| |
150 double gamma = std::asin(reading.values[0] / kMeanGravity); | |
151 | |
152 ComputeQuaternionFromEulerAngles( | |
153 alpha, beta, gamma, &reading.values[0].value(), | |
154 &reading.values[1].value(), &reading.values[2].value(), | |
155 &reading.values[3].value()); | |
156 | |
157 if (!IsSignificantlyDifferent(reading_, reading)) | |
158 return; | |
159 | |
160 reading_ = reading; | |
161 UpdateSensorReading(reading_, true); | |
162 } | |
163 | |
164 } // namespace device | |
OLD | NEW |