Make the driver care about tracker status messages. (#23)

SlimeVR · Jul 4, 2022 · b4d9afa · b4d9afa
1 parent a5cce86
commit b4d9afa
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 0 deletions.
diff --git a/src/IVRDevice.hpp b/src/IVRDevice.hpp
@@ -61,6 +61,7 @@ namespace SlimeVRDriver {
 
         virtual int getDeviceId() = 0;
         virtual void PositionMessage(messages::Position& position) = 0;
+        virtual void StatusMessage(messages::TrackerStatus& status) = 0;
 
         ~IVRDevice() = default;
     };

diff --git a/src/TrackerDevice.cpp b/src/TrackerDevice.cpp
@@ -64,6 +64,33 @@ void SlimeVRDriver::TrackerDevice::PositionMessage(messages::Position &position)
     this->last_pose_ = pose;
 }
 
+void SlimeVRDriver::TrackerDevice::StatusMessage(messages::TrackerStatus &status)
+{
+    auto pose = this->last_pose_;
+    switch (status.status())
+    {
+    case messages::TrackerStatus_Status_OK:
+        pose.deviceIsConnected = true;
+        pose.poseIsValid = true;
+        break;
+    case messages::TrackerStatus_Status_DISCONNECTED:
+        pose.deviceIsConnected = false;
+        pose.poseIsValid = false;
+    default:
+    case messages::TrackerStatus_Status_ERROR:
+    case messages::TrackerStatus_Status_BUSY:
+        pose.deviceIsConnected = true;
+        pose.poseIsValid = false;
+        break;
+    }
+
+    // TODO: send position/rotation of 0 instead of last pose?
+
+    GetDriver()->GetDriverHost()->TrackedDevicePoseUpdated(this->device_index_, pose, sizeof(vr::DriverPose_t));
+
+    // TODO: update this->last_pose_?
+}
+
 DeviceType SlimeVRDriver::TrackerDevice::GetDeviceType()
 {
     return DeviceType::TRACKER;

diff --git a/src/TrackerDevice.hpp b/src/TrackerDevice.hpp
@@ -36,6 +36,7 @@ namespace SlimeVRDriver {
             virtual vr::DriverPose_t GetPose() override;
             virtual int getDeviceId() override;
             virtual void PositionMessage(messages::Position &position) override;
+            virtual void StatusMessage(messages::TrackerStatus &status) override;
     private:
         vr::TrackedDeviceIndex_t device_index_ = vr::k_unTrackedDeviceIndexInvalid;
         std::string serial_;

diff --git a/src/VRDriver.cpp b/src/VRDriver.cpp
@@ -66,6 +66,12 @@ void SlimeVRDriver::VRDriver::RunFrame()
                 if(device != this->devices_by_id.end()) {
                     device->second->PositionMessage(pos);
                 }
+            } else if(message->has_tracker_status()) {
+                messages::TrackerStatus status = message->tracker_status();
+                auto device = this->devices_by_id.find(status.tracker_id());
+                if (device != this->devices_by_id.end()) {
+                    device->second->StatusMessage(status);
+                }
             }
         }