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/relative_orientation_fusion_algorithm_using_acce lerometer.h" | |
6 | |
7 #include <stdint.h> | |
8 | |
9 #include <cmath> | |
10 | |
11 #include "device/generic_sensor/generic_sensor_consts.h" | |
12 | |
13 namespace { | |
14 | |
15 void ComputeQuaternionFromEulerAngles(double alpha, | |
16 double beta, | |
17 double gamma, | |
18 double* x, | |
19 double* y, | |
20 double* z, | |
21 double* w) { | |
22 double cx = std::cos(beta / 2); | |
23 double cy = std::cos(gamma / 2); | |
24 double cz = std::cos(alpha / 2); | |
25 double sx = std::sin(beta / 2); | |
26 double sy = std::sin(gamma / 2); | |
27 double sz = std::sin(alpha / 2); | |
28 | |
29 *x = sx * cy * cz - cx * sy * sz; | |
30 *y = cx * sy * cz + sx * cy * sz; | |
31 *z = cx * cy * sz + sx * sy * cz; | |
32 *w = cx * cy * cz - sx * sy * sz; | |
33 } | |
34 | |
35 } // namespace | |
36 | |
37 namespace device { | |
38 | |
39 RelativeOrientationFusionAlgorithmUsingAccelerometer:: | |
40 RelativeOrientationFusionAlgorithmUsingAccelerometer() {} | |
41 | |
42 RelativeOrientationFusionAlgorithmUsingAccelerometer:: | |
43 ~RelativeOrientationFusionAlgorithmUsingAccelerometer() {} | |
44 | |
45 void RelativeOrientationFusionAlgorithmUsingAccelerometer:: | |
46 GetRelativeOrientationData(double* x, double* y, double* z, double* w) { | |
47 // Transform the accelerometer values to W3C draft angles. | |
48 // | |
49 // Accelerometer values are just dot products of the sensor axes | |
50 // by the gravity vector 'g' with the result for the z axis inverted. | |
51 // | |
52 // To understand this transformation calculate the 3rd row of the z-x-y | |
53 // Euler angles rotation matrix (because of the 'g' vector, only 3rd row | |
54 // affects to the result). Note that z-x-y matrix means R = Ry * Rx * Rz. | |
55 // Then, assume alpha = 0 and you get this: | |
56 // | |
57 // x_acc = sin(gamma) | |
58 // y_acc = - cos(gamma) * sin(beta) | |
59 // z_acc = cos(beta) * cos(gamma) | |
60 // | |
61 // After that the rest is just a bit of trigonometry. | |
62 // | |
63 // Also note that alpha can't be provided but it's assumed to be always zero. | |
64 // This is necessary in order to provide enough information to solve | |
65 // the equations. | |
66 // | |
67 double alpha = 0.0; | |
68 double beta = std::atan2(-acceleration_y_, acceleration_z_); | |
69 double gamma = std::asin(acceleration_x_ / kMeanGravity); | |
timvolodine
2017/06/21 18:22:12
strictly speaking this is actually not real sensor
juncai
2017/07/20 00:22:16
Yes, the sensor fusion concept is general here.
| |
70 | |
71 ComputeQuaternionFromEulerAngles(alpha, beta, gamma, x, y, z, w); | |
timvolodine
2017/06/21 18:22:12
looking ahead e.g. when providing RELATIVE_ORIENTA
juncai
2017/07/20 00:22:16
In the new CL:
https://chromium-review.googlesourc
| |
72 } | |
73 | |
74 } // namespace device | |
OLD | NEW |