text: // Copyright 2017 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/linear_acceleration_fusion_algorithm_using_accelerometer.h"
  
  #include "base/logging.h"
  #include "services/device/generic_sensor/platform_sensor_fusion.h"
  
  namespace device {
  
  LinearAccelerationFusionAlgorithmUsingAccelerometer::
      LinearAccelerationFusionAlgorithmUsingAccelerometer()
      : PlatformSensorFusionAlgorithm(mojom::SensorType::LINEAR_ACCELERATION,
                                      {mojom::SensorType::ACCELEROMETER}) {
    Reset();
  }
  
  LinearAccelerationFusionAlgorithmUsingAccelerometer::
      ~LinearAccelerationFusionAlgorithmUsingAccelerometer() = default;
  
  void LinearAccelerationFusionAlgorithmUsingAccelerometer::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 LinearAccelerationFusionAlgorithmUsingAccelerometer::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 LinearAccelerationFusionAlgorithmUsingAccelerometer::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 linear acceleration.
    fused_reading->accel.x = acceleration_x - gravity_x_;
    fused_reading->accel.y = acceleration_y - gravity_y_;
    fused_reading->accel.z = acceleration_z - gravity_z_;
  
    return true;
  }
  
  }  // namespace device
  