| /* | |
| * 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 <locale> | |
| #include <ftl/enum.h> | |
| #include "../Macros.h" | |
| #include "SensorInputMapper.h" | |
| // Log detailed debug messages about each sensor event notification to the dispatcher. | |
| constexpr bool DEBUG_SENSOR_EVENT_DETAILS = false; | |
| namespace android { | |
| // Mask for the LSB 2nd, 3rd and fourth bits. | |
| constexpr int REPORTING_MODE_MASK = 0xE; | |
| constexpr int REPORTING_MODE_SHIFT = 1; | |
| constexpr float GRAVITY_MS2_UNIT = 9.80665f; | |
| constexpr float DEGREE_RADIAN_UNIT = 0.0174533f; | |
| /* Convert the sensor data from Linux to Android | |
| * Linux accelerometer unit is per g, Android unit is m/s^2 | |
| * Linux gyroscope unit is degree/second, Android unit is radians/second | |
| */ | |
| static void convertFromLinuxToAndroid(std::vector<float>& values, | |
| InputDeviceSensorType sensorType) { | |
| for (size_t i = 0; i < values.size(); i++) { | |
| switch (sensorType) { | |
| case InputDeviceSensorType::ACCELEROMETER: | |
| values[i] *= GRAVITY_MS2_UNIT; | |
| break; | |
| case InputDeviceSensorType::GYROSCOPE: | |
| values[i] *= DEGREE_RADIAN_UNIT; | |
| break; | |
| default: | |
| break; | |
| } | |
| } | |
| } | |
| SensorInputMapper::SensorInputMapper(InputDeviceContext& deviceContext) | |
| : InputMapper(deviceContext) {} | |
| SensorInputMapper::~SensorInputMapper() {} | |
| uint32_t SensorInputMapper::getSources() { | |
| return AINPUT_SOURCE_SENSOR; | |
| } | |
| template <typename T> | |
| bool SensorInputMapper::tryGetProperty(std::string keyName, T& outValue) { | |
| const auto& config = getDeviceContext().getConfiguration(); | |
| return config.tryGetProperty(String8(keyName.c_str()), outValue); | |
| } | |
| void SensorInputMapper::parseSensorConfiguration(InputDeviceSensorType sensorType, int32_t absCode, | |
| int32_t sensorDataIndex, const Axis& axis) { | |
| auto it = mSensors.find(sensorType); | |
| if (it == mSensors.end()) { | |
| Sensor sensor = createSensor(sensorType, axis); | |
| sensor.dataVec[sensorDataIndex] = absCode; | |
| mSensors.emplace(sensorType, sensor); | |
| } else { | |
| it->second.dataVec[sensorDataIndex] = absCode; | |
| } | |
| } | |
| void SensorInputMapper::populateDeviceInfo(InputDeviceInfo* info) { | |
| InputMapper::populateDeviceInfo(info); | |
| for (const auto& [sensorType, sensor] : mSensors) { | |
| info->addSensorInfo(sensor.sensorInfo); | |
| info->setHasSensor(true); | |
| } | |
| } | |
| void SensorInputMapper::dump(std::string& dump) { | |
| dump += INDENT2 "Sensor Input Mapper:\n"; | |
| dump += StringPrintf(INDENT3 " isDeviceEnabled %d\n", getDeviceContext().isDeviceEnabled()); | |
| dump += StringPrintf(INDENT3 " mHasHardwareTimestamp %d\n", mHasHardwareTimestamp); | |
| dump += INDENT3 "Sensors:\n"; | |
| for (const auto& [sensorType, sensor] : mSensors) { | |
| dump += StringPrintf(INDENT4 "%s\n", ftl::enum_string(sensorType).c_str()); | |
| dump += StringPrintf(INDENT5 "enabled: %d\n", sensor.enabled); | |
| dump += StringPrintf(INDENT5 "samplingPeriod: %lld\n", sensor.samplingPeriod.count()); | |
| dump += StringPrintf(INDENT5 "maxBatchReportLatency: %lld\n", | |
| sensor.maxBatchReportLatency.count()); | |
| dump += StringPrintf(INDENT5 "maxRange: %f\n", sensor.sensorInfo.maxRange); | |
| dump += StringPrintf(INDENT5 "power: %f\n", sensor.sensorInfo.power); | |
| for (ssize_t i = 0; i < SENSOR_VEC_LEN; i++) { | |
| int32_t rawAxis = sensor.dataVec[i]; | |
| dump += StringPrintf(INDENT5 "[%zd]: rawAxis: %d \n", i, rawAxis); | |
| const auto it = mAxes.find(rawAxis); | |
| if (it != mAxes.end()) { | |
| const Axis& axis = it->second; | |
| dump += StringPrintf(INDENT5 " min=%0.5f, max=%0.5f, flat=%0.5f, fuzz=%0.5f," | |
| "resolution=%0.5f\n", | |
| axis.min, axis.max, axis.flat, axis.fuzz, axis.resolution); | |
| dump += StringPrintf(INDENT5 " scale=%0.5f, offset=%0.5f\n", axis.scale, | |
| axis.offset); | |
| dump += StringPrintf(INDENT5 " rawMin=%d, rawMax=%d, " | |
| "rawFlat=%d, rawFuzz=%d, rawResolution=%d\n", | |
| axis.rawAxisInfo.minValue, axis.rawAxisInfo.maxValue, | |
| axis.rawAxisInfo.flat, axis.rawAxisInfo.fuzz, | |
| axis.rawAxisInfo.resolution); | |
| } | |
| } | |
| } | |
| } | |
| void SensorInputMapper::configure(nsecs_t when, const InputReaderConfiguration* config, | |
| uint32_t changes) { | |
| InputMapper::configure(when, config, changes); | |
| if (!changes) { // first time only | |
| mDeviceEnabled = true; | |
| // Check if device has MSC_TIMESTAMP event. | |
| mHasHardwareTimestamp = getDeviceContext().hasMscEvent(MSC_TIMESTAMP); | |
| // Collect all axes. | |
| for (int32_t abs = ABS_X; abs <= ABS_MAX; abs++) { | |
| // axis must be claimed by sensor class device | |
| if (!(getAbsAxisUsage(abs, getDeviceContext().getDeviceClasses()) | |
| .test(InputDeviceClass::SENSOR))) { | |
| continue; | |
| } | |
| RawAbsoluteAxisInfo rawAxisInfo; | |
| getAbsoluteAxisInfo(abs, &rawAxisInfo); | |
| if (rawAxisInfo.valid) { | |
| AxisInfo axisInfo; | |
| // Axis doesn't need to be mapped, as sensor mapper doesn't generate any motion | |
| // input events | |
| axisInfo.mode = AxisInfo::MODE_NORMAL; | |
| axisInfo.axis = -1; | |
| // Check key layout map for sensor data mapping to axes | |
| auto ret = getDeviceContext().mapSensor(abs); | |
| if (ret.ok()) { | |
| InputDeviceSensorType sensorType = (*ret).first; | |
| int32_t sensorDataIndex = (*ret).second; | |
| const Axis& axis = createAxis(axisInfo, rawAxisInfo); | |
| parseSensorConfiguration(sensorType, abs, sensorDataIndex, axis); | |
| mAxes.insert({abs, axis}); | |
| } | |
| } | |
| } | |
| } | |
| } | |
| SensorInputMapper::Axis SensorInputMapper::createAxis(const AxisInfo& axisInfo, | |
| const RawAbsoluteAxisInfo& rawAxisInfo) { | |
| // Apply flat override. | |
| int32_t rawFlat = axisInfo.flatOverride < 0 ? rawAxisInfo.flat : axisInfo.flatOverride; | |
| float scale = std::numeric_limits<float>::signaling_NaN(); | |
| float offset = 0; | |
| // resolution is 1 of sensor's unit. For accelerometer, it is G, for gyroscope, | |
| // it is degree/s. | |
| scale = 1.0f / rawAxisInfo.resolution; | |
| offset = avg(rawAxisInfo.minValue, rawAxisInfo.maxValue) * -scale; | |
| const float max = rawAxisInfo.maxValue / rawAxisInfo.resolution; | |
| const float min = rawAxisInfo.minValue / rawAxisInfo.resolution; | |
| const float flat = rawFlat * scale; | |
| const float fuzz = rawAxisInfo.fuzz * scale; | |
| const float resolution = rawAxisInfo.resolution; | |
| // To eliminate noise while the Sensor is at rest, filter out small variations | |
| // in axis values up front. | |
| const float filter = fuzz ? fuzz : flat * 0.25f; | |
| return Axis(rawAxisInfo, axisInfo, scale, offset, min, max, flat, fuzz, resolution, filter); | |
| } | |
| void SensorInputMapper::reset(nsecs_t when) { | |
| // Recenter all axes. | |
| for (std::pair<const int32_t, Axis>& pair : mAxes) { | |
| Axis& axis = pair.second; | |
| axis.resetValue(); | |
| } | |
| mHardwareTimestamp = 0; | |
| mPrevMscTime = 0; | |
| InputMapper::reset(when); | |
| } | |
| SensorInputMapper::Sensor SensorInputMapper::createSensor(InputDeviceSensorType sensorType, | |
| const Axis& axis) { | |
| InputDeviceIdentifier identifier = getDeviceContext().getDeviceIdentifier(); | |
| // Sensor Id will be assigned to device Id to distinguish same sensor from multiple input | |
| // devices, in such a way that the sensor Id will be same as input device Id. | |
| // The sensorType is to distinguish different sensors within one device. | |
| // One input device can only have 1 sensor for each sensor Type. | |
| InputDeviceSensorInfo sensorInfo(identifier.name, std::to_string(identifier.vendor), | |
| identifier.version, sensorType, | |
| InputDeviceSensorAccuracy::ACCURACY_HIGH, | |
| axis.max /* maxRange */, axis.scale /* resolution */, | |
| 0.0f /* power */, 0 /* minDelay */, | |
| 0 /* fifoReservedEventCount */, 0 /* fifoMaxEventCount */, | |
| ftl::enum_string(sensorType), 0 /* maxDelay */, 0 /* flags */, | |
| getDeviceId()); | |
| std::string prefix = "sensor." + ftl::enum_string(sensorType); | |
| transform(prefix.begin(), prefix.end(), prefix.begin(), ::tolower); | |
| int32_t reportingMode = 0; | |
| if (!tryGetProperty(prefix + ".reportingMode", reportingMode)) { | |
| sensorInfo.flags |= (reportingMode & REPORTING_MODE_MASK) << REPORTING_MODE_SHIFT; | |
| } | |
| tryGetProperty(prefix + ".maxDelay", sensorInfo.maxDelay); | |
| tryGetProperty(prefix + ".minDelay", sensorInfo.minDelay); | |
| tryGetProperty(prefix + ".power", sensorInfo.power); | |
| tryGetProperty(prefix + ".fifoReservedEventCount", sensorInfo.fifoReservedEventCount); | |
| tryGetProperty(prefix + ".fifoMaxEventCount", sensorInfo.fifoMaxEventCount); | |
| return Sensor(sensorInfo); | |
| } | |
| void SensorInputMapper::processHardWareTimestamp(nsecs_t evTime, int32_t mscTime) { | |
| // Since MSC_TIMESTAMP initial state is different from the system time, we | |
| // calculate the difference between two MSC_TIMESTAMP events, and use that | |
| // to calculate the system time that should be tagged on the event. | |
| // if the first time MSC_TIMESTAMP, store it | |
| // else calculate difference between previous and current MSC_TIMESTAMP | |
| if (mPrevMscTime == 0) { | |
| mHardwareTimestamp = evTime; | |
| if (DEBUG_SENSOR_EVENT_DETAILS) { | |
| ALOGD("Initialize hardware timestamp = %" PRId64, mHardwareTimestamp); | |
| } | |
| } else { | |
| // Calculate the difference between current msc_timestamp and | |
| // previous msc_timestamp, including when msc_timestamp wraps around. | |
| uint32_t timeDiff = (mPrevMscTime > static_cast<uint32_t>(mscTime)) | |
| ? (UINT32_MAX - mPrevMscTime + static_cast<uint32_t>(mscTime + 1)) | |
| : (static_cast<uint32_t>(mscTime) - mPrevMscTime); | |
| mHardwareTimestamp += timeDiff * 1000LL; | |
| } | |
| mPrevMscTime = static_cast<uint32_t>(mscTime); | |
| } | |
| void SensorInputMapper::process(const RawEvent* rawEvent) { | |
| switch (rawEvent->type) { | |
| case EV_ABS: { | |
| auto it = mAxes.find(rawEvent->code); | |
| if (it != mAxes.end()) { | |
| Axis& axis = it->second; | |
| axis.newValue = rawEvent->value * axis.scale + axis.offset; | |
| } | |
| break; | |
| } | |
| case EV_SYN: | |
| switch (rawEvent->code) { | |
| case SYN_REPORT: | |
| for (std::pair<const int32_t, Axis>& pair : mAxes) { | |
| Axis& axis = pair.second; | |
| axis.currentValue = axis.newValue; | |
| } | |
| sync(rawEvent->when, false /*force*/); | |
| break; | |
| } | |
| break; | |
| case EV_MSC: | |
| switch (rawEvent->code) { | |
| case MSC_TIMESTAMP: | |
| // hardware timestamp is nano seconds | |
| processHardWareTimestamp(rawEvent->when, rawEvent->value); | |
| break; | |
| } | |
| } | |
| } | |
| bool SensorInputMapper::setSensorEnabled(InputDeviceSensorType sensorType, bool enabled) { | |
| auto it = mSensors.find(sensorType); | |
| if (it == mSensors.end()) { | |
| return false; | |
| } | |
| it->second.enabled = enabled; | |
| if (!enabled) { | |
| it->second.resetValue(); | |
| } | |
| /* Currently we can't enable/disable sensors individually. Enabling any sensor will enable | |
| * the device | |
| */ | |
| mDeviceEnabled = false; | |
| for (const auto& [_, sensor] : mSensors) { | |
| // If any sensor is on we will turn on the device. | |
| if (sensor.enabled) { | |
| mDeviceEnabled = true; | |
| break; | |
| } | |
| } | |
| return true; | |
| } | |
| void SensorInputMapper::flushSensor(InputDeviceSensorType sensorType) { | |
| auto it = mSensors.find(sensorType); | |
| if (it == mSensors.end()) { | |
| return; | |
| } | |
| auto& sensor = it->second; | |
| sensor.lastSampleTimeNs = 0; | |
| for (size_t i = 0; i < SENSOR_VEC_LEN; i++) { | |
| int32_t abs = sensor.dataVec[i]; | |
| auto itAxis = mAxes.find(abs); | |
| if (itAxis != mAxes.end()) { | |
| Axis& axis = itAxis->second; | |
| axis.resetValue(); | |
| } | |
| } | |
| } | |
| bool SensorInputMapper::enableSensor(InputDeviceSensorType sensorType, | |
| std::chrono::microseconds samplingPeriod, | |
| std::chrono::microseconds maxBatchReportLatency) { | |
| if (DEBUG_SENSOR_EVENT_DETAILS) { | |
| ALOGD("Enable Sensor %s samplingPeriod %lld maxBatchReportLatency %lld", | |
| ftl::enum_string(sensorType).c_str(), samplingPeriod.count(), | |
| maxBatchReportLatency.count()); | |
| } | |
| if (!setSensorEnabled(sensorType, true /* enabled */)) { | |
| return false; | |
| } | |
| // Enable device | |
| if (mDeviceEnabled) { | |
| getDeviceContext().enableDevice(); | |
| } | |
| // We know the sensor exists now, update the sampling period and batch report latency. | |
| auto it = mSensors.find(sensorType); | |
| it->second.samplingPeriod = | |
| std::chrono::duration_cast<std::chrono::nanoseconds>(samplingPeriod); | |
| it->second.maxBatchReportLatency = | |
| std::chrono::duration_cast<std::chrono::nanoseconds>(maxBatchReportLatency); | |
| return true; | |
| } | |
| void SensorInputMapper::disableSensor(InputDeviceSensorType sensorType) { | |
| if (DEBUG_SENSOR_EVENT_DETAILS) { | |
| ALOGD("Disable Sensor %s", ftl::enum_string(sensorType).c_str()); | |
| } | |
| if (!setSensorEnabled(sensorType, false /* enabled */)) { | |
| return; | |
| } | |
| // Disable device | |
| if (!mDeviceEnabled) { | |
| mHardwareTimestamp = 0; | |
| mPrevMscTime = 0; | |
| getDeviceContext().disableDevice(); | |
| } | |
| } | |
| void SensorInputMapper::sync(nsecs_t when, bool force) { | |
| for (auto& [sensorType, sensor] : mSensors) { | |
| // Skip if sensor not enabled | |
| if (!sensor.enabled) { | |
| continue; | |
| } | |
| std::vector<float> values; | |
| for (ssize_t i = 0; i < SENSOR_VEC_LEN; i++) { | |
| int32_t abs = sensor.dataVec[i]; | |
| auto it = mAxes.find(abs); | |
| if (it != mAxes.end()) { | |
| const Axis& axis = it->second; | |
| values.push_back(axis.currentValue); | |
| } | |
| } | |
| nsecs_t timestamp = mHasHardwareTimestamp ? mHardwareTimestamp : when; | |
| if (DEBUG_SENSOR_EVENT_DETAILS) { | |
| ALOGD("Sensor %s timestamp %" PRIu64 " values [%f %f %f]", | |
| ftl::enum_string(sensorType).c_str(), timestamp, values[0], values[1], values[2]); | |
| } | |
| if (sensor.lastSampleTimeNs.has_value() && | |
| timestamp - sensor.lastSampleTimeNs.value() < sensor.samplingPeriod.count()) { | |
| if (DEBUG_SENSOR_EVENT_DETAILS) { | |
| ALOGD("Sensor %s Skip a sample.", ftl::enum_string(sensorType).c_str()); | |
| } | |
| } else { | |
| // Convert to Android unit | |
| convertFromLinuxToAndroid(values, sensorType); | |
| // Notify dispatcher for sensor event | |
| NotifySensorArgs args(getContext()->getNextId(), when, getDeviceId(), | |
| AINPUT_SOURCE_SENSOR, sensorType, sensor.sensorInfo.accuracy, | |
| sensor.accuracy != | |
| sensor.sensorInfo.accuracy /* accuracyChanged */, | |
| timestamp /* hwTimestamp */, values); | |
| getListener().notifySensor(&args); | |
| sensor.lastSampleTimeNs = timestamp; | |
| sensor.accuracy = sensor.sensorInfo.accuracy; | |
| } | |
| } | |
| } | |
| } // namespace android |