absolute_orientation_euler_angles_fusion_algorithm_using_accelerometer_and_magnetometer.cc 5.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146
  1. // Copyright 2017 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/absolute_orientation_euler_angles_fusion_algorithm_using_accelerometer_and_magnetometer.h"
  5. #include <cmath>
  6. #include "base/check.h"
  7. #include "base/numerics/math_constants.h"
  8. #include "services/device/generic_sensor/orientation_util.h"
  9. #include "services/device/generic_sensor/platform_sensor_fusion.h"
  10. namespace {
  11. // Helper function to compute the rotation matrix using gravity and geomagnetic
  12. // data. Returns a 9 element rotation matrix:
  13. // r[ 0] r[ 1] r[ 2]
  14. // r[ 3] r[ 4] r[ 5]
  15. // r[ 6] r[ 7] r[ 8]
  16. // r is meaningful only when the device is not free-falling and it is not close
  17. // to the magnetic north. If the device is accelerating, or placed into a
  18. // strong magnetic field, the returned matricx may be inaccurate.
  19. //
  20. // Free fall is defined as condition when the magnitude of the gravity is less
  21. // than 1/10 of the nominal value.
  22. bool ComputeRotationMatrixFromGravityAndGeomagnetic(double gravity_x,
  23. double gravity_y,
  24. double gravity_z,
  25. double geomagnetic_x,
  26. double geomagnetic_y,
  27. double geomagnetic_z,
  28. std::vector<double>* r) {
  29. double gravity_squared =
  30. (gravity_x * gravity_x + gravity_y * gravity_y + gravity_z * gravity_z);
  31. double free_fall_gravity_squared =
  32. 0.01 * base::kMeanGravityDouble * base::kMeanGravityDouble;
  33. if (gravity_squared < free_fall_gravity_squared) {
  34. // gravity less than 10% of normal value
  35. return false;
  36. }
  37. double hx = geomagnetic_y * gravity_z - geomagnetic_z * gravity_y;
  38. double hy = geomagnetic_z * gravity_x - geomagnetic_x * gravity_z;
  39. double hz = geomagnetic_x * gravity_y - geomagnetic_y * gravity_x;
  40. double norm_h = std::sqrt(hx * hx + hy * hy + hz * hz);
  41. if (norm_h < 0.1) {
  42. // device is close to free fall, or close to magnetic north pole.
  43. // Typical values are > 100.
  44. return false;
  45. }
  46. double inv_h = 1.0 / norm_h;
  47. hx *= inv_h;
  48. hy *= inv_h;
  49. hz *= inv_h;
  50. double inv_gravity = 1.0 / std::sqrt(gravity_squared);
  51. gravity_x *= inv_gravity;
  52. gravity_y *= inv_gravity;
  53. gravity_z *= inv_gravity;
  54. double mx = gravity_y * hz - gravity_z * hy;
  55. double my = gravity_z * hx - gravity_x * hz;
  56. double mz = gravity_x * hy - gravity_y * hx;
  57. r->resize(9);
  58. (*r)[0] = hx;
  59. (*r)[1] = hy;
  60. (*r)[2] = hz;
  61. (*r)[3] = mx;
  62. (*r)[4] = my;
  63. (*r)[5] = mz;
  64. (*r)[6] = gravity_x;
  65. (*r)[7] = gravity_y;
  66. (*r)[8] = gravity_z;
  67. return true;
  68. }
  69. bool ComputeAbsoluteOrientationEulerAnglesFromGravityAndGeomagnetic(
  70. double gravity_x,
  71. double gravity_y,
  72. double gravity_z,
  73. double geomagnetic_x,
  74. double geomagnetic_y,
  75. double geomagnetic_z,
  76. double* alpha_in_degrees,
  77. double* beta_in_degrees,
  78. double* gamma_in_degrees) {
  79. std::vector<double> rotation_matrix;
  80. if (!ComputeRotationMatrixFromGravityAndGeomagnetic(
  81. gravity_x, gravity_y, gravity_z, geomagnetic_x, geomagnetic_y,
  82. geomagnetic_z, &rotation_matrix)) {
  83. return false;
  84. }
  85. device::ComputeOrientationEulerAnglesFromRotationMatrix(
  86. rotation_matrix, alpha_in_degrees, beta_in_degrees, gamma_in_degrees);
  87. return true;
  88. }
  89. } // namespace
  90. namespace device {
  91. AbsoluteOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndMagnetometer::
  92. AbsoluteOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndMagnetometer()
  93. : PlatformSensorFusionAlgorithm(
  94. mojom::SensorType::ABSOLUTE_ORIENTATION_EULER_ANGLES,
  95. {mojom::SensorType::ACCELEROMETER, mojom::SensorType::MAGNETOMETER}) {
  96. }
  97. AbsoluteOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndMagnetometer::
  98. ~AbsoluteOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndMagnetometer() =
  99. default;
  100. bool AbsoluteOrientationEulerAnglesFusionAlgorithmUsingAccelerometerAndMagnetometer::
  101. GetFusedDataInternal(mojom::SensorType which_sensor_changed,
  102. SensorReading* fused_reading) {
  103. // Transform the accelerometer and magnetometer values to W3C draft Euler
  104. // angles.
  105. DCHECK(fusion_sensor_);
  106. // Only generate a new sensor value when the accelerometer reading changes.
  107. if (which_sensor_changed != mojom::SensorType::ACCELEROMETER)
  108. return false;
  109. SensorReading gravity_reading;
  110. SensorReading geomagnetic_reading;
  111. if (!fusion_sensor_->GetSourceReading(mojom::SensorType::ACCELEROMETER,
  112. &gravity_reading) ||
  113. !fusion_sensor_->GetSourceReading(mojom::SensorType::MAGNETOMETER,
  114. &geomagnetic_reading)) {
  115. return false;
  116. }
  117. return ComputeAbsoluteOrientationEulerAnglesFromGravityAndGeomagnetic(
  118. gravity_reading.accel.x, gravity_reading.accel.y, gravity_reading.accel.z,
  119. geomagnetic_reading.magn.x, geomagnetic_reading.magn.y,
  120. geomagnetic_reading.magn.z,
  121. &fused_reading->orientation_euler.z.value() /* alpha_in_degrees */,
  122. &fused_reading->orientation_euler.x.value() /* beta_in_degrees */,
  123. &fused_reading->orientation_euler.y.value() /* gamma_in_degrees */);
  124. }
  125. } // namespace device