1 /*
2 * Copyright (C) 2023 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "host/frontend/webrtc/sensors_simulator.h"
18
19 #include <android-base/logging.h>
20
21 #include <cmath>
22
23 namespace cuttlefish {
24 namespace webrtc_streaming {
25 namespace {
26
27 constexpr double kG = 9.80665; // meter per second^2
28 const Eigen::Vector3d kGravityVec{0, kG, 0}, kMagneticField{0, 5.9, -48.4};
29
toRadians(double x)30 inline double toRadians(double x) { return x * M_PI / 180; }
31
32 // Calculate the rotation matrix of the pitch, roll, and yaw angles.
getRotationMatrix(double x,double y,double z)33 static Eigen::Matrix3d getRotationMatrix(double x, double y, double z) {
34 x = toRadians(-x);
35 y = toRadians(-y);
36 z = toRadians(-z);
37 // Create rotation matrices for each Euler angle
38 Eigen::Matrix3d rx = Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()).toRotationMatrix();
39 Eigen::Matrix3d ry = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY()).toRotationMatrix();
40 Eigen::Matrix3d rz = Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ()).toRotationMatrix();
41
42 return rz * (ry * rx);
43 }
44
45 // Calculate new Accelerometer values of the new rotation degrees.
calculateAcceleration(Eigen::Matrix3d current_rotation_matrix)46 static inline Eigen::Vector3d calculateAcceleration(Eigen::Matrix3d current_rotation_matrix) {
47 return current_rotation_matrix * kGravityVec;
48 }
49
50 // Calculate new Magnetometer values of the new rotation degrees.
calculateMagnetometer(Eigen::Matrix3d current_rotation_matrix)51 static inline Eigen::Vector3d calculateMagnetometer(Eigen::Matrix3d current_rotation_matrix) {
52 return current_rotation_matrix * kMagneticField;
53 }
54
55 // Calculate new Gyroscope values of the new rotation degrees.
calculateGyroscope(std::chrono::duration<double> duration,Eigen::Matrix3d prior_rotation_matrix,Eigen::Matrix3d current_rotation_matrix)56 static Eigen::Vector3d calculateGyroscope(std::chrono::duration<double> duration,
57 Eigen::Matrix3d prior_rotation_matrix,
58 Eigen::Matrix3d current_rotation_matrix) {
59 double time_diff = duration.count();
60 if (time_diff == 0) {
61 return Eigen::Vector3d{0, 0, 0};
62 }
63 Eigen::Matrix3d transition_matrix = prior_rotation_matrix * current_rotation_matrix.inverse();
64 // Convert rotation matrix to angular velocity numerator.
65 Eigen::AngleAxisd angle_axis(transition_matrix);
66 double angle = angle_axis.angle();
67 Eigen::Vector3d gyro = angle_axis.axis();
68 gyro *= angle;
69 gyro /= time_diff;
70 return gyro;
71 }
72
SerializeVector(const Eigen::Vector3d & v)73 std::string SerializeVector(const Eigen::Vector3d& v) {
74 std::stringstream s;
75 s << v(0) << " " << v(1) << " " << v(2);
76 return s.str();
77 }
78
79 } // namespace
80
SensorsSimulator()81 SensorsSimulator::SensorsSimulator()
82 : current_rotation_matrix_(getRotationMatrix(0, 0, 0)),
83 last_event_timestamp_(std::chrono::high_resolution_clock::now()) {}
84
85 // Update sensor values based on new rotation status.
RefreshSensors(double x,double y,double z)86 void SensorsSimulator::RefreshSensors(double x, double y, double z) {
87 xyz_ << x, y, z;
88 prior_rotation_matrix_ = current_rotation_matrix_;
89 current_rotation_matrix_ = getRotationMatrix(x, y, z);
90 acc_xyz_ = calculateAcceleration(current_rotation_matrix_);
91 mgn_xyz_ = calculateMagnetometer(current_rotation_matrix_);
92 auto current_time = std::chrono::high_resolution_clock::now();
93 std::chrono::duration<double> duration = current_time - last_event_timestamp_;
94 gyro_xyz_ = calculateGyroscope(duration, prior_rotation_matrix_,
95 current_rotation_matrix_);
96 last_event_timestamp_ = current_time;
97 }
98
99 // Get sensors' data in string format to be passed as a message.
GetSensorsData()100 std::string SensorsSimulator::GetSensorsData() {
101 std::stringstream sensors_data;
102 sensors_data << SerializeVector(xyz_) << " " << SerializeVector(acc_xyz_) << " "
103 << SerializeVector(mgn_xyz_) << " " << SerializeVector(gyro_xyz_);
104 return sensors_data.str();
105 }
106
107 } // namespace webrtc_streaming
108 } // namespace cuttlefish
109