Merge "Add support for ASENSOR_TYPE_HEAD_TRACKER" (63e1f0e0) · Commits · e / os / android_frameworks_av

media/libheadtracking/SensorPoseProvider.cpp

+35 −11

Original line number	Diff line number	Diff line
		@@ -133,14 +133,14 @@ class SensorPoseProviderImpl : public SensorPoseProvider {

		{
		std::lock_guard lock(mMutex);
		mEnabledSensorFormats.emplace(sensor, format);
		mEnabledSensorsExtra.emplace(sensor, SensorExtra{ .format = format });
		}

		// Enable the sensor.
		if (mQueue->enableSensor(sensor, samplingPeriod.count(), 0, 0)) {
		ALOGE("Failed to enable sensor");
		std::lock_guard lock(mMutex);
		mEnabledSensorFormats.erase(sensor);
		mEnabledSensorsExtra.erase(sensor);
		return false;
		}

		@@ -151,7 +151,7 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		void stopSensor(int handle) override {
		mEnabledSensors.erase(handle);
		std::lock_guard lock(mMutex);
		mEnabledSensorFormats.erase(handle);
		mEnabledSensorsExtra.erase(handle);
		}

		private:
		@@ -159,6 +159,7 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		kUnknown,
		kQuaternion,
		kRotationVectorsAndFlags,
		kRotationVectorsAndDiscontinuityCount,
		};

		struct PoseEvent {
		@@ -167,13 +168,18 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		bool isNewReference;
		};

		struct SensorExtra {
		DataFormat format;
		std::optional<int32_t> discontinuityCount;
		};

		sp<Looper> mLooper;
		Listener* const mListener;
		SensorManager* const mSensorManager;
		std::thread mThread;
		std::mutex mMutex;
		std::map<int32_t, SensorEnableGuard> mEnabledSensors;
		std::map<int32_t, DataFormat> mEnabledSensorFormats GUARDED_BY(mMutex);
		std::map<int32_t, SensorExtra> mEnabledSensorsExtra GUARDED_BY(mMutex);
		sp<SensorEventQueue> mQueue;

		// We must do some of the initialization operations on the worker thread, because the API relies
		@@ -248,17 +254,16 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		}

		void handleEvent(const ASensorEvent& event) {
		DataFormat format;
		PoseEvent value;
		{
		std::lock_guard lock(mMutex);
		auto iter = mEnabledSensorFormats.find(event.sensor);
		if (iter == mEnabledSensorFormats.end()) {
		auto iter = mEnabledSensorsExtra.find(event.sensor);
		if (iter == mEnabledSensorsExtra.end()) {
		// This can happen if we have any pending events shortly after stopping.
		return;
		}
		format = iter->second;
		value = parseEvent(event, iter->second.format, &iter->second.discontinuityCount);
		}
		auto value = parseEvent(event, format);
		mListener->onPose(event.timestamp, event.sensor, value.pose, value.twist,
		value.isNewReference);
		}
		@@ -274,6 +279,10 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		return DataFormat::kQuaternion;
		}

		if (sensor->getType() == ASENSOR_TYPE_HEAD_TRACKER) {
		return DataFormat::kRotationVectorsAndDiscontinuityCount;
		}

		if (sensor->getStringType() == "com.google.hardware.sensor.hid_dynamic.headtracker") {
		return DataFormat::kRotationVectorsAndFlags;
		}
		@@ -313,8 +322,8 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		return std::nullopt;
		}

		static PoseEvent parseEvent(const ASensorEvent& event, DataFormat format) {
		// TODO(ytai): Add more types.
		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]);
		@@ -338,6 +347,21 @@ class SensorPoseProviderImpl : public SensorPoseProvider {
		(flags & (1 << 0)) != 0};
		}

		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.rz};
		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));
		}