1 // Copyright 2018 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 "services/device/generic_sensor/relative_orientation_euler_angles_fusion_algorithm_using_accelerometer_and_gyroscope.h"
6 
7 #include <cmath>
8 
9 #include "base/check.h"
10 #include "base/numerics/math_constants.h"
11 #include "services/device/generic_sensor/generic_sensor_consts.h"
12 #include "services/device/generic_sensor/platform_sensor_fusion.h"
13 #include "ui/gfx/geometry/angle_conversions.h"
14 
15 namespace device {
16 
17 RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope::
RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope()18     RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope()
19     : PlatformSensorFusionAlgorithm(
20           mojom::SensorType::RELATIVE_ORIENTATION_EULER_ANGLES,
21           {mojom::SensorType::ACCELEROMETER, mojom::SensorType::GYROSCOPE}) {
22   Reset();
23 }
24 
25 RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope::
26     ~RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope() =
27         default;
28 
29 void RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope::
Reset()30     Reset() {
31   timestamp_ = 0.0;
32   alpha_ = 0.0;
33   beta_ = 0.0;
34   gamma_ = 0.0;
35 }
36 
37 bool RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndGyroscope::
GetFusedDataInternal(mojom::SensorType which_sensor_changed,SensorReading * fused_reading)38     GetFusedDataInternal(mojom::SensorType which_sensor_changed,
39                          SensorReading* fused_reading) {
40   DCHECK(fusion_sensor_);
41 
42   // Only generate a new sensor value when the gyroscope reading changes.
43   if (which_sensor_changed != mojom::SensorType::GYROSCOPE)
44     return false;
45 
46   SensorReading accelerometer_reading;
47   SensorReading gyroscope_reading;
48   if (!fusion_sensor_->GetSourceReading(mojom::SensorType::ACCELEROMETER,
49                                         &accelerometer_reading) ||
50       !fusion_sensor_->GetSourceReading(mojom::SensorType::GYROSCOPE,
51                                         &gyroscope_reading)) {
52     return false;
53   }
54 
55   double dt =
56       (timestamp_ != 0.0) ? (gyroscope_reading.timestamp() - timestamp_) : 0.0;
57   timestamp_ = gyroscope_reading.timestamp();
58 
59   double accel_x = accelerometer_reading.accel.x;
60   double accel_y = accelerometer_reading.accel.y;
61   double accel_z = accelerometer_reading.accel.z;
62   double gyro_x = gyroscope_reading.gyro.x;
63   double gyro_y = gyroscope_reading.gyro.y;
64   double gyro_z = gyroscope_reading.gyro.z;
65 
66   // Treat the acceleration vector as an orientation vector by normalizing it.
67   // Keep in mind that the if the device is flipped, the vector will just be
68   // pointing in the other direction, so we have no way to know from the
69   // accelerometer data which way the device is oriented.
70   double norm =
71       std::sqrt(accel_x * accel_x + accel_y * accel_y + accel_z * accel_z);
72   double norm_reciprocal = 0.0;
73   // Avoid dividing by zero.
74   if (norm > kEpsilon)
75     norm_reciprocal = 1 / norm;
76 
77   // As we only can cover half (PI rad) of the full spectrum (2*PI rad) we
78   // multiply the unit vector with values from [-1, 1] with PI/2, covering
79   // [-PI/2, PI/2].
80   double scale = base::kPiDouble / 2;
81 
82   alpha_ += gyro_z * dt;
83   // Make sure |alpha_| is in [0, 2*PI).
84   alpha_ = std::fmod(alpha_, 2 * base::kPiDouble);
85   if (alpha_ < 0)
86     alpha_ += 2 * base::kPiDouble;
87 
88   beta_ = kBias * (beta_ + gyro_x * dt) +
89           (1.0 - kBias) * (accel_x * scale * norm_reciprocal);
90   // Make sure |beta_| is in [-PI, PI).
91   beta_ = std::fmod(beta_, 2 * base::kPiDouble);
92   if (beta_ >= base::kPiDouble)
93     beta_ -= 2 * base::kPiDouble;
94   else if (beta_ < -base::kPiDouble)
95     beta_ += 2 * base::kPiDouble;
96 
97   gamma_ = kBias * (gamma_ + gyro_y * dt) +
98            (1.0 - kBias) * (accel_y * -scale * norm_reciprocal);
99   // Make sure |gamma_| is in [-PI/2, PI/2).
100   gamma_ = std::fmod(gamma_, base::kPiDouble);
101   if (gamma_ >= base::kPiDouble / 2)
102     gamma_ -= base::kPiDouble;
103   else if (gamma_ < -base::kPiDouble / 2)
104     gamma_ += base::kPiDouble;
105 
106   fused_reading->orientation_euler.z = gfx::RadToDeg(alpha_);
107   fused_reading->orientation_euler.x = gfx::RadToDeg(beta_);
108   fused_reading->orientation_euler.y = gfx::RadToDeg(gamma_);
109 
110   return true;
111 }
112 
113 }  // namespace device
114