123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778 |
- // Copyright 2021 The Chromium Authors. All rights reserved.
- // Use of this source code is governed by a BSD-style license that can be
- // found in the LICENSE file.
- #include "services/device/generic_sensor/gravity_fusion_algorithm_using_accelerometer.h"
- #include "base/check.h"
- #include "services/device/generic_sensor/platform_sensor_fusion.h"
- namespace device {
- GravityFusionAlgorithmUsingAccelerometer::
- GravityFusionAlgorithmUsingAccelerometer()
- : PlatformSensorFusionAlgorithm(mojom::SensorType::GRAVITY,
- {mojom::SensorType::ACCELEROMETER}) {
- Reset();
- }
- GravityFusionAlgorithmUsingAccelerometer::
- ~GravityFusionAlgorithmUsingAccelerometer() = default;
- void GravityFusionAlgorithmUsingAccelerometer::SetFrequency(double frequency) {
- DCHECK(frequency > 0);
- // Set time constant to be bound to requested rate, if sensor can provide
- // data at such rate, low-pass filter alpha would be ~0.5 that is effective
- // at smoothing data and provides minimal latency from LPF.
- time_constant_ = 1 / frequency;
- }
- void GravityFusionAlgorithmUsingAccelerometer::Reset() {
- reading_updates_count_ = 0;
- time_constant_ = 0.0;
- initial_timestamp_ = 0.0;
- gravity_x_ = 0.0;
- gravity_y_ = 0.0;
- gravity_z_ = 0.0;
- }
- bool GravityFusionAlgorithmUsingAccelerometer::GetFusedDataInternal(
- mojom::SensorType which_sensor_changed,
- SensorReading* fused_reading) {
- DCHECK(fusion_sensor_);
- ++reading_updates_count_;
- SensorReading reading;
- if (!fusion_sensor_->GetSourceReading(mojom::SensorType::ACCELEROMETER,
- &reading)) {
- return false;
- }
- // First reading.
- if (initial_timestamp_ == 0.0) {
- initial_timestamp_ = reading.timestamp();
- return false;
- }
- double delivery_rate =
- (reading.timestamp() - initial_timestamp_) / reading_updates_count_;
- double alpha = time_constant_ / (time_constant_ + delivery_rate);
- double acceleration_x = reading.accel.x;
- double acceleration_y = reading.accel.y;
- double acceleration_z = reading.accel.z;
- // Isolate gravity.
- gravity_x_ = alpha * gravity_x_ + (1 - alpha) * acceleration_x;
- gravity_y_ = alpha * gravity_y_ + (1 - alpha) * acceleration_y;
- gravity_z_ = alpha * gravity_z_ + (1 - alpha) * acceleration_z;
- // Get gravity.
- fused_reading->accel.x = gravity_x_;
- fused_reading->accel.y = gravity_y_;
- fused_reading->accel.z = gravity_z_;
- return true;
- }
- } // namespace device
|