447 lines
16 KiB
C++
447 lines
16 KiB
C++
/*
|
|
* 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
|