gravity_fusion_algorithm_using_accelerometer.cc 2.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778
  1. // Copyright 2021 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/gravity_fusion_algorithm_using_accelerometer.h"
  5. #include "base/check.h"
  6. #include "services/device/generic_sensor/platform_sensor_fusion.h"
  7. namespace device {
  8. GravityFusionAlgorithmUsingAccelerometer::
  9. GravityFusionAlgorithmUsingAccelerometer()
  10. : PlatformSensorFusionAlgorithm(mojom::SensorType::GRAVITY,
  11. {mojom::SensorType::ACCELEROMETER}) {
  12. Reset();
  13. }
  14. GravityFusionAlgorithmUsingAccelerometer::
  15. ~GravityFusionAlgorithmUsingAccelerometer() = default;
  16. void GravityFusionAlgorithmUsingAccelerometer::SetFrequency(double frequency) {
  17. DCHECK(frequency > 0);
  18. // Set time constant to be bound to requested rate, if sensor can provide
  19. // data at such rate, low-pass filter alpha would be ~0.5 that is effective
  20. // at smoothing data and provides minimal latency from LPF.
  21. time_constant_ = 1 / frequency;
  22. }
  23. void GravityFusionAlgorithmUsingAccelerometer::Reset() {
  24. reading_updates_count_ = 0;
  25. time_constant_ = 0.0;
  26. initial_timestamp_ = 0.0;
  27. gravity_x_ = 0.0;
  28. gravity_y_ = 0.0;
  29. gravity_z_ = 0.0;
  30. }
  31. bool GravityFusionAlgorithmUsingAccelerometer::GetFusedDataInternal(
  32. mojom::SensorType which_sensor_changed,
  33. SensorReading* fused_reading) {
  34. DCHECK(fusion_sensor_);
  35. ++reading_updates_count_;
  36. SensorReading reading;
  37. if (!fusion_sensor_->GetSourceReading(mojom::SensorType::ACCELEROMETER,
  38. &reading)) {
  39. return false;
  40. }
  41. // First reading.
  42. if (initial_timestamp_ == 0.0) {
  43. initial_timestamp_ = reading.timestamp();
  44. return false;
  45. }
  46. double delivery_rate =
  47. (reading.timestamp() - initial_timestamp_) / reading_updates_count_;
  48. double alpha = time_constant_ / (time_constant_ + delivery_rate);
  49. double acceleration_x = reading.accel.x;
  50. double acceleration_y = reading.accel.y;
  51. double acceleration_z = reading.accel.z;
  52. // Isolate gravity.
  53. gravity_x_ = alpha * gravity_x_ + (1 - alpha) * acceleration_x;
  54. gravity_y_ = alpha * gravity_y_ + (1 - alpha) * acceleration_y;
  55. gravity_z_ = alpha * gravity_z_ + (1 - alpha) * acceleration_z;
  56. // Get gravity.
  57. fused_reading->accel.x = gravity_x_;
  58. fused_reading->accel.y = gravity_y_;
  59. fused_reading->accel.z = gravity_z_;
  60. return true;
  61. }
  62. } // namespace device