relative_orientation_euler_angles_fusion_algorithm_using_accelerometer_and_gyroscope.cc 4.1 KB

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