Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 87b0621a authored by Ytai Ben-Tsvi's avatar Ytai Ben-Tsvi
Browse files

Add support for ASENSOR_TYPE_HEAD_TRACKER 

This is the official type for head-trackers.
This change adds support for such sensors as pose providers.

Test: Pending manual verification once the code that produces such
      sensor events lands.
Change-Id: I5e580daf12b8d13434e2f14fdd77a430207c1494
parent 14929832

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));

        }