Index: device/generic_sensor/relative_orientation_fusion_algorithm_using_accelerometer.cc |
diff --git a/device/generic_sensor/relative_orientation_fusion_algorithm_using_accelerometer.cc b/device/generic_sensor/relative_orientation_fusion_algorithm_using_accelerometer.cc |
new file mode 100644 |
index 0000000000000000000000000000000000000000..cd0dafbeaacd7bf66e01d83270649ec20717b8cd |
--- /dev/null |
+++ b/device/generic_sensor/relative_orientation_fusion_algorithm_using_accelerometer.cc |
@@ -0,0 +1,74 @@ |
+// Copyright 2017 The Chromium Authors. All rights reserved. |
+// Use of this source code is governed by a BSD-style license that can be |
+// found in the LICENSE file. |
+ |
+#include "device/generic_sensor/relative_orientation_fusion_algorithm_using_accelerometer.h" |
+ |
+#include <stdint.h> |
+ |
+#include <cmath> |
+ |
+#include "device/generic_sensor/generic_sensor_consts.h" |
+ |
+namespace { |
+ |
+void ComputeQuaternionFromEulerAngles(double alpha, |
+ double beta, |
+ double gamma, |
+ double* x, |
+ double* y, |
+ double* z, |
+ double* w) { |
+ double cx = std::cos(beta / 2); |
+ double cy = std::cos(gamma / 2); |
+ double cz = std::cos(alpha / 2); |
+ double sx = std::sin(beta / 2); |
+ double sy = std::sin(gamma / 2); |
+ double sz = std::sin(alpha / 2); |
+ |
+ *x = sx * cy * cz - cx * sy * sz; |
+ *y = cx * sy * cz + sx * cy * sz; |
+ *z = cx * cy * sz + sx * sy * cz; |
+ *w = cx * cy * cz - sx * sy * sz; |
+} |
+ |
+} // namespace |
+ |
+namespace device { |
+ |
+RelativeOrientationFusionAlgorithmUsingAccelerometer:: |
+ RelativeOrientationFusionAlgorithmUsingAccelerometer() {} |
+ |
+RelativeOrientationFusionAlgorithmUsingAccelerometer:: |
+ ~RelativeOrientationFusionAlgorithmUsingAccelerometer() {} |
+ |
+void RelativeOrientationFusionAlgorithmUsingAccelerometer:: |
+ GetRelativeOrientationData(double* x, double* y, double* z, double* w) { |
+ // Transform the accelerometer values to W3C draft angles. |
+ // |
+ // Accelerometer values are just dot products of the sensor axes |
+ // by the gravity vector 'g' with the result for the z axis inverted. |
+ // |
+ // To understand this transformation calculate the 3rd row of the z-x-y |
+ // Euler angles rotation matrix (because of the 'g' vector, only 3rd row |
+ // affects to the result). Note that z-x-y matrix means R = Ry * Rx * Rz. |
+ // Then, assume alpha = 0 and you get this: |
+ // |
+ // x_acc = sin(gamma) |
+ // y_acc = - cos(gamma) * sin(beta) |
+ // z_acc = cos(beta) * cos(gamma) |
+ // |
+ // After that the rest is just a bit of trigonometry. |
+ // |
+ // Also note that alpha can't be provided but it's assumed to be always zero. |
+ // This is necessary in order to provide enough information to solve |
+ // the equations. |
+ // |
+ double alpha = 0.0; |
+ double beta = std::atan2(-acceleration_y_, acceleration_z_); |
+ 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.
|
+ |
+ 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
|
+} |
+ |
+} // namespace device |