Index: device/generic_sensor/platform_sensor_relative_orientation_using_accelerometer.cc |
diff --git a/device/generic_sensor/platform_sensor_relative_orientation_using_accelerometer.cc b/device/generic_sensor/platform_sensor_relative_orientation_using_accelerometer.cc |
new file mode 100644 |
index 0000000000000000000000000000000000000000..186eeee58238841b333a0409e864cc24ce527596 |
--- /dev/null |
+++ b/device/generic_sensor/platform_sensor_relative_orientation_using_accelerometer.cc |
@@ -0,0 +1,164 @@ |
+// 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 "device/generic_sensor/platform_sensor_relative_orientation_using_accelerometer.h" |
+ |
+#include <stdint.h> |
+ |
+#include <cmath> |
+ |
+#include "base/bind.h" |
+#include "base/memory/ptr_util.h" |
+#include "device/generic_sensor/generic_sensor_consts.h" |
+#include "device/generic_sensor/platform_sensor_provider.h" |
+#include "device/generic_sensor/public/cpp/sensor_shared_buffer_reader.h" |
+ |
+namespace { |
+ |
+constexpr double kQuaternionThreshold = 0.01; |
+ |
+bool IsSignificantlyDifferent(const device::SensorReading& reading1, |
+ const device::SensorReading& reading2) { |
+ return (std::fabs(reading1.values[0] - reading2.values[0]) >= |
+ kQuaternionThreshold) || |
+ (std::fabs(reading1.values[1] - reading2.values[1]) >= |
+ kQuaternionThreshold) || |
+ (std::fabs(reading1.values[2] - reading2.values[2]) >= |
+ kQuaternionThreshold); |
+} |
+ |
+void ComputeQuaternionFromEulerAngles(double alpha, |
+ double beta, |
+ double gamma, |
+ double* x, |
+ double* y, |
+ double* z, |
+ double* w) { |
+ double cx = std::cos(beta / 2); |
+ double cy = std::cos(gamma / 2); |
+ double cz = std::cos(alpha / 2); |
+ double sx = std::sin(beta / 2); |
+ double sy = std::sin(gamma / 2); |
+ double sz = std::sin(alpha / 2); |
+ |
+ *x = sx * cy * cz - cx * sy * sz; |
+ *y = cx * sy * cz + sx * cy * sz; |
+ *z = cx * cy * sz + sx * sy * cz; |
+ *w = cx * cy * cz - sx * sy * sz; |
+} |
+ |
+} // namespace |
+ |
+namespace device { |
+ |
+PlatformSensorRelativeOrientationUsingAccelerometer:: |
+ PlatformSensorRelativeOrientationUsingAccelerometer( |
+ mojo::ScopedSharedBufferMapping mapping, |
+ PlatformSensorProvider* provider) |
+ : PlatformSensor(mojom::SensorType::RELATIVE_ORIENTATION, |
shalamov
2017/06/14 07:22:45
Would it be better to create 'fusion' (PlatformSen
juncai
2017/06/19 23:23:25
Thanks. For this CL, the relative orientation fusi
|
+ std::move(mapping), |
+ provider) { |
+ provider->CreateSensor( |
+ mojom::SensorType::ACCELEROMETER, |
+ base::Bind(&PlatformSensorRelativeOrientationUsingAccelerometer:: |
+ CreateAccelerometerCallback, |
+ base::Unretained(this))); |
+} |
+ |
+PlatformSensorRelativeOrientationUsingAccelerometer:: |
+ ~PlatformSensorRelativeOrientationUsingAccelerometer() = default; |
+ |
+mojom::ReportingMode |
+PlatformSensorRelativeOrientationUsingAccelerometer::GetReportingMode() { |
+ return mojom::ReportingMode::ON_CHANGE; |
Reilly Grant (use Gerrit)
2017/06/14 00:59:56
This should return accelerometer_->GetReportingMod
juncai
2017/06/19 23:23:25
Done.
|
+} |
+ |
+PlatformSensorConfiguration |
+PlatformSensorRelativeOrientationUsingAccelerometer::GetDefaultConfiguration() { |
+ return PlatformSensorConfiguration(kDefaultAccelerometerFrequencyHz); |
+} |
+ |
+bool PlatformSensorRelativeOrientationUsingAccelerometer::StartSensor( |
+ const PlatformSensorConfiguration& configuration) { |
+ if (!accelerometer_ || |
+ !accelerometer_->StartSensor( |
+ PlatformSensorConfiguration(kDefaultAccelerometerFrequencyHz))) { |
+ return false; |
+ } |
+ |
+ accelerometer_reader_ = base::MakeUnique<SensorSharedBufferReader>( |
+ static_cast<const SensorReadingSharedBuffer*>( |
+ accelerometer_->shared_buffer_mapping().get())); |
+ |
+ timer_.Start( |
Reilly Grant (use Gerrit)
2017/06/14 00:59:56
|accelerometer_| already has a timer and can notif
juncai
2017/06/19 23:23:25
Done.
|
+ FROM_HERE, |
+ base::TimeDelta::FromMicroseconds(base::Time::kMicrosecondsPerSecond / |
+ configuration.frequency()), |
+ this, &PlatformSensorRelativeOrientationUsingAccelerometer::PollForData); |
+ |
+ return true; |
+} |
+ |
+void PlatformSensorRelativeOrientationUsingAccelerometer::StopSensor() { |
+ timer_.Stop(); |
+ if (accelerometer_) |
+ accelerometer_->StopSensor(); |
+} |
+ |
+bool PlatformSensorRelativeOrientationUsingAccelerometer:: |
+ CheckSensorConfiguration(const PlatformSensorConfiguration& configuration) { |
+ return configuration.frequency() > 0 && |
+ configuration.frequency() <= |
+ mojom::SensorConfiguration::kMaxAllowedFrequency; |
+} |
+ |
+void PlatformSensorRelativeOrientationUsingAccelerometer:: |
+ CreateAccelerometerCallback(scoped_refptr<PlatformSensor> accelerometer) { |
+ accelerometer_ = accelerometer; |
+} |
+ |
+void PlatformSensorRelativeOrientationUsingAccelerometer::PollForData() { |
shalamov
2017/06/14 07:22:45
Wouldn't it be better to move all algorithms outsi
juncai
2017/06/19 23:23:25
Done.
|
+ SensorReading reading; |
+ reading.timestamp = (base::TimeTicks::Now() - base::TimeTicks()).InSecondsF(); |
+ |
+ if (!accelerometer_reader_->GetSensorReading(&reading)) |
+ return; |
+ |
+ // Transform the accelerometer values to W3C draft angles. |
+ // |
+ // Accelerometer values are just dot products of the sensor axes |
+ // by the gravity vector 'g' with the result for the z axis inverted. |
+ // |
+ // To understand this transformation calculate the 3rd row of the z-x-y |
+ // Euler angles rotation matrix (because of the 'g' vector, only 3rd row |
+ // affects to the result). Note that z-x-y matrix means R = Ry * Rx * Rz. |
+ // Then, assume alpha = 0 and you get this: |
+ // |
+ // x_acc = sin(gamma) |
+ // y_acc = - cos(gamma) * sin(beta) |
+ // z_acc = cos(beta) * cos(gamma) |
+ // |
+ // After that the rest is just a bit of trigonometry. |
+ // |
+ // Also note that alpha can't be provided but it's assumed to be always zero. |
+ // This is necessary in order to provide enough information to solve |
+ // the equations. |
+ // |
+ double alpha = 0.0; |
+ double beta = std::atan2(-reading.values[1], reading.values[2]); |
shalamov
2017/06/14 07:22:44
Do we need to filter (low-pass) accelerometer read
juncai
2017/06/19 23:23:25
The main purpose of this CL is to add relative ori
|
+ double gamma = std::asin(reading.values[0] / kMeanGravity); |
+ |
+ ComputeQuaternionFromEulerAngles( |
+ alpha, beta, gamma, &reading.values[0].value(), |
+ &reading.values[1].value(), &reading.values[2].value(), |
+ &reading.values[3].value()); |
+ |
+ if (!IsSignificantlyDifferent(reading_, reading)) |
+ return; |
+ |
+ reading_ = reading; |
+ UpdateSensorReading(reading_, true); |
+} |
+ |
+} // namespace device |