void LighthouseDriverWrapper::RunFrame() { // Create a debug log file FILE* logFile = fopen("lighthouse_runframe_debug.log", "a"); if (logFile) { fprintf(logFile, "RunFrame called at %lld\n", (long long)time(nullptr)); } if (m_pLighthouseProvider) { if (logFile) { fprintf(logFile, "Calling m_pLighthouseProvider->RunFrame()\n"); fflush(logFile); } try { m_pLighthouseProvider->RunFrame(); if (logFile) { fprintf(logFile, "RunFrame completed successfully\n"); fflush(logFile); } } catch (...) { if (logFile) { fprintf(logFile, "Exception in RunFrame\n"); fflush(logFile); } } } else { if (logFile) { fprintf(logFile, "m_pLighthouseProvider is null\n"); fflush(logFile); } } // Read IMU data from the lighthouse devices if (!m_lighthouseDevices.empty() && m_pIMUDataProvider) { if (logFile) { fprintf(logFile, "Reading IMU data from %d lighthouse devices\n", (int)m_lighthouseDevices.size()); fflush(logFile); } // For each lighthouse device for (const auto& devicePath : m_lighthouseDevices) { // Open the device HANDLE deviceHandle = CreateFileA(devicePath.c_str(), GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, 0, NULL); if (deviceHandle != INVALID_HANDLE_VALUE) { if (logFile) { fprintf(logFile, "Opened device: %s\n", devicePath.c_str()); fflush(logFile); } // Get device attributes to identify the device HIDD_ATTRIBUTES deviceAttributes; deviceAttributes.Size = sizeof(HIDD_ATTRIBUTES); if (HidD_GetAttributes(deviceHandle, &deviceAttributes)) { if (logFile) { fprintf(logFile, "Device attributes: VID=0x%04X, PID=0x%04X\n", deviceAttributes.VendorID, deviceAttributes.ProductID); fflush(logFile); } // Check if this is the IMU device (VID: 0x28DE, PID: 0x2300) if (deviceAttributes.VendorID == 0x28DE && deviceAttributes.ProductID == 0x2300) { // Create a buffer for reading const int bufferSize = 64; unsigned char buffer[bufferSize]; // Define the supported report IDs for IMU data const uint8_t supportedReportIds[] = {0, 1, 2}; // Try each supported report ID for (int i = 0; i < 3; i++) { uint8_t reportId = supportedReportIds[i]; // Set the report ID in the buffer buffer[0] = reportId; // Try to get the input report bool dataRead = false; DWORD bytesRead = 0; if (HidD_GetInputReport(deviceHandle, buffer, bufferSize)) { if (logFile) { fprintf(logFile, "Successfully read report ID %d using HidD_GetInputReport\n", reportId); fflush(logFile); } dataRead = true; bytesRead = bufferSize; } else { // If HidD_GetInputReport failed, try ReadFile if (ReadFile(deviceHandle, buffer, bufferSize, &bytesRead, NULL)) { if (bytesRead > 0) { if (logFile) { fprintf(logFile, "Read %d bytes from device using ReadFile\n", bytesRead); fflush(logFile); } dataRead = true; } } } // Process the data if we successfully read it if (dataRead && bytesRead > 0) { // Log the buffer contents for debugging if (logFile) { fprintf(logFile, "Buffer contents (first 16 bytes): "); for (int j = 0; j < min(16, (int)bytesRead); j++) { fprintf(logFile, "%02X ", buffer[j]); } fprintf(logFile, "\n"); fflush(logFile); } // Check if this is an IMU report with the expected report ID if (buffer[0] == reportId) { if (logFile) { fprintf(logFile, "Processing IMU data with report ID %d\n", reportId); fflush(logFile); } // Create an IMU sample vr::ImuSample_t sample; // Define the offsets based on our findings const uint8_t accelXOffset = 1; const uint8_t accelYOffset = 3; const uint8_t accelZOffset = 5; const uint8_t gyroXOffset = 7; const uint8_t gyroYOffset = 9; const uint8_t gyroZOffset = 11; const uint8_t timestampOffset = 13; const uint8_t timestampBytes = 4; // Define the scaling factors based on our findings const float accelScaleFactor = 0.0024f; // g per raw unit const float gyroScaleFactor = 0.001f; // rad/s per raw unit // Validate buffer size to ensure we can safely read all data if (bytesRead >= timestampOffset + timestampBytes) { // Extract accelerometer data (little-endian) int16_t accelX = (buffer[accelXOffset + 1] << 8) | buffer[accelXOffset]; int16_t accelY = (buffer[accelYOffset + 1] << 8) | buffer[accelYOffset]; int16_t accelZ = (buffer[accelZOffset + 1] << 8) | buffer[accelZOffset]; // Extract gyroscope data (little-endian) int16_t gyroX = (buffer[gyroXOffset + 1] << 8) | buffer[gyroXOffset]; int16_t gyroY = (buffer[gyroYOffset + 1] << 8) | buffer[gyroYOffset]; int16_t gyroZ = (buffer[gyroZOffset + 1] << 8) | buffer[gyroZOffset]; // Convert to the appropriate units using the scale factors sample.vAccel.v[0] = accelX * accelScaleFactor; sample.vAccel.v[1] = accelY * accelScaleFactor; sample.vAccel.v[2] = accelZ * accelScaleFactor; sample.vGyro.v[0] = gyroX * gyroScaleFactor; sample.vGyro.v[1] = gyroY * gyroScaleFactor; sample.vGyro.v[2] = gyroZ * gyroScaleFactor; // Extract timestamp (little-endian, 32-bit value) uint32_t timestamp = 0; for (int j = 0; j < timestampBytes; j++) { timestamp |= (buffer[timestampOffset + j] << (j * 8)); } // Convert to seconds (assuming microseconds) sample.fSampleTime = timestamp / 1000000.0f; if (logFile) { fprintf(logFile, "Using device timestamp: %u (%.6f seconds)\n", timestamp, sample.fSampleTime); fflush(logFile); } // Register the device if not already registered // Use a unique device index based on the device path uint32_t deviceIndex = 0; for (char c : devicePath) { deviceIndex = (deviceIndex * 31) + c; } deviceIndex = deviceIndex % 64; // Limit to a reasonable range m_pIMUDataProvider->RegisterDevice(deviceIndex); // Add the sample m_pIMUDataProvider->AddIMUSample(deviceIndex, sample); if (logFile) { fprintf(logFile, "Parsed IMU data: Accel=[%f, %f, %f], Gyro=[%f, %f, %f]\n", sample.vAccel.v[0], sample.vAccel.v[1], sample.vAccel.v[2], sample.vGyro.v[0], sample.vGyro.v[1], sample.vGyro.v[2]); fflush(logFile); } } else { if (logFile) { fprintf(logFile, "Buffer size too small to extract all IMU data\n"); fflush(logFile); } } } else { if (logFile) { fprintf(logFile, "Report ID mismatch: expected %d, got %d\n", reportId, buffer[0]); fflush(logFile); } } } } } } else { if (logFile) { fprintf(logFile, "Failed to get device attributes\n"); fflush(logFile); } } // Close the device CloseHandle(deviceHandle); } else { if (logFile) { fprintf(logFile, "Failed to open device: %s\n", devicePath.c_str()); fflush(logFile); } } } } if (logFile) { fclose(logFile); } }