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