| 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);
|
| +
|
| + ComputeQuaternionFromEulerAngles(alpha, beta, gamma, x, y, z, w);
|
| +}
|
| +
|
| +} // namespace device
|
|
|