blob: 8a2902708e7daa62b6f0dab0a157e100d96242ed [file] [log] [blame]
/*
* Copyright (C) 2021 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 <media/SensorPoseProvider.h>
#define LOG_TAG "SensorPoseProvider"
#include <algorithm>
#include <future>
#include <inttypes.h>
#include <limits>
#include <map>
#include <thread>
#include <android-base/stringprintf.h>
#include <android-base/thread_annotations.h>
#include <log/log_main.h>
#include <sensor/SensorEventQueue.h>
#include <sensor/SensorManager.h>
#include <utils/Looper.h>
#include "media/QuaternionUtil.h"
namespace android {
namespace media {
namespace {
using android::base::StringAppendF;
// Identifier to use for our event queue on the loop.
// The number 19 is arbitrary, only useful if using multiple objects on the same looper.
// Note: Instead of a fixed number, the SensorEventQueue's fd could be used instead.
constexpr int kIdent = 19;
static inline Looper* ALooper_to_Looper(ALooper* alooper) {
return reinterpret_cast<Looper*>(alooper);
}
static inline ALooper* Looper_to_ALooper(Looper* looper) {
return reinterpret_cast<ALooper*>(looper);
}
/**
* RAII-wrapper around SensorEventQueue, which unregisters it on destruction.
*/
class EventQueueGuard {
public:
EventQueueGuard(const sp<SensorEventQueue>& queue, Looper* looper) : mQueue(queue) {
mQueue->looper = Looper_to_ALooper(looper);
mQueue->requestAdditionalInfo = false;
looper->addFd(mQueue->getFd(), kIdent, ALOOPER_EVENT_INPUT,
nullptr /* callback */, nullptr /* data */);
}
~EventQueueGuard() {
if (mQueue) {
ALooper_to_Looper(mQueue->looper)->removeFd(mQueue->getFd());
}
}
EventQueueGuard(const EventQueueGuard&) = delete;
EventQueueGuard& operator=(const EventQueueGuard&) = delete;
[[nodiscard]] SensorEventQueue* get() const { return mQueue.get(); }
private:
const sp<SensorEventQueue> mQueue;
};
/**
* RAII-wrapper around an enabled sensor, which disables it upon destruction.
*/
class SensorEnableGuard {
public:
SensorEnableGuard(const sp<SensorEventQueue>& queue, int32_t sensor)
: mQueue(queue), mSensor(sensor) {}
~SensorEnableGuard() {
if (mSensor != SensorPoseProvider::INVALID_HANDLE) {
int ret = mQueue->disableSensor(mSensor);
if (ret) {
ALOGE("Failed to disable sensor: %s", strerror(ret));
}
}
}
// Enable move and delete default copy-ctor/copy-assignment.
SensorEnableGuard(SensorEnableGuard&& other) : mQueue(other.mQueue), mSensor(other.mSensor) {
other.mSensor = SensorPoseProvider::INVALID_HANDLE;
}
private:
sp<SensorEventQueue> const mQueue;
int32_t mSensor;
};
/**
* Streams the required events to a PoseListener, based on events originating from the Sensor stack.
*/
class SensorPoseProviderImpl : public SensorPoseProvider {
public:
static std::unique_ptr<SensorPoseProvider> create(const char* packageName, Listener* listener) {
std::unique_ptr<SensorPoseProviderImpl> result(
new SensorPoseProviderImpl(packageName, listener));
return result->waitInitFinished() ? std::move(result) : nullptr;
}
~SensorPoseProviderImpl() override {
// Disable all active sensors.
mEnabledSensors.clear();
mQuit = true;
mLooper->wake();
mThread.join();
}
bool startSensor(int32_t sensor, std::chrono::microseconds samplingPeriod) override {
// Figure out the sensor's data format.
DataFormat format = getSensorFormat(sensor);
if (format == DataFormat::kUnknown) {
ALOGE("%s: Unknown format for sensor %" PRId32, __func__, sensor);
return false;
}
{
std::lock_guard lock(mMutex);
mEnabledSensorsExtra.emplace(
sensor,
SensorExtra{.format = format,
.samplingPeriod = static_cast<int32_t>(samplingPeriod.count())});
}
// Enable the sensor.
if (mQueue->enableSensor(sensor, samplingPeriod.count(), 0, 0)) {
ALOGE("%s: Failed to enable sensor %" PRId32, __func__, sensor);
std::lock_guard lock(mMutex);
mEnabledSensorsExtra.erase(sensor);
return false;
}
mEnabledSensors.emplace(sensor, SensorEnableGuard(mQueue, sensor));
ALOGD("%s: Sensor %" PRId32 " started", __func__, sensor);
return true;
}
void stopSensor(int handle) override {
ALOGD("%s: Sensor %" PRId32 " stopped", __func__, handle);
mEnabledSensors.erase(handle);
std::lock_guard lock(mMutex);
mEnabledSensorsExtra.erase(handle);
}
std::string toString(unsigned level) override {
std::string prefixSpace(level, ' ');
std::string ss = prefixSpace + "SensorPoseProvider:\n";
bool needUnlock = false;
prefixSpace += " ";
auto now = std::chrono::steady_clock::now();
if (!mMutex.try_lock_until(now + media::kSpatializerDumpSysTimeOutInSecond)) {
ss.append(prefixSpace).append("try_lock failed, dumpsys below maybe INACCURATE!\n");
} else {
needUnlock = true;
}
// Enabled sensor information
StringAppendF(&ss, "%sSensors total number %zu:\n", prefixSpace.c_str(),
mEnabledSensorsExtra.size());
for (auto sensor : mEnabledSensorsExtra) {
StringAppendF(&ss,
"%s[Handle: 0x%08x, Format %s Period (set %d max %0.4f min %0.4f) ms",
prefixSpace.c_str(), sensor.first, toString(sensor.second.format).c_str(),
sensor.second.samplingPeriod, media::nsToFloatMs(sensor.second.maxPeriod),
media::nsToFloatMs(sensor.second.minPeriod));
if (sensor.second.discontinuityCount.has_value()) {
StringAppendF(&ss, ", DiscontinuityCount: %d",
sensor.second.discontinuityCount.value());
}
ss += "]\n";
}
if (needUnlock) {
mMutex.unlock();
}
return ss;
}
private:
enum DataFormat {
kUnknown,
kQuaternion,
kRotationVectorsAndDiscontinuityCount,
};
struct PoseEvent {
Pose3f pose;
std::optional<Twist3f> twist;
bool isNewReference;
};
struct SensorExtra {
DataFormat format = DataFormat::kUnknown;
int32_t samplingPeriod = 0;
int64_t latestTimestamp = 0;
int64_t maxPeriod = 0;
int64_t minPeriod = std::numeric_limits<int64_t>::max();
std::optional<int32_t> discontinuityCount;
};
bool mQuit = false;
sp<Looper> mLooper;
Listener* const mListener;
SensorManager* const mSensorManager;
std::timed_mutex mMutex;
sp<SensorEventQueue> mQueue;
std::map<int32_t, SensorEnableGuard> mEnabledSensors;
std::map<int32_t, SensorExtra> mEnabledSensorsExtra GUARDED_BY(mMutex);
// We must do some of the initialization operations on the worker thread, because the API relies
// on the thread-local looper. In addition, as a matter of convenience, we store some of the
// state on the stack.
// For that reason, we use a two-step initialization approach, where the ctor mostly just starts
// the worker thread and that thread would notify, via the promise below whenever initialization
// is finished, and whether it was successful.
std::promise<bool> mInitPromise;
std::thread mThread;
SensorPoseProviderImpl(const char* packageName, Listener* listener)
: mListener(listener),
mSensorManager(&SensorManager::getInstanceForPackage(String16(packageName))) {
mThread = std::thread([this] { threadFunc(); });
}
void initFinished(bool success) { mInitPromise.set_value(success); }
bool waitInitFinished() { return mInitPromise.get_future().get(); }
void threadFunc() {
// Name our std::thread to help identification. As is, canCallJava == false.
androidSetThreadName("SensorPoseProvider-looper");
// Run at the highest non-realtime priority.
androidSetThreadPriority(gettid(), PRIORITY_URGENT_AUDIO);
// The looper is started on the created std::thread.
mLooper = Looper::prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
// Create event queue.
mQueue = mSensorManager->createEventQueue();
if (mQueue == nullptr) {
ALOGE("Failed to create a sensor event queue");
initFinished(false);
return;
}
EventQueueGuard eventQueueGuard(mQueue, mLooper.get());
initFinished(true);
while (!mQuit) {
const int ret = mLooper->pollOnce(-1 /* no timeout */, nullptr /* outFd */,
nullptr /* outEvents */, nullptr /* outData */);
switch (ret) {
case ALOOPER_POLL_WAKE:
// Continue to see if mQuit flag is set.
// This can be spurious (due to bugreport being taken).
continue;
case kIdent:
// Possible events on our queue.
break;
default:
// Besides WAKE and kIdent, there should be no timeouts, callbacks,
// ALOOPER_POLL_ERROR, or other events.
// Exit now to avoid high frequency log spam on error,
// e.g. if the fd becomes invalid (b/31093485).
ALOGE("%s: Unexpected status out of Looper::pollOnce: %d", __func__, ret);
mQuit = true;
continue;
}
// Process an event.
ASensorEvent event;
ssize_t actual = mQueue->read(&event, 1);
if (actual > 0) {
mQueue->sendAck(&event, actual);
}
ssize_t size = mQueue->filterEvents(&event, actual);
if (size < 0 || size > 1) {
ALOGE("%s: Unexpected return value from SensorEventQueue::filterEvents: %zd",
__func__, size);
break;
}
if (size == 0) {
// No events.
continue;
}
handleEvent(event);
}
ALOGD("%s: Exiting sensor event loop", __func__);
}
void handleEvent(const ASensorEvent& event) {
PoseEvent value;
{
std::lock_guard lock(mMutex);
auto iter = mEnabledSensorsExtra.find(event.sensor);
if (iter == mEnabledSensorsExtra.end()) {
// This can happen if we have any pending events shortly after stopping.
return;
}
value = parseEvent(event, iter->second.format, &iter->second.discontinuityCount);
updateEventTimestamp(event, iter->second);
}
mListener->onPose(event.timestamp, event.sensor, value.pose, value.twist,
value.isNewReference);
}
DataFormat getSensorFormat(int32_t handle) {
std::optional<const Sensor> sensor = getSensorByHandle(handle);
if (!sensor) {
ALOGE("Sensor not found: %d", handle);
return DataFormat::kUnknown;
}
if (sensor->getType() == ASENSOR_TYPE_ROTATION_VECTOR ||
sensor->getType() == ASENSOR_TYPE_GAME_ROTATION_VECTOR) {
return DataFormat::kQuaternion;
}
if (sensor->getType() == ASENSOR_TYPE_HEAD_TRACKER) {
return DataFormat::kRotationVectorsAndDiscontinuityCount;
}
return DataFormat::kUnknown;
}
std::optional<const Sensor> getSensorByHandle(int32_t handle) override {
const Sensor* const* list;
ssize_t size;
// Search static sensor list.
size = mSensorManager->getSensorList(&list);
if (size < 0) {
ALOGE("getSensorList failed with error code %zd", size);
return std::nullopt;
}
for (size_t i = 0; i < size; ++i) {
if (list[i]->getHandle() == handle) {
return *list[i];
}
}
// Search dynamic sensor list.
Vector<Sensor> dynList;
size = mSensorManager->getDynamicSensorList(dynList);
if (size < 0) {
ALOGE("getDynamicSensorList failed with error code %zd", size);
return std::nullopt;
}
for (size_t i = 0; i < size; ++i) {
if (dynList[i].getHandle() == handle) {
return dynList[i];
}
}
return std::nullopt;
}
void updateEventTimestamp(const ASensorEvent& event, SensorExtra& extra) {
if (extra.latestTimestamp != 0) {
int64_t gap = event.timestamp - extra.latestTimestamp;
extra.maxPeriod = std::max(gap, extra.maxPeriod);
extra.minPeriod = std::min(gap, extra.minPeriod);
}
extra.latestTimestamp = event.timestamp;
}
static PoseEvent parseEvent(const ASensorEvent& event, DataFormat format,
std::optional<int32_t>* discontinutyCount) {
switch (format) {
case DataFormat::kQuaternion: {
Eigen::Quaternionf quat(event.data[3], event.data[0], event.data[1], event.data[2]);
// Adapt to different frame convention.
quat *= rotateX(-M_PI_2);
return PoseEvent{Pose3f(quat), std::optional<Twist3f>(), false};
}
case DataFormat::kRotationVectorsAndDiscontinuityCount: {
Eigen::Vector3f rotation = {event.head_tracker.rx, event.head_tracker.ry,
event.head_tracker.rz};
Eigen::Vector3f twist = {event.head_tracker.vx, event.head_tracker.vy,
event.head_tracker.vz};
Eigen::Quaternionf quat = rotationVectorToQuaternion(rotation);
bool isNewReference =
!discontinutyCount->has_value() ||
discontinutyCount->value() != event.head_tracker.discontinuity_count;
*discontinutyCount = event.head_tracker.discontinuity_count;
return PoseEvent{Pose3f(quat), Twist3f(Eigen::Vector3f::Zero(), twist),
isNewReference};
}
default:
LOG_ALWAYS_FATAL("Unexpected sensor type: %d", static_cast<int>(format));
}
}
const static std::string toString(DataFormat format) {
switch (format) {
case DataFormat::kUnknown:
return "kUnknown";
case DataFormat::kQuaternion:
return "kQuaternion";
case DataFormat::kRotationVectorsAndDiscontinuityCount:
return "kRotationVectorsAndDiscontinuityCount";
default:
return "NotImplemented";
}
}
};
} // namespace
std::unique_ptr<SensorPoseProvider> SensorPoseProvider::create(const char* packageName,
Listener* listener) {
return SensorPoseProviderImpl::create(packageName, listener);
}
} // namespace media
} // namespace android