336 lines
14 KiB
C++
336 lines
14 KiB
C++
/*
|
|
* Copyright (C) 2020 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include <log/log.h>
|
|
#include <utils/SystemClock.h>
|
|
#include <math.h>
|
|
#include <qemud.h>
|
|
#include <random>
|
|
#include "multihal_sensors.h"
|
|
#include "sensor_list.h"
|
|
|
|
namespace goldfish {
|
|
using ahs10::EventPayload;
|
|
using ahs21::SensorType;
|
|
using ahs10::SensorStatus;
|
|
|
|
namespace {
|
|
const char* testPrefix(const char* i, const char* end, const char* v, const char sep) {
|
|
while (i < end) {
|
|
if (*v == 0) {
|
|
return (*i == sep) ? (i + 1) : nullptr;
|
|
} else if (*v == *i) {
|
|
++v;
|
|
++i;
|
|
} else {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
bool approximatelyEqual(double a, double b, double eps) {
|
|
return fabs(a - b) <= std::max(fabs(a), fabs(b)) * eps;
|
|
}
|
|
|
|
int64_t weigthedAverage(const int64_t a, int64_t aw, int64_t b, int64_t bw) {
|
|
return (a * aw + b * bw) / (aw + bw);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
bool MultihalSensors::activateQemuSensorImpl(const int pipe,
|
|
const int sensorHandle,
|
|
const bool enabled) {
|
|
char buffer[64];
|
|
int len = snprintf(buffer, sizeof(buffer),
|
|
"set:%s:%d",
|
|
getQemuSensorNameByHandle(sensorHandle),
|
|
(enabled ? 1 : 0));
|
|
|
|
if (qemud_channel_send(pipe, buffer, len) < 0) {
|
|
ALOGE("%s:%d: qemud_channel_send failed", __func__, __LINE__);
|
|
return false;
|
|
} else {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
bool MultihalSensors::setAllQemuSensors(const bool enabled) {
|
|
uint32_t mask = m_availableSensorsMask;
|
|
for (int i = 0; mask; ++i, mask >>= 1) {
|
|
if (mask & 1) {
|
|
if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, enabled)) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
double MultihalSensors::randomError(float lo, float hi) {
|
|
std::uniform_real_distribution<> distribution(lo, hi);
|
|
return distribution(gen);
|
|
}
|
|
|
|
void MultihalSensors::parseQemuSensorEvent(const int pipe,
|
|
QemuSensorsProtocolState* state) {
|
|
char buf[256];
|
|
const int len = qemud_channel_recv(pipe, buf, sizeof(buf) - 1);
|
|
if (len < 0) {
|
|
ALOGE("%s:%d: qemud_channel_recv failed", __func__, __LINE__);
|
|
}
|
|
const int64_t nowNs = ::android::elapsedRealtimeNano();
|
|
buf[len] = 0;
|
|
const char* end = buf + len;
|
|
bool parsed = false;
|
|
Event event;
|
|
EventPayload* payload = &event.u;
|
|
ahs10::Vec3* vec3 = &payload->vec3;
|
|
ahs10::Uncal* uncal = &payload->uncal;
|
|
if (const char* values = testPrefix(buf, end, "acceleration", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&vec3->x, &vec3->y, &vec3->z) == 3) {
|
|
vec3->status = SensorStatus::ACCURACY_MEDIUM;
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleAccelerometer;
|
|
event.sensorType = SensorType::ACCELEROMETER;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "acceleration-uncalibrated", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&uncal->x, &uncal->y, &uncal->z) == 3) {
|
|
// A little bias noise to pass CTS
|
|
uncal->x_bias = randomError(-0.003f, 0.003f);
|
|
uncal->y_bias = randomError(-0.003f, 0.003f);
|
|
uncal->z_bias = randomError(-0.003f, 0.003f);
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleAccelerometerUncalibrated;
|
|
event.sensorType = SensorType::ACCELEROMETER_UNCALIBRATED;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "gyroscope", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&vec3->x, &vec3->y, &vec3->z) == 3) {
|
|
vec3->status = SensorStatus::ACCURACY_MEDIUM;
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleGyroscope;
|
|
event.sensorType = SensorType::GYROSCOPE;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "gyroscope-uncalibrated", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&uncal->x, &uncal->y, &uncal->z) == 3) {
|
|
//Uncalibrated gyro values needs to be close to 0,0,0.
|
|
uncal->x += randomError(0.00005f, 0.001f);
|
|
uncal->y += randomError(0.00005f, 0.001f);
|
|
uncal->z += randomError(0.00005f, 0.001f);
|
|
// Bias noise
|
|
uncal->x_bias = randomError(-0.0003f, 0.0003f);
|
|
uncal->y_bias = randomError(-0.0003f, 0.0003f);
|
|
uncal->z_bias = randomError(-0.0003f, 0.0003f);
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleGyroscopeFieldUncalibrated;
|
|
event.sensorType = SensorType::GYROSCOPE_UNCALIBRATED;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "orientation", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&vec3->x, &vec3->y, &vec3->z) == 3) {
|
|
vec3->status = SensorStatus::ACCURACY_HIGH;
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleOrientation;
|
|
event.sensorType = SensorType::ORIENTATION;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "magnetic", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&vec3->x, &vec3->y, &vec3->z) == 3) {
|
|
vec3->status = SensorStatus::ACCURACY_HIGH;
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleMagneticField;
|
|
event.sensorType = SensorType::MAGNETIC_FIELD;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "magnetic-uncalibrated", ':')) {
|
|
if (sscanf(values, "%f:%f:%f",
|
|
&uncal->x, &uncal->y, &uncal->z) == 3) {
|
|
// A little bias noise to pass CTS
|
|
uncal->x_bias = randomError( -0.003f, 0.003f);
|
|
uncal->y_bias = randomError(-0.003f, 0.003f);
|
|
uncal->z_bias = randomError(-0.003f, 0.003f);
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleMagneticFieldUncalibrated;
|
|
event.sensorType = SensorType::MAGNETIC_FIELD_UNCALIBRATED;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "temperature", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastAmbientTemperatureValue,
|
|
payload->scalar, 0.001)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleAmbientTemperature;
|
|
event.sensorType = SensorType::AMBIENT_TEMPERATURE;
|
|
postSensorEvent(event);
|
|
state->lastAmbientTemperatureValue = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "proximity", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastProximityValue,
|
|
payload->scalar, 0.001)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleProximity;
|
|
event.sensorType = SensorType::PROXIMITY;
|
|
postSensorEvent(event);
|
|
state->lastProximityValue = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "light", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastLightValue,
|
|
payload->scalar, 0.001)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleLight;
|
|
event.sensorType = SensorType::LIGHT;
|
|
postSensorEvent(event);
|
|
state->lastLightValue = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "pressure", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandlePressure;
|
|
event.sensorType = SensorType::PRESSURE;
|
|
postSensorEvent(event);
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "humidity", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastRelativeHumidityValue,
|
|
payload->scalar, 0.001)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleRelativeHumidity;
|
|
event.sensorType = SensorType::RELATIVE_HUMIDITY;
|
|
postSensorEvent(event);
|
|
state->lastRelativeHumidityValue = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "hinge-angle0", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastHingeAngle0Value,
|
|
payload->scalar, 0.001) &&
|
|
// b/197586273, ignore the state tracking if system sensor
|
|
// service has not enabled hinge sensor
|
|
isSensorActive(kSensorHandleHingeAngle0)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleHingeAngle0;
|
|
event.sensorType = SensorType::HINGE_ANGLE;
|
|
postSensorEvent(event);
|
|
state->lastHingeAngle0Value = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "hinge-angle1", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastHingeAngle1Value,
|
|
payload->scalar, 0.001) &&
|
|
isSensorActive(kSensorHandleHingeAngle1)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleHingeAngle1;
|
|
event.sensorType = SensorType::HINGE_ANGLE;
|
|
postSensorEvent(event);
|
|
state->lastHingeAngle1Value = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "hinge-angle2", ':')) {
|
|
if (sscanf(values, "%f", &payload->scalar) == 1) {
|
|
if (!approximatelyEqual(state->lastHingeAngle2Value,
|
|
payload->scalar, 0.001) &&
|
|
isSensorActive(kSensorHandleHingeAngle2)) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleHingeAngle2;
|
|
event.sensorType = SensorType::HINGE_ANGLE;
|
|
postSensorEvent(event);
|
|
state->lastHingeAngle2Value = payload->scalar;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "heart-rate", ':')) {
|
|
if (sscanf(values, "%f", &payload->heartRate.bpm) == 1) {
|
|
if (!approximatelyEqual(state->lastHeartRateValue,
|
|
payload->heartRate.bpm, 0.001)) {
|
|
payload->heartRate.status = SensorStatus::ACCURACY_HIGH;
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleHeartRate;
|
|
event.sensorType = SensorType::HEART_RATE;
|
|
postSensorEvent(event);
|
|
state->lastHeartRateValue = payload->heartRate.bpm;
|
|
}
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "wrist-tilt", ':')) {
|
|
long measurementId;
|
|
int args = sscanf(values, "%f:%ld", &payload->scalar, &measurementId);
|
|
if (args == 2) {
|
|
if (state->lastWristTiltMeasurement != measurementId) {
|
|
event.timestamp = nowNs + state->timeBiasNs;
|
|
event.sensorHandle = kSensorHandleWristTilt;
|
|
event.sensorType = SensorType::WRIST_TILT_GESTURE;
|
|
postSensorEvent(event);
|
|
state->lastWristTiltMeasurement = measurementId;
|
|
}
|
|
}
|
|
if (args >= 1) {
|
|
// Skip if the measurement id is not included.
|
|
parsed = true;
|
|
}
|
|
|
|
} else if (const char* values = testPrefix(buf, end, "guest-sync", ':')) {
|
|
long long value;
|
|
if ((sscanf(values, "%lld", &value) == 1) && (value >= 0)) {
|
|
const int64_t guestTimeNs = static_cast<int64_t>(value * 1000LL);
|
|
const int64_t timeBiasNs = guestTimeNs - nowNs;
|
|
state->timeBiasNs =
|
|
std::min(int64_t(0),
|
|
weigthedAverage(state->timeBiasNs, 3, timeBiasNs, 1));
|
|
parsed = true;
|
|
}
|
|
} else if (const char* values = testPrefix(buf, end, "sync", ':')) {
|
|
parsed = true;
|
|
}
|
|
|
|
if (!parsed) {
|
|
ALOGW("%s:%d: don't know how to parse '%s'", __func__, __LINE__, buf);
|
|
}
|
|
}
|
|
|
|
} // namespace
|