1 /*
2 * Copyright (C) 2020 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 <log/log.h>
18 #include <utils/SystemClock.h>
19 #include <math.h>
20 #include <qemud.h>
21 #include <random>
22 #include "multihal_sensors.h"
23 #include "sensor_list.h"
24
25 namespace goldfish {
26 using ahs10::EventPayload;
27 using ahs21::SensorType;
28 using ahs10::SensorStatus;
29
30 namespace {
testPrefix(const char * i,const char * end,const char * v,const char sep)31 const char* testPrefix(const char* i, const char* end, const char* v, const char sep) {
32 while (i < end) {
33 if (*v == 0) {
34 return (*i == sep) ? (i + 1) : nullptr;
35 } else if (*v == *i) {
36 ++v;
37 ++i;
38 } else {
39 return nullptr;
40 }
41 }
42
43 return nullptr;
44 }
45
approximatelyEqual(double a,double b,double eps)46 bool approximatelyEqual(double a, double b, double eps) {
47 return fabs(a - b) <= std::max(fabs(a), fabs(b)) * eps;
48 }
49
weigthedAverage(const int64_t a,int64_t aw,int64_t b,int64_t bw)50 int64_t weigthedAverage(const int64_t a, int64_t aw, int64_t b, int64_t bw) {
51 return (a * aw + b * bw) / (aw + bw);
52 }
53
54 } // namespace
55
activateQemuSensorImpl(const int pipe,const int sensorHandle,const bool enabled)56 bool MultihalSensors::activateQemuSensorImpl(const int pipe,
57 const int sensorHandle,
58 const bool enabled) {
59 char buffer[64];
60 int len = snprintf(buffer, sizeof(buffer),
61 "set:%s:%d",
62 getQemuSensorNameByHandle(sensorHandle),
63 (enabled ? 1 : 0));
64
65 if (qemud_channel_send(pipe, buffer, len) < 0) {
66 ALOGE("%s:%d: qemud_channel_send failed", __func__, __LINE__);
67 return false;
68 } else {
69 return true;
70 }
71 }
72
setAllQemuSensors(const bool enabled)73 bool MultihalSensors::setAllQemuSensors(const bool enabled) {
74 uint32_t mask = m_availableSensorsMask;
75 for (int i = 0; mask; ++i, mask >>= 1) {
76 if (mask & 1) {
77 if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, enabled)) {
78 return false;
79 }
80 }
81 }
82
83 return true;
84 }
85
randomError(float lo,float hi)86 double MultihalSensors::randomError(float lo, float hi) {
87 std::uniform_real_distribution<> distribution(lo, hi);
88 return distribution(gen);
89 }
90
parseQemuSensorEvent(const int pipe,QemuSensorsProtocolState * state)91 void MultihalSensors::parseQemuSensorEvent(const int pipe,
92 QemuSensorsProtocolState* state) {
93 char buf[256];
94 const int len = qemud_channel_recv(pipe, buf, sizeof(buf) - 1);
95 if (len < 0) {
96 ALOGE("%s:%d: qemud_channel_recv failed", __func__, __LINE__);
97 }
98 const int64_t nowNs = ::android::elapsedRealtimeNano();
99 buf[len] = 0;
100 const char* end = buf + len;
101 bool parsed = false;
102 Event event;
103 EventPayload* payload = &event.u;
104 ahs10::Vec3* vec3 = &payload->vec3;
105 ahs10::Uncal* uncal = &payload->uncal;
106 if (const char* values = testPrefix(buf, end, "acceleration", ':')) {
107 if (sscanf(values, "%f:%f:%f",
108 &vec3->x, &vec3->y, &vec3->z) == 3) {
109 vec3->status = SensorStatus::ACCURACY_MEDIUM;
110 event.timestamp = nowNs + state->timeBiasNs;
111 event.sensorHandle = kSensorHandleAccelerometer;
112 event.sensorType = SensorType::ACCELEROMETER;
113 postSensorEvent(event);
114 parsed = true;
115 }
116 } else if (const char* values = testPrefix(buf, end, "acceleration-uncalibrated", ':')) {
117 if (sscanf(values, "%f:%f:%f",
118 &uncal->x, &uncal->y, &uncal->z) == 3) {
119 // A little bias noise to pass CTS
120 uncal->x_bias = randomError(-0.003f, 0.003f);
121 uncal->y_bias = randomError(-0.003f, 0.003f);
122 uncal->z_bias = randomError(-0.003f, 0.003f);
123 event.timestamp = nowNs + state->timeBiasNs;
124 event.sensorHandle = kSensorHandleAccelerometerUncalibrated;
125 event.sensorType = SensorType::ACCELEROMETER_UNCALIBRATED;
126 postSensorEvent(event);
127 parsed = true;
128 }
129 } else if (const char* values = testPrefix(buf, end, "gyroscope", ':')) {
130 if (sscanf(values, "%f:%f:%f",
131 &vec3->x, &vec3->y, &vec3->z) == 3) {
132 vec3->status = SensorStatus::ACCURACY_MEDIUM;
133 event.timestamp = nowNs + state->timeBiasNs;
134 event.sensorHandle = kSensorHandleGyroscope;
135 event.sensorType = SensorType::GYROSCOPE;
136 postSensorEvent(event);
137 parsed = true;
138 }
139 } else if (const char* values = testPrefix(buf, end, "gyroscope-uncalibrated", ':')) {
140 if (sscanf(values, "%f:%f:%f",
141 &uncal->x, &uncal->y, &uncal->z) == 3) {
142 //Uncalibrated gyro values needs to be close to 0,0,0.
143 uncal->x += randomError(0.00005f, 0.001f);
144 uncal->y += randomError(0.00005f, 0.001f);
145 uncal->z += randomError(0.00005f, 0.001f);
146 // Bias noise
147 uncal->x_bias = randomError(-0.0003f, 0.0003f);
148 uncal->y_bias = randomError(-0.0003f, 0.0003f);
149 uncal->z_bias = randomError(-0.0003f, 0.0003f);
150 event.timestamp = nowNs + state->timeBiasNs;
151 event.sensorHandle = kSensorHandleGyroscopeFieldUncalibrated;
152 event.sensorType = SensorType::GYROSCOPE_UNCALIBRATED;
153 postSensorEvent(event);
154 parsed = true;
155 }
156 } else if (const char* values = testPrefix(buf, end, "orientation", ':')) {
157 if (sscanf(values, "%f:%f:%f",
158 &vec3->x, &vec3->y, &vec3->z) == 3) {
159 vec3->status = SensorStatus::ACCURACY_HIGH;
160 event.timestamp = nowNs + state->timeBiasNs;
161 event.sensorHandle = kSensorHandleOrientation;
162 event.sensorType = SensorType::ORIENTATION;
163 postSensorEvent(event);
164 parsed = true;
165 }
166 } else if (const char* values = testPrefix(buf, end, "magnetic", ':')) {
167 if (sscanf(values, "%f:%f:%f",
168 &vec3->x, &vec3->y, &vec3->z) == 3) {
169 vec3->status = SensorStatus::ACCURACY_HIGH;
170 event.timestamp = nowNs + state->timeBiasNs;
171 event.sensorHandle = kSensorHandleMagneticField;
172 event.sensorType = SensorType::MAGNETIC_FIELD;
173 postSensorEvent(event);
174 parsed = true;
175 }
176 } else if (const char* values = testPrefix(buf, end, "magnetic-uncalibrated", ':')) {
177 if (sscanf(values, "%f:%f:%f",
178 &uncal->x, &uncal->y, &uncal->z) == 3) {
179 // A little bias noise to pass CTS
180 uncal->x_bias = randomError( -0.003f, 0.003f);
181 uncal->y_bias = randomError(-0.003f, 0.003f);
182 uncal->z_bias = randomError(-0.003f, 0.003f);
183 event.timestamp = nowNs + state->timeBiasNs;
184 event.sensorHandle = kSensorHandleMagneticFieldUncalibrated;
185 event.sensorType = SensorType::MAGNETIC_FIELD_UNCALIBRATED;
186 postSensorEvent(event);
187 parsed = true;
188 }
189 } else if (const char* values = testPrefix(buf, end, "temperature", ':')) {
190 if (sscanf(values, "%f", &payload->scalar) == 1) {
191 if (!approximatelyEqual(state->lastAmbientTemperatureValue,
192 payload->scalar, 0.001)) {
193 event.timestamp = nowNs + state->timeBiasNs;
194 event.sensorHandle = kSensorHandleAmbientTemperature;
195 event.sensorType = SensorType::AMBIENT_TEMPERATURE;
196 postSensorEvent(event);
197 state->lastAmbientTemperatureValue = payload->scalar;
198 }
199 parsed = true;
200 }
201 } else if (const char* values = testPrefix(buf, end, "proximity", ':')) {
202 if (sscanf(values, "%f", &payload->scalar) == 1) {
203 if (!approximatelyEqual(state->lastProximityValue,
204 payload->scalar, 0.001)) {
205 event.timestamp = nowNs + state->timeBiasNs;
206 event.sensorHandle = kSensorHandleProximity;
207 event.sensorType = SensorType::PROXIMITY;
208 postSensorEvent(event);
209 state->lastProximityValue = payload->scalar;
210 }
211 parsed = true;
212 }
213 } else if (const char* values = testPrefix(buf, end, "light", ':')) {
214 if (sscanf(values, "%f", &payload->scalar) == 1) {
215 if (!approximatelyEqual(state->lastLightValue,
216 payload->scalar, 0.001)) {
217 event.timestamp = nowNs + state->timeBiasNs;
218 event.sensorHandle = kSensorHandleLight;
219 event.sensorType = SensorType::LIGHT;
220 postSensorEvent(event);
221 state->lastLightValue = payload->scalar;
222 }
223 parsed = true;
224 }
225 } else if (const char* values = testPrefix(buf, end, "pressure", ':')) {
226 if (sscanf(values, "%f", &payload->scalar) == 1) {
227 event.timestamp = nowNs + state->timeBiasNs;
228 event.sensorHandle = kSensorHandlePressure;
229 event.sensorType = SensorType::PRESSURE;
230 postSensorEvent(event);
231 parsed = true;
232 }
233 } else if (const char* values = testPrefix(buf, end, "humidity", ':')) {
234 if (sscanf(values, "%f", &payload->scalar) == 1) {
235 if (!approximatelyEqual(state->lastRelativeHumidityValue,
236 payload->scalar, 0.001)) {
237 event.timestamp = nowNs + state->timeBiasNs;
238 event.sensorHandle = kSensorHandleRelativeHumidity;
239 event.sensorType = SensorType::RELATIVE_HUMIDITY;
240 postSensorEvent(event);
241 state->lastRelativeHumidityValue = payload->scalar;
242 }
243 parsed = true;
244 }
245 } else if (const char* values = testPrefix(buf, end, "hinge-angle0", ':')) {
246 if (sscanf(values, "%f", &payload->scalar) == 1) {
247 if (!approximatelyEqual(state->lastHingeAngle0Value,
248 payload->scalar, 0.001) &&
249 // b/197586273, ignore the state tracking if system sensor
250 // service has not enabled hinge sensor
251 isSensorActive(kSensorHandleHingeAngle0)) {
252 event.timestamp = nowNs + state->timeBiasNs;
253 event.sensorHandle = kSensorHandleHingeAngle0;
254 event.sensorType = SensorType::HINGE_ANGLE;
255 postSensorEvent(event);
256 state->lastHingeAngle0Value = payload->scalar;
257 }
258 parsed = true;
259 }
260 } else if (const char* values = testPrefix(buf, end, "hinge-angle1", ':')) {
261 if (sscanf(values, "%f", &payload->scalar) == 1) {
262 if (!approximatelyEqual(state->lastHingeAngle1Value,
263 payload->scalar, 0.001) &&
264 isSensorActive(kSensorHandleHingeAngle1)) {
265 event.timestamp = nowNs + state->timeBiasNs;
266 event.sensorHandle = kSensorHandleHingeAngle1;
267 event.sensorType = SensorType::HINGE_ANGLE;
268 postSensorEvent(event);
269 state->lastHingeAngle1Value = payload->scalar;
270 }
271 parsed = true;
272 }
273 } else if (const char* values = testPrefix(buf, end, "hinge-angle2", ':')) {
274 if (sscanf(values, "%f", &payload->scalar) == 1) {
275 if (!approximatelyEqual(state->lastHingeAngle2Value,
276 payload->scalar, 0.001) &&
277 isSensorActive(kSensorHandleHingeAngle2)) {
278 event.timestamp = nowNs + state->timeBiasNs;
279 event.sensorHandle = kSensorHandleHingeAngle2;
280 event.sensorType = SensorType::HINGE_ANGLE;
281 postSensorEvent(event);
282 state->lastHingeAngle2Value = payload->scalar;
283 }
284 parsed = true;
285 }
286 } else if (const char* values = testPrefix(buf, end, "heart-rate", ':')) {
287 if (sscanf(values, "%f", &payload->heartRate.bpm) == 1) {
288 if (!approximatelyEqual(state->lastHeartRateValue,
289 payload->heartRate.bpm, 0.001)) {
290 payload->heartRate.status = SensorStatus::ACCURACY_HIGH;
291 event.timestamp = nowNs + state->timeBiasNs;
292 event.sensorHandle = kSensorHandleHeartRate;
293 event.sensorType = SensorType::HEART_RATE;
294 postSensorEvent(event);
295 state->lastHeartRateValue = payload->heartRate.bpm;
296 }
297 parsed = true;
298 }
299 } else if (const char* values = testPrefix(buf, end, "wrist-tilt", ':')) {
300 long measurementId;
301 int args = sscanf(values, "%f:%ld", &payload->scalar, &measurementId);
302 if (args == 2) {
303 if (state->lastWristTiltMeasurement != measurementId) {
304 event.timestamp = nowNs + state->timeBiasNs;
305 event.sensorHandle = kSensorHandleWristTilt;
306 event.sensorType = SensorType::WRIST_TILT_GESTURE;
307 postSensorEvent(event);
308 state->lastWristTiltMeasurement = measurementId;
309 }
310 }
311 if (args >= 1) {
312 // Skip if the measurement id is not included.
313 parsed = true;
314 }
315
316 } else if (const char* values = testPrefix(buf, end, "guest-sync", ':')) {
317 long long value;
318 if ((sscanf(values, "%lld", &value) == 1) && (value >= 0)) {
319 const int64_t guestTimeNs = static_cast<int64_t>(value * 1000LL);
320 const int64_t timeBiasNs = guestTimeNs - nowNs;
321 state->timeBiasNs =
322 std::min(int64_t(0),
323 weigthedAverage(state->timeBiasNs, 3, timeBiasNs, 1));
324 parsed = true;
325 }
326 } else if (const char* values = testPrefix(buf, end, "sync", ':')) {
327 parsed = true;
328 }
329
330 if (!parsed) {
331 ALOGW("%s:%d: don't know how to parse '%s'", __func__, __LINE__, buf);
332 }
333 }
334
335 } // namespace
336