| // 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 "services/device/generic_sensor/relative_orientation_euler_angles_fusion_algorithm_using_accelerometer.h" |
| |
| #include <cmath> |
| |
| #include "base/logging.h" |
| #include "services/device/generic_sensor/generic_sensor_consts.h" |
| #include "services/device/generic_sensor/platform_sensor_fusion.h" |
| #include "ui/gfx/geometry/angle_conversions.h" |
| |
| namespace device { |
| |
| namespace { |
| |
| void ComputeRelativeOrientationFromAccelerometer(double acceleration_x, |
| double acceleration_y, |
| double acceleration_z, |
| double* alpha_in_degrees, |
| double* beta_in_degrees, |
| double* gamma_in_degrees) { |
| // 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. |
| *alpha_in_degrees = NAN; |
| *beta_in_degrees = gfx::RadToDeg(std::atan2(-acceleration_y, acceleration_z)); |
| *gamma_in_degrees = gfx::RadToDeg(std::asin(acceleration_x / kMeanGravity)); |
| |
| // Convert beta and gamma to fit the intervals in the specification. Beta is |
| // [-180, 180) and gamma is [-90, 90). |
| if (*beta_in_degrees >= 180.0) |
| *beta_in_degrees = -180.0; |
| if (*gamma_in_degrees >= 90.0) |
| *gamma_in_degrees = -90.0; |
| |
| DCHECK_GE(*beta_in_degrees, -180.0); |
| DCHECK_LT(*beta_in_degrees, 180.0); |
| DCHECK_GE(*gamma_in_degrees, -90.0); |
| DCHECK_LT(*gamma_in_degrees, 90.0); |
| } |
| |
| } // namespace |
| |
| RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometer:: |
| RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometer() |
| : PlatformSensorFusionAlgorithm( |
| mojom::SensorType::RELATIVE_ORIENTATION_EULER_ANGLES, |
| {mojom::SensorType::ACCELEROMETER}) {} |
| |
| RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometer:: |
| ~RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometer() = |
| default; |
| |
| bool RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometer:: |
| GetFusedDataInternal(mojom::SensorType which_sensor_changed, |
| SensorReading* fused_reading) { |
| DCHECK(fusion_sensor_); |
| |
| SensorReading reading; |
| if (!fusion_sensor_->GetSourceReading(mojom::SensorType::ACCELEROMETER, |
| &reading)) { |
| return false; |
| } |
| |
| ComputeRelativeOrientationFromAccelerometer( |
| reading.accel.x, reading.accel.y, reading.accel.z, |
| &fused_reading->orientation_euler.z.value() /* alpha_in_degrees */, |
| &fused_reading->orientation_euler.x.value() /* beta_in_degrees */, |
| &fused_reading->orientation_euler.y.value() /* gamma_in_degrees */); |
| |
| return true; |
| } |
| |
| } // namespace device |