495 lines
18 KiB
C++
495 lines
18 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.
|
|
*/
|
|
#define LOG_TAG "GoogleIIOSensorSubHal"
|
|
|
|
#include "Sensor.h"
|
|
#include <hardware/sensors.h>
|
|
#include <log/log.h>
|
|
#include <utils/SystemClock.h>
|
|
#include <cmath>
|
|
|
|
namespace android {
|
|
namespace hardware {
|
|
namespace sensors {
|
|
namespace V2_0 {
|
|
namespace subhal {
|
|
namespace implementation {
|
|
|
|
using ::android::hardware::sensors::V1_0::AdditionalInfoType;
|
|
using ::android::hardware::sensors::V1_0::MetaDataEventType;
|
|
using ::android::hardware::sensors::V1_0::SensorFlagBits;
|
|
using ::android::hardware::sensors::V1_0::SensorStatus;
|
|
using ::sensor::hal::configuration::V1_0::Location;
|
|
using ::sensor::hal::configuration::V1_0::Orientation;
|
|
|
|
SensorBase::SensorBase(int32_t sensorHandle, ISensorsEventCallback* callback, SensorType type)
|
|
: mIsEnabled(false),
|
|
mSamplingPeriodNs(0),
|
|
mCallback(callback),
|
|
mMode(OperationMode::NORMAL),
|
|
mSensorThread(this) {
|
|
mSensorInfo.type = type;
|
|
mSensorInfo.sensorHandle = sensorHandle;
|
|
mSensorInfo.vendor = "Google";
|
|
mSensorInfo.version = 1;
|
|
mSensorInfo.fifoReservedEventCount = 0;
|
|
mSensorInfo.fifoMaxEventCount = 0;
|
|
mSensorInfo.requiredPermission = "";
|
|
mSensorInfo.flags = 0;
|
|
|
|
switch (type) {
|
|
case SensorType::ACCELEROMETER:
|
|
mSensorInfo.typeAsString = SENSOR_STRING_TYPE_ACCELEROMETER;
|
|
break;
|
|
case SensorType::GYROSCOPE:
|
|
mSensorInfo.typeAsString = SENSOR_STRING_TYPE_GYROSCOPE;
|
|
break;
|
|
default:
|
|
ALOGE("unsupported sensor type %d", type);
|
|
break;
|
|
}
|
|
|
|
mSensorThread.start();
|
|
}
|
|
|
|
SensorBase::~SensorBase() {
|
|
mIsEnabled = false;
|
|
}
|
|
|
|
bool SensorBase::isEnabled() const {
|
|
return mIsEnabled;
|
|
}
|
|
|
|
OperationMode SensorBase::getOperationMode() const {
|
|
return mMode;
|
|
}
|
|
|
|
HWSensorBase::~HWSensorBase() {
|
|
close(mPollFdIio.fd);
|
|
}
|
|
|
|
const SensorInfo& SensorBase::getSensorInfo() const {
|
|
return mSensorInfo;
|
|
}
|
|
|
|
void HWSensorBase::batch(int32_t samplingPeriodNs) {
|
|
samplingPeriodNs =
|
|
std::clamp(samplingPeriodNs, mSensorInfo.minDelay * 1000, mSensorInfo.maxDelay * 1000);
|
|
if (mSamplingPeriodNs != samplingPeriodNs) {
|
|
unsigned int sampling_frequency = ns_to_frequency(samplingPeriodNs);
|
|
int i = 0;
|
|
mSamplingPeriodNs = samplingPeriodNs;
|
|
std::vector<double>::iterator low =
|
|
std::lower_bound(mIioData.sampling_freq_avl.begin(),
|
|
mIioData.sampling_freq_avl.end(), sampling_frequency);
|
|
i = low - mIioData.sampling_freq_avl.begin();
|
|
set_sampling_frequency(mIioData.sysfspath, mIioData.sampling_freq_avl[i]);
|
|
// Wake up the 'run' thread to check if a new event should be generated now
|
|
mSensorThread.notifyAll();
|
|
}
|
|
}
|
|
|
|
void HWSensorBase::sendAdditionalInfoReport() {
|
|
std::vector<Event> events;
|
|
|
|
for (const auto& frame : mAdditionalInfoFrames) {
|
|
events.emplace_back(Event{
|
|
.sensorHandle = mSensorInfo.sensorHandle,
|
|
.sensorType = SensorType::ADDITIONAL_INFO,
|
|
.timestamp = android::elapsedRealtimeNano(),
|
|
.u.additional = frame,
|
|
});
|
|
}
|
|
|
|
if (!events.empty()) mCallback->postEvents(events, isWakeUpSensor());
|
|
}
|
|
|
|
void HWSensorBase::activate(bool enable) {
|
|
std::unique_lock<std::mutex> lock(mSensorThread.lock());
|
|
if (mIsEnabled != enable) {
|
|
mIsEnabled = enable;
|
|
enable_sensor(mIioData.sysfspath, enable);
|
|
if (enable) sendAdditionalInfoReport();
|
|
mSensorThread.notifyAll();
|
|
}
|
|
}
|
|
|
|
Result SensorBase::flush() {
|
|
// Only generate a flush complete event if the sensor is enabled and if the sensor is not a
|
|
// one-shot sensor.
|
|
if (!mIsEnabled || (mSensorInfo.flags & static_cast<uint32_t>(SensorFlagBits::ONE_SHOT_MODE))) {
|
|
return Result::BAD_VALUE;
|
|
}
|
|
|
|
// Note: If a sensor supports batching, write all of the currently batched events for the sensor
|
|
// to the Event FMQ prior to writing the flush complete event.
|
|
Event ev;
|
|
ev.sensorHandle = mSensorInfo.sensorHandle;
|
|
ev.sensorType = SensorType::META_DATA;
|
|
ev.u.meta.what = MetaDataEventType::META_DATA_FLUSH_COMPLETE;
|
|
std::vector<Event> evs{ev};
|
|
mCallback->postEvents(evs, isWakeUpSensor());
|
|
return Result::OK;
|
|
}
|
|
|
|
Result HWSensorBase::flush() {
|
|
Result result = Result::OK;
|
|
result = SensorBase::flush();
|
|
if (result == Result::OK) sendAdditionalInfoReport();
|
|
return result;
|
|
}
|
|
|
|
template <size_t N>
|
|
static float getChannelData(const std::array<float, N>& channelData, int64_t map, bool negate) {
|
|
return negate ? -channelData[map] : channelData[map];
|
|
}
|
|
|
|
void HWSensorBase::processScanData(uint8_t* data, Event* evt) {
|
|
std::array<float, NUM_OF_DATA_CHANNELS> channelData;
|
|
unsigned int chanIdx;
|
|
evt->sensorHandle = mSensorInfo.sensorHandle;
|
|
evt->sensorType = mSensorInfo.type;
|
|
for (auto i = 0u; i < mIioData.channelInfo.size(); i++) {
|
|
chanIdx = mIioData.channelInfo[i].index;
|
|
|
|
const int64_t val =
|
|
*reinterpret_cast<int64_t*>(data + chanIdx * mIioData.channelInfo[i].storage_bytes);
|
|
// If the channel index is the last, it is timestamp
|
|
// else it is sensor data
|
|
if (chanIdx == mIioData.channelInfo.size() - 1) {
|
|
evt->timestamp = val;
|
|
} else {
|
|
channelData[chanIdx] = static_cast<float>(val) * mIioData.scale;
|
|
}
|
|
}
|
|
|
|
evt->u.vec3.x = getChannelData(channelData, mXMap, mXNegate);
|
|
evt->u.vec3.y = getChannelData(channelData, mYMap, mYNegate);
|
|
evt->u.vec3.z = getChannelData(channelData, mZMap, mZNegate);
|
|
evt->u.vec3.status = SensorStatus::ACCURACY_HIGH;
|
|
}
|
|
|
|
void HWSensorBase::pollForEvents() {
|
|
int err = poll(&mPollFdIio, 1, mSamplingPeriodNs * 1000);
|
|
if (err <= 0) {
|
|
ALOGE("Sensor %s poll returned %d", mIioData.name.c_str(), err);
|
|
return;
|
|
}
|
|
|
|
if (mPollFdIio.revents & POLLIN) {
|
|
int read_size = read(mPollFdIio.fd, &mSensorRawData[0], mScanSize);
|
|
if (read_size <= 0) {
|
|
ALOGE("%s: Failed to read data from iio char device.", mIioData.name.c_str());
|
|
return;
|
|
}
|
|
|
|
Event evt;
|
|
processScanData(&mSensorRawData[0], &evt);
|
|
mCallback->postEvents({evt}, isWakeUpSensor());
|
|
}
|
|
}
|
|
|
|
void HWSensorBase::idleLoop() {
|
|
mSensorThread.wait([this] {
|
|
return ((mIsEnabled && mMode == OperationMode::NORMAL) || mSensorThread.isStopped());
|
|
});
|
|
}
|
|
|
|
void HWSensorBase::pollSensor() {
|
|
if (!mIsEnabled || mMode == OperationMode::DATA_INJECTION) {
|
|
idleLoop();
|
|
} else {
|
|
pollForEvents();
|
|
}
|
|
}
|
|
|
|
bool SensorBase::isWakeUpSensor() {
|
|
return mSensorInfo.flags & static_cast<uint32_t>(SensorFlagBits::WAKE_UP);
|
|
}
|
|
|
|
void SensorBase::setOperationMode(OperationMode mode) {
|
|
std::unique_lock<std::mutex> lock(mSensorThread.lock());
|
|
if (mMode != mode) {
|
|
mMode = mode;
|
|
mSensorThread.notifyAll();
|
|
}
|
|
}
|
|
|
|
bool SensorBase::supportsDataInjection() const {
|
|
return mSensorInfo.flags & static_cast<uint32_t>(SensorFlagBits::DATA_INJECTION);
|
|
}
|
|
|
|
Result SensorBase::injectEvent(const Event& event) {
|
|
Result result = Result::OK;
|
|
if (event.sensorType == SensorType::ADDITIONAL_INFO) {
|
|
// When in OperationMode::NORMAL, SensorType::ADDITIONAL_INFO is used to push operation
|
|
// environment data into the device.
|
|
} else if (!supportsDataInjection()) {
|
|
result = Result::INVALID_OPERATION;
|
|
} else if (mMode == OperationMode::DATA_INJECTION) {
|
|
mCallback->postEvents(std::vector<Event>{event}, isWakeUpSensor());
|
|
} else {
|
|
result = Result::BAD_VALUE;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ssize_t HWSensorBase::calculateScanSize() {
|
|
ssize_t numBytes = 0;
|
|
for (auto i = 0u; i < mIioData.channelInfo.size(); i++) {
|
|
numBytes += mIioData.channelInfo[i].storage_bytes;
|
|
}
|
|
return numBytes;
|
|
}
|
|
|
|
static status_t checkAxis(int64_t map) {
|
|
if (map < 0 || map >= NUM_OF_DATA_CHANNELS)
|
|
return BAD_VALUE;
|
|
else
|
|
return OK;
|
|
}
|
|
|
|
static std::optional<std::vector<Orientation>> getOrientation(
|
|
std::optional<std::vector<Configuration>> config) {
|
|
if (!config) return std::nullopt;
|
|
if (config->empty()) return std::nullopt;
|
|
Configuration& sensorCfg = (*config)[0];
|
|
return sensorCfg.getOrientation();
|
|
}
|
|
|
|
static std::optional<std::vector<Location>> getLocation(
|
|
std::optional<std::vector<Configuration>> config) {
|
|
if (!config) return std::nullopt;
|
|
if (config->empty()) return std::nullopt;
|
|
Configuration& sensorCfg = (*config)[0];
|
|
return sensorCfg.getLocation();
|
|
}
|
|
|
|
static status_t checkOrientation(std::optional<std::vector<Configuration>> config) {
|
|
status_t ret = OK;
|
|
std::optional<std::vector<Orientation>> sensorOrientationList = getOrientation(config);
|
|
if (!sensorOrientationList) return OK;
|
|
if (sensorOrientationList->empty()) return OK;
|
|
Orientation& sensorOrientation = (*sensorOrientationList)[0];
|
|
if (!sensorOrientation.getFirstX() || !sensorOrientation.getFirstY() ||
|
|
!sensorOrientation.getFirstZ())
|
|
return BAD_VALUE;
|
|
|
|
int64_t xMap = sensorOrientation.getFirstX()->getMap();
|
|
ret = checkAxis(xMap);
|
|
if (ret != OK) return ret;
|
|
int64_t yMap = sensorOrientation.getFirstY()->getMap();
|
|
ret = checkAxis(yMap);
|
|
if (ret != OK) return ret;
|
|
int64_t zMap = sensorOrientation.getFirstZ()->getMap();
|
|
ret = checkAxis(zMap);
|
|
if (ret != OK) return ret;
|
|
if (xMap == yMap || yMap == zMap || zMap == xMap) return BAD_VALUE;
|
|
return ret;
|
|
}
|
|
|
|
void HWSensorBase::setAxisDefaultValues() {
|
|
mXMap = 0;
|
|
mYMap = 1;
|
|
mZMap = 2;
|
|
mXNegate = mYNegate = mZNegate = false;
|
|
}
|
|
void HWSensorBase::setOrientation(std::optional<std::vector<Configuration>> config) {
|
|
std::optional<std::vector<Orientation>> sensorOrientationList = getOrientation(config);
|
|
|
|
if (sensorOrientationList && !sensorOrientationList->empty()) {
|
|
Orientation& sensorOrientation = (*sensorOrientationList)[0];
|
|
|
|
if (sensorOrientation.getRotate()) {
|
|
mXMap = sensorOrientation.getFirstX()->getMap();
|
|
mXNegate = sensorOrientation.getFirstX()->getNegate();
|
|
mYMap = sensorOrientation.getFirstY()->getMap();
|
|
mYNegate = sensorOrientation.getFirstY()->getNegate();
|
|
mZMap = sensorOrientation.getFirstZ()->getMap();
|
|
mZNegate = sensorOrientation.getFirstZ()->getNegate();
|
|
} else {
|
|
setAxisDefaultValues();
|
|
}
|
|
} else {
|
|
setAxisDefaultValues();
|
|
}
|
|
}
|
|
|
|
static status_t checkIIOData(const struct iio_device_data& iio_data) {
|
|
status_t ret = OK;
|
|
for (auto i = 0u; i < iio_data.channelInfo.size(); i++) {
|
|
if (iio_data.channelInfo[i].index > NUM_OF_DATA_CHANNELS) return BAD_VALUE;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
static status_t setSensorPlacementData(AdditionalInfo* sensorPlacement, int index, float value) {
|
|
if (!sensorPlacement) return BAD_VALUE;
|
|
|
|
int arraySize =
|
|
sizeof(sensorPlacement->u.data_float) / sizeof(sensorPlacement->u.data_float[0]);
|
|
if (index < 0 || index >= arraySize) return BAD_VALUE;
|
|
|
|
sensorPlacement->u.data_float[index] = value;
|
|
return OK;
|
|
}
|
|
|
|
status_t HWSensorBase::getSensorPlacement(AdditionalInfo* sensorPlacement,
|
|
const std::optional<std::vector<Configuration>>& config) {
|
|
if (!sensorPlacement) return BAD_VALUE;
|
|
|
|
auto sensorLocationList = getLocation(config);
|
|
if (!sensorLocationList) return BAD_VALUE;
|
|
if (sensorLocationList->empty()) return BAD_VALUE;
|
|
|
|
auto sensorOrientationList = getOrientation(config);
|
|
if (!sensorOrientationList) return BAD_VALUE;
|
|
if (sensorOrientationList->empty()) return BAD_VALUE;
|
|
|
|
sensorPlacement->type = AdditionalInfoType::AINFO_SENSOR_PLACEMENT;
|
|
sensorPlacement->serial = 0;
|
|
memset(&sensorPlacement->u.data_float, 0, sizeof(sensorPlacement->u.data_float));
|
|
|
|
Location& sensorLocation = (*sensorLocationList)[0];
|
|
// SensorPlacementData is given as a 3x4 matrix consisting of a 3x3 rotation matrix (R)
|
|
// concatenated with a 3x1 location vector (t) in row major order. Example: This raw buffer:
|
|
// {x1,y1,z1,l1,x2,y2,z2,l2,x3,y3,z3,l3} corresponds to the following 3x4 matrix:
|
|
// x1 y1 z1 l1
|
|
// x2 y2 z2 l2
|
|
// x3 y3 z3 l3
|
|
// LOCATION_X_IDX,LOCATION_Y_IDX,LOCATION_Z_IDX corresponds to the indexes of the location
|
|
// vector (l1,l2,l3) in the raw buffer.
|
|
status_t ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_X_IDX,
|
|
sensorLocation.getX());
|
|
if (ret != OK) return ret;
|
|
ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_Y_IDX,
|
|
sensorLocation.getY());
|
|
if (ret != OK) return ret;
|
|
ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_Z_IDX,
|
|
sensorLocation.getZ());
|
|
if (ret != OK) return ret;
|
|
|
|
Orientation& sensorOrientation = (*sensorOrientationList)[0];
|
|
if (sensorOrientation.getRotate()) {
|
|
// If the HAL is already rotating the sensor orientation to align with the Android
|
|
// Coordinate system, then the sensor rotation matrix will be an identity matrix
|
|
// ROTATION_X_IDX, ROTATION_Y_IDX, ROTATION_Z_IDX corresponds to indexes of the
|
|
// (x1,y1,z1) in the raw buffer.
|
|
ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_X_IDX + 0, 1);
|
|
if (ret != OK) return ret;
|
|
ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_Y_IDX + 4, 1);
|
|
if (ret != OK) return ret;
|
|
ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_Z_IDX + 8, 1);
|
|
if (ret != OK) return ret;
|
|
} else {
|
|
ret = setSensorPlacementData(
|
|
sensorPlacement,
|
|
HWSensorBase::ROTATION_X_IDX + 4 * sensorOrientation.getFirstX()->getMap(),
|
|
sensorOrientation.getFirstX()->getNegate() ? -1 : 1);
|
|
if (ret != OK) return ret;
|
|
ret = setSensorPlacementData(
|
|
sensorPlacement,
|
|
HWSensorBase::ROTATION_Y_IDX + 4 * sensorOrientation.getFirstY()->getMap(),
|
|
sensorOrientation.getFirstY()->getNegate() ? -1 : 1);
|
|
if (ret != OK) return ret;
|
|
ret = setSensorPlacementData(
|
|
sensorPlacement,
|
|
HWSensorBase::ROTATION_Z_IDX + 4 * sensorOrientation.getFirstZ()->getMap(),
|
|
sensorOrientation.getFirstZ()->getNegate() ? -1 : 1);
|
|
if (ret != OK) return ret;
|
|
}
|
|
return OK;
|
|
}
|
|
|
|
status_t HWSensorBase::setAdditionalInfoFrames(
|
|
const std::optional<std::vector<Configuration>>& config) {
|
|
AdditionalInfo additionalInfoSensorPlacement;
|
|
status_t ret = getSensorPlacement(&additionalInfoSensorPlacement, config);
|
|
if (ret != OK) return ret;
|
|
|
|
const AdditionalInfo additionalInfoBegin = {
|
|
.type = AdditionalInfoType::AINFO_BEGIN,
|
|
.serial = 0,
|
|
};
|
|
const AdditionalInfo additionalInfoEnd = {
|
|
.type = AdditionalInfoType::AINFO_END,
|
|
.serial = 0,
|
|
};
|
|
|
|
mAdditionalInfoFrames.insert(
|
|
mAdditionalInfoFrames.end(),
|
|
{additionalInfoBegin, additionalInfoSensorPlacement, additionalInfoEnd});
|
|
return OK;
|
|
}
|
|
|
|
HWSensorBase* HWSensorBase::buildSensor(int32_t sensorHandle, ISensorsEventCallback* callback,
|
|
const struct iio_device_data& iio_data,
|
|
const std::optional<std::vector<Configuration>>& config) {
|
|
if (checkOrientation(config) != OK) {
|
|
ALOGE("Orientation of the sensor %s in the configuration file is invalid",
|
|
iio_data.name.c_str());
|
|
return nullptr;
|
|
}
|
|
if (checkIIOData(iio_data) != OK) {
|
|
ALOGE("IIO channel index of the sensor %s is invalid", iio_data.name.c_str());
|
|
return nullptr;
|
|
}
|
|
return new HWSensorBase(sensorHandle, callback, iio_data, config);
|
|
}
|
|
|
|
HWSensorBase::HWSensorBase(int32_t sensorHandle, ISensorsEventCallback* callback,
|
|
const struct iio_device_data& data,
|
|
const std::optional<std::vector<Configuration>>& config)
|
|
: SensorBase(sensorHandle, callback, data.type) {
|
|
std::string buffer_path;
|
|
mSensorInfo.flags |= SensorFlagBits::CONTINUOUS_MODE;
|
|
mSensorInfo.name = data.name;
|
|
mSensorInfo.resolution = data.resolution * data.scale;
|
|
mSensorInfo.maxRange = data.max_range * data.scale;
|
|
mSensorInfo.power = 0;
|
|
mIioData = data;
|
|
setOrientation(config);
|
|
status_t ret = setAdditionalInfoFrames(config);
|
|
if (ret == OK) mSensorInfo.flags |= SensorFlagBits::ADDITIONAL_INFO;
|
|
unsigned int max_sampling_frequency = 0;
|
|
unsigned int min_sampling_frequency = UINT_MAX;
|
|
for (auto i = 0u; i < data.sampling_freq_avl.size(); i++) {
|
|
if (max_sampling_frequency < data.sampling_freq_avl[i])
|
|
max_sampling_frequency = data.sampling_freq_avl[i];
|
|
if (min_sampling_frequency > data.sampling_freq_avl[i])
|
|
min_sampling_frequency = data.sampling_freq_avl[i];
|
|
}
|
|
mSensorInfo.minDelay = frequency_to_us(max_sampling_frequency);
|
|
mSensorInfo.maxDelay = frequency_to_us(min_sampling_frequency);
|
|
mScanSize = calculateScanSize();
|
|
buffer_path = "/dev/iio:device";
|
|
buffer_path.append(std::to_string(mIioData.iio_dev_num));
|
|
mPollFdIio.fd = open(buffer_path.c_str(), O_RDONLY | O_NONBLOCK);
|
|
if (mPollFdIio.fd < 0) {
|
|
ALOGE("%s: Failed to open iio char device (%s).", data.name.c_str(), buffer_path.c_str());
|
|
return;
|
|
}
|
|
mPollFdIio.events = POLLIN;
|
|
mSensorRawData.resize(mScanSize);
|
|
}
|
|
|
|
} // namespace implementation
|
|
} // namespace subhal
|
|
} // namespace V2_0
|
|
} // namespace sensors
|
|
} // namespace hardware
|
|
} // namespace android
|