blob: f261810b58a1a90e26509f755999d2c020fbe730 [file] [log] [blame]
/*
* 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 "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;
}
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, "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) {
uncal->x_bias = 0.0;
uncal->y_bias = 0.0;
uncal->z_bias = 0.0;
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) {
uncal->x_bias = 0.0;
uncal->y_bias = 0.0;
uncal->z_bias = 0.0;
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, "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