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/commands/sensors_simulator/sensors_simulator.h"
18
19 #include <cmath>
20
21 #include <android-base/logging.h>
22
23 namespace cuttlefish {
24 namespace sensors {
25
26 namespace {
27
28 constexpr double kG = 9.80665; // meter per second^2
29 const Eigen::Vector3d kGravityVec{0, kG, 0}, kMagneticField{0, 5.9, -48.4};
30
ToRadians(double x)31 inline double ToRadians(double x) { return x * M_PI / 180; }
32
33 // Calculate the rotation matrix of the pitch, roll, and yaw angles.
GetRotationMatrix(double x,double y,double z)34 static Eigen::Matrix3d GetRotationMatrix(double x, double y, double z) {
35 x = ToRadians(-x);
36 y = ToRadians(-y);
37 z = ToRadians(-z);
38 // Create rotation matrices for each Euler angle
39 Eigen::Matrix3d rx =
40 Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()).toRotationMatrix();
41 Eigen::Matrix3d ry =
42 Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY()).toRotationMatrix();
43 Eigen::Matrix3d rz =
44 Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ()).toRotationMatrix();
45
46 return rz * (ry * rx);
47 }
48
49 // Calculate new Accelerometer values of the new rotation degrees.
CalculateAcceleration(Eigen::Matrix3d current_rotation_matrix)50 static inline Eigen::Vector3d CalculateAcceleration(
51 Eigen::Matrix3d current_rotation_matrix) {
52 return current_rotation_matrix * kGravityVec;
53 }
54
55 // Calculate new Magnetometer values of the new rotation degrees.
CalculateMagnetometer(Eigen::Matrix3d current_rotation_matrix)56 static inline Eigen::Vector3d CalculateMagnetometer(
57 Eigen::Matrix3d current_rotation_matrix) {
58 return current_rotation_matrix * kMagneticField;
59 }
60
61 // 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)62 static Eigen::Vector3d CalculateGyroscope(
63 std::chrono::duration<double> duration,
64 Eigen::Matrix3d prior_rotation_matrix,
65 Eigen::Matrix3d current_rotation_matrix) {
66 double time_diff = duration.count();
67 if (time_diff == 0) {
68 return Eigen::Vector3d{0, 0, 0};
69 }
70 Eigen::Matrix3d transition_matrix =
71 prior_rotation_matrix * current_rotation_matrix.inverse();
72 // Convert rotation matrix to angular velocity numerator.
73 Eigen::AngleAxisd angle_axis(transition_matrix);
74 double angle = angle_axis.angle();
75 Eigen::Vector3d gyro = angle_axis.axis();
76 gyro *= angle;
77 gyro /= time_diff;
78 return gyro;
79 }
80 } // namespace
81
SensorsSimulator()82 SensorsSimulator::SensorsSimulator()
83 : current_rotation_matrix_(GetRotationMatrix(0, 0, 0)),
84 last_event_timestamp_(std::chrono::high_resolution_clock::now()) {
85 // Initialize sensors_data_ based on rotation vector = (0, 0, 0)
86 RefreshSensors(0, 0, 0);
87 }
88
RefreshSensors(double x,double y,double z)89 void SensorsSimulator::RefreshSensors(double x, double y, double z) {
90 auto rotation_matrix_update = GetRotationMatrix(x, y, z);
91 auto acc_update = CalculateAcceleration(rotation_matrix_update);
92 auto mgn_update = CalculateMagnetometer(rotation_matrix_update);
93
94 std::lock_guard<std::mutex> lock(sensors_data_mtx_);
95 auto current_time = std::chrono::high_resolution_clock::now();
96 auto duration = current_time - last_event_timestamp_;
97 last_event_timestamp_ = current_time;
98
99 auto gyro_update = CalculateGyroscope(duration, current_rotation_matrix_,
100 rotation_matrix_update);
101
102 current_rotation_matrix_ = rotation_matrix_update;
103
104 sensors_data_[kRotationVecId] << x, y, z;
105 sensors_data_[kAccelerationId] = acc_update;
106 sensors_data_[kGyroscopeId] = gyro_update;
107 sensors_data_[kMagneticId] = mgn_update;
108
109 // Copy the calibrated sensor data over for uncalibrated sensor support
110 sensors_data_[kUncalibAccelerationId] = acc_update;
111 sensors_data_[kUncalibGyroscopeId] = gyro_update;
112 sensors_data_[kUncalibMagneticId] = mgn_update;
113 }
114
GetSensorsData(const SensorsMask mask)115 std::string SensorsSimulator::GetSensorsData(const SensorsMask mask) {
116 std::stringstream sensors_msg;
117 std::lock_guard<std::mutex> lock(sensors_data_mtx_);
118 for (int id = 0; id <= kMaxSensorId; id++) {
119 if (mask & (1 << id)) {
120 auto v = sensors_data_[id];
121 sensors_msg << v(0) << INNER_DELIM << v(1) << INNER_DELIM << v(2)
122 << OUTER_DELIM;
123 }
124 }
125 return sensors_msg.str();
126 }
127
128 } // namespace sensors
129 } // namespace cuttlefish