diff --git a/CMakeLists.txt b/CMakeLists.txt index 01ed690..fdc2619 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,8 @@ if (UNIX) set(CMAKE_POSITION_INDEPENDENT_CODE ON) endif() +enable_testing() + find_program(CLANG_FORMAT_EXECUTABLE NAMES clang-format) if (CLANG_FORMAT_EXECUTABLE) diff --git a/projects/CMakeLists.txt b/projects/CMakeLists.txt index 80193a9..10d2c89 100644 --- a/projects/CMakeLists.txt +++ b/projects/CMakeLists.txt @@ -1,4 +1,7 @@ add_subdirectory(common) +add_subdirectory(psvr2_gen_data_parser_test) +add_subdirectory(psvr2_gaze_filter_test) +add_subdirectory(psvr2_pupillometry_test) add_subdirectory(libcrossipc) add_subdirectory(libcustomshare) if (WIN32) diff --git a/projects/common/common.h b/projects/common/common.h index e10ee2f..2699350 100644 --- a/projects/common/common.h +++ b/projects/common/common.h @@ -1,6 +1,8 @@ #pragma once +#include #include +#include #include "hmd2_gaze.h" #include "pad_trigger_effect.h" @@ -69,6 +71,29 @@ struct GazeCalibrationCommand { GazeCalibrationPacket payload; }; +// Processed pupillometry. +// +// Plain data with fixed-width fields so non-C++ consumers can marshal it: the Baballonia module and the +// Unity calibration app both P/Invoke this API from managed code. +// +// Prefer activityIndex over deltaMm. There is no display-luminance signal anywhere in the headset +// telemetry, so absolute diameter cannot be separated from the scene simply getting brighter and activityIndex counts abrupt dilations instead and rides out slow light changes. read pupillometry.h!!!!!! +struct Psvr2tkPupillometry { + int64_t timestamp; + + uint32_t isValid; + uint32_t isBaselineReady; + uint32_t isActivityReady; + uint32_t contributingEyes; // 0, 1 or 2 + + float correctedMm; // foreshortening-corrected, binocular where both eyes were usable + float smoothedMm; + float baselineMm; + float deltaMm; // smoothedMm - baselineMm + float relative; // deltaMm / baselineMm + float activityIndex; // abrupt dilations per second +}; + struct HeadsetRumbleCommand { uint8_t rumbleHz; }; @@ -92,7 +117,16 @@ enum class DriverCommandType : uint32_t { struct DriverCommand { DriverCommandType type; - volatile bool isFulfilled; + + // Cross-process completion handshake, written by the driver and polled by the submitting process + // without holding the command mutex. + // + // Declared as a plain bool and accessed through std::atomic_ref at every site (see + // custom_share_manager.cpp) rather than as std::atomic: std::atomic is not copyable, and this + // struct is assigned by value into and out of the shared-memory ring. `volatile`, which this used to + // be, is not a synchronisation primitive and carried no ordering guarantees at all. + bool isFulfilled; + union { GazeCalibrationCommand gazeCalibration; HeadsetRumbleCommand headsetRumble; @@ -100,4 +134,12 @@ struct DriverCommand { TriggerEffectCommand triggerEffect; }; }; -#pragma pack(pop) \ No newline at end of file +#pragma pack(pop) + +static_assert(std::is_trivially_copyable_v, "DriverCommand is copied by value through shared memory and must stay trivially copyable!"); +// std::atomic_ref is C++20. This header ships in the public CAPI dist include folder, where consumers +// may still be C++17, so the check is conditional -- the driver and libcustomshare build as C++23 and +// will evaluate it. +#if defined(__cpp_lib_atomic_ref) +static_assert(std::atomic_ref::is_always_lock_free, "std::atomic_ref must be lock-free to be usable across processes!"); +#endif diff --git a/projects/common/gaze_filter.h b/projects/common/gaze_filter.h new file mode 100644 index 0000000..9bac65a --- /dev/null +++ b/projects/common/gaze_filter.h @@ -0,0 +1,241 @@ +#pragma once + +// This file i'll be using the following filters: +// * a one-euro filter (Casiez, Roussel & Vogel, 2012) whose cutoff rises with gaze velocity, and +// * an I-VT classifier that detects saccades and passes them through untouched. +// +// It also holds the last good direction briefly across blinks, where the tracker drops out for ~100-150 +// ms and would otherwise make a gaze cursor fall to the floor and snap back +// +// Deliberately free of SteamVR, Windows and driver dependencies so it can be exercised off-device; +// see psvr2_gaze_filter_test. + +#include +#include + +struct GazeFilterVec3 { + float x, y, z; +}; + +enum class GazeMotionState : uint8_t { + Unknown = 0, + Fixation = 1, + Saccade = 2, +}; + +struct GazeFilterConfig { + // One-euro: the cutoff applied when gaze is still. Lower is smoother and laggier. + float minCutoffHz = 1.0f; + // IF YOU DON'T KNOW WHA'S GOING ON, READ THE ONE-EURO PAPER. This is the "beta" parameter that controls how much the cutoff rises with velocity + // The canonical one-euro beta is dimensioned by whatever units the input carries. Gaze directions are + // unit vectors, so their derivative is in rad/s -- only ~0.44 at 25 deg/s. Feeding that to the + // literature's beta of 0.007 moves the cutoff by 0.003 Hz, i.e. no adaptation whatsoever. Working in + // deg/s instead makes this value interpretable and actually effective. + + // I recommend reading about the 1 euro formula btw, it's a very good filter and I have been using it on my tracking and math projects :pppp + float beta = 0.12f; + // One-euro: cutoff for the velocity estimate itself. + float derivativeCutoffHz = 1.0f; + // I-VT threshold. + // + // Must sit above the tracker's noise floor, not just above normal eye drift: +/-0.3 deg of per-sample + // noise at 120 Hz already looks like ~70 deg/s. A threshold down at the 30 deg/s often quoted for + // clean lab data would classify ordinary fixation noise as saccades and bypass filtering entirely. + // Real saccades of any size run well into the hundreds of deg/s. + float saccadeVelocityDegPerSec = 120.0f; + // How long to keep reporting the last good direction after validity drops. + int64_t blinkHoldUs = 200000; + // Beyond this gap the filter history is stale and gets discarded rather than smoothed against. + int64_t maxSampleGapUs = 500000; +}; + +struct GazeFilterOutput { + GazeFilterVec3 direction{0.0f, 0.0f, 0.0f}; + GazeMotionState state = GazeMotionState::Unknown; + // Whether a usable direction is being reported at all. + bool valid = false; + // True when `direction` is a held sample carried across a dropout rather than a fresh measurement. + bool held = false; +}; + +class GazeFilter { +public: + void Configure(const GazeFilterConfig &config) { m_config = config; } + const GazeFilterConfig &Config() const { return m_config; } + + void Reset() { + m_hasPrevious = false; + m_hasLastValid = false; + m_state = GazeMotionState::Unknown; + } + + GazeFilterOutput Update(const GazeFilterVec3 &direction, bool inputValid, int64_t timestampUs) { + GazeFilterOutput out; + + if (!inputValid) { + return HoldOrDrop(timestampUs); + } + + // normalise to {0,0,0}, which is finite and would sail through as if it were a real measurement + const float inputLength = Length(direction); + if (!std::isfinite(inputLength) || inputLength <= 1e-6f) { + return HoldOrDrop(timestampUs); + } + + const GazeFilterVec3 normalized{direction.x / inputLength, direction.y / inputLength, direction.z / inputLength}; + + // First usable sample, or the history is too old to smooth against. + const int64_t deltaUs = m_hasPrevious ? (timestampUs - m_previousTimestampUs) : 0; + if (!m_hasPrevious || deltaUs <= 0 || deltaUs > m_config.maxSampleGapUs) { + SeedFrom(normalized, timestampUs); + + out.direction = normalized; + out.state = GazeMotionState::Unknown; + out.valid = true; + out.held = false; + return out; + } + + const float deltaSeconds = static_cast(deltaUs) / 1e6f; + + // Angular velocity between consecutive raw samples drives both the classifier and the one-euro + // cutoff. Using the raw input rather than the filtered output keeps the filter from hiding the + // very motion it needs to react to. + const float velocityDegPerSec = AngleBetweenDegrees(m_previousRaw, normalized) / deltaSeconds; + + m_previousRaw = normalized; + m_previousTimestampUs = timestampUs; + + if (velocityDegPerSec >= m_config.saccadeVelocityDegPerSec) { + // Saccade: report the measurement as-is and restart smoothing from it. Filtering here would add + // latency exactly where the eye is moving fastest and the user notices most. + m_state = GazeMotionState::Saccade; + m_filtered = normalized; + m_derivative = GazeFilterVec3{0.0f, 0.0f, 0.0f}; + m_lastValid = normalized; + m_lastValidTimestampUs = timestampUs; + m_hasLastValid = true; + + out.direction = normalized; + out.state = GazeMotionState::Saccade; + out.valid = true; + out.held = false; + return out; + } + + m_state = GazeMotionState::Fixation; + + // One-euro here. + const GazeFilterVec3 rawDerivative{(normalized.x - m_filtered.x) / deltaSeconds, (normalized.y - m_filtered.y) / deltaSeconds, + (normalized.z - m_filtered.z) / deltaSeconds}; + + const float derivativeAlpha = SmoothingFactor(m_config.derivativeCutoffHz, deltaSeconds); + m_derivative = Lerp(m_derivative, rawDerivative, derivativeAlpha); + + // For a unit-length direction the derivative magnitude is angular velocity in rad/s; beta is + // expressed per deg/s, so convert. Note this uses the LOW-PASSED derivative, not the raw derivative: the one-euro paper explicitly warns that raw derivative + // noise averages towards zero here, which is what keeps the cutoff low while the eye is still + const float derivativeDegPerSec = Length(m_derivative) * (180.0f / 3.14159265358979323846f); + const float cutoff = m_config.minCutoffHz + m_config.beta * derivativeDegPerSec; + const float alpha = SmoothingFactor(cutoff, deltaSeconds); + + m_filtered = Normalize(Lerp(m_filtered, normalized, alpha)); + + m_lastValid = m_filtered; + m_lastValidTimestampUs = timestampUs; + m_hasLastValid = true; + + out.direction = m_filtered; + out.state = GazeMotionState::Fixation; + out.valid = true; + out.held = false; + return out; + } + +private: + void SeedFrom(const GazeFilterVec3 &direction, int64_t timestampUs) { + m_filtered = direction; + m_previousRaw = direction; + m_derivative = GazeFilterVec3{0.0f, 0.0f, 0.0f}; + m_previousTimestampUs = timestampUs; + m_hasPrevious = true; + + m_lastValid = direction; + m_lastValidTimestampUs = timestampUs; + m_hasLastValid = true; + m_state = GazeMotionState::Unknown; + } + + GazeFilterOutput HoldOrDrop(int64_t timestampUs) { + GazeFilterOutput out; + + if (!m_hasLastValid) { + return out; + } + + const int64_t heldForUs = timestampUs - m_lastValidTimestampUs; + if (heldForUs < 0 || heldForUs > m_config.blinkHoldUs) { + m_hasPrevious = false; + m_hasLastValid = false; + m_state = GazeMotionState::Unknown; + return out; + } + + + m_hasPrevious = false; + + out.direction = m_lastValid; + out.state = m_state; + out.valid = true; + out.held = true; + return out; + } + + static float SmoothingFactor(float cutoffHz, float deltaSeconds) { + if (cutoffHz <= 0.0f) { + return 1.0f; + } + const float tau = 1.0f / (6.2831853f * cutoffHz); + return 1.0f / (1.0f + tau / deltaSeconds); + } + + static GazeFilterVec3 Lerp(const GazeFilterVec3 &from, const GazeFilterVec3 &to, float alpha) { + return GazeFilterVec3{from.x + (to.x - from.x) * alpha, from.y + (to.y - from.y) * alpha, from.z + (to.z - from.z) * alpha}; + } + + static float Length(const GazeFilterVec3 &v) { return std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); } + + static bool IsFinite(const GazeFilterVec3 &v) { return std::isfinite(v.x) && std::isfinite(v.y) && std::isfinite(v.z); } + + static GazeFilterVec3 Normalize(const GazeFilterVec3 &v) { + const float length = Length(v); + if (length <= 1e-6f || !std::isfinite(length)) { + return GazeFilterVec3{0.0f, 0.0f, 0.0f}; + } + return GazeFilterVec3{v.x / length, v.y / length, v.z / length}; + } + + static float AngleBetweenDegrees(const GazeFilterVec3 &a, const GazeFilterVec3 &b) { + float dot = a.x * b.x + a.y * b.y + a.z * b.z; + if (dot > 1.0f) { + dot = 1.0f; + } else if (dot < -1.0f) { + dot = -1.0f; + } + return std::acos(dot) * (180.0f / 3.14159265358979323846f); + } + + GazeFilterConfig m_config; + + GazeFilterVec3 m_filtered{0.0f, 0.0f, 0.0f}; + GazeFilterVec3 m_previousRaw{0.0f, 0.0f, 0.0f}; + GazeFilterVec3 m_derivative{0.0f, 0.0f, 0.0f}; + int64_t m_previousTimestampUs = 0; + bool m_hasPrevious = false; + + GazeFilterVec3 m_lastValid{0.0f, 0.0f, 0.0f}; + int64_t m_lastValidTimestampUs = 0; + bool m_hasLastValid = false; + + GazeMotionState m_state = GazeMotionState::Unknown; +}; diff --git a/projects/common/hmd2_gen_data.h b/projects/common/hmd2_gen_data.h new file mode 100644 index 0000000..18c95ce --- /dev/null +++ b/projects/common/hmd2_gen_data.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include + +#include "hmd2_gaze.h" + +// Wire format for the headset's "VD" general-data payloads, delivered on the CaesarUsbThreadGenData +// pipe. A payload is a sequence of self-describing blocks; each block carries a table of items, and +// item id 0x3 holds the gaze calibration blob. + +#pragma pack(push, 1) +typedef struct Hmd2GenDataHeader { + char magic[2]; + uint16_t minSize; + uint32_t size; + uint16_t numItems; + uint8_t padding[22]; +} Hmd2GenDataHeader; + +typedef struct Hmd2GenDataItem { + uint16_t id; + uint16_t unk1; + uint32_t size; + uint32_t offset; +} Hmd2GenDataItem; + +typedef struct Hmd2GazeCalibHeader { + char magic[2]; + uint16_t struct_version; + uint16_t data_version; + uint16_t data_id; + uint32_t payload_size; + uint32_t calib_id; + hmd2_gaze_enabled_eye_t calib_eye; + uint8_t padding[12]; +} Hmd2GazeCalibHeader; +#pragma pack(pop) + +static_assert(sizeof(Hmd2GenDataHeader) == 32, "Size of Hmd2GenDataHeader is not 32 bytes!"); +static_assert(sizeof(Hmd2GenDataItem) == 12, "Size of Hmd2GenDataItem is not 12 bytes!"); + +constexpr uint16_t k_hmd2GenDataBlockGranularity = 0x200; +constexpr uint16_t k_hmd2GenDataItemIdGazeCalib = 0x3; + + +constexpr uint32_t k_hmd2GazeCalibMaxBlobSize = 0x10000; + +// Filename the gaze calibration blob is persisted under, in the SteamVR user config directory. +#define HMD2_GAZE_CALIB_BLOB_FILENAME "gaze_calibration_blob.bin" + +// Does this look like a calibration blob worth sending to the headset? +inline bool Hmd2IsGazeCalibBlobValid(const char *data, uint32_t size) { + if (data == nullptr || size < sizeof(Hmd2GazeCalibHeader) || size > k_hmd2GazeCalibMaxBlobSize) { + return false; + } + + const Hmd2GazeCalibHeader *header = reinterpret_cast(data); + + // Same acceptance test the save path applies, so we never upload something we would not have stored. + return header->calib_id != 0 && header->calib_eye == HMD2_GAZE_ENABLED_EYE_BOTH; +} + +template inline int Hmd2ParseGenData(char *buffer, uint32_t bufferSize, OnGazeCalibItem &&onGazeCalibItem) { + uint32_t remaining = bufferSize; + + while (remaining > 0) { + if (remaining < k_hmd2GenDataBlockGranularity) { + return -1; + } + + const Hmd2GenDataHeader *header = reinterpret_cast(buffer); + if (header->magic[0] != 'V' || header->magic[1] != 'D' || header->minSize != k_hmd2GenDataBlockGranularity || header->size < sizeof(Hmd2GenDataHeader) || + header->size > remaining) { + return -1; + } + + const uint32_t itemTableBytes = static_cast(header->numItems) * static_cast(sizeof(Hmd2GenDataItem)); + if (itemTableBytes > header->size - sizeof(Hmd2GenDataHeader)) { + return -1; + } + + const Hmd2GenDataItem *items = reinterpret_cast(buffer + sizeof(Hmd2GenDataHeader)); + + for (uint16_t i = 0; i < header->numItems; ++i) { + const Hmd2GenDataItem *item = &items[i]; + + if (item->offset > header->size || item->size > header->size - item->offset) { + return -1; + } + + if (item->id == k_hmd2GenDataItemIdGazeCalib) { + onGazeCalibItem(buffer + item->offset, item->size); + } + } + + buffer += header->size; + remaining -= header->size; + } + + return 0; +} diff --git a/projects/common/pupillometry.h b/projects/common/pupillometry.h new file mode 100644 index 0000000..6e1864f --- /dev/null +++ b/projects/common/pupillometry.h @@ -0,0 +1,495 @@ +#pragma once + +// Pupillometry +// +// The headset reports pupil diameter per eye at 120 Hz. Delivering that number to an application directly would be misleading: it is dominated by the pupillary light reflex, contaminated by blink artifacts +// of the same magnitude as the effect people actually want, and biased by the angle the eye is turned +// relative to the camera. This runs the corrections that make it mean something. +// +// Stages, in order (at least of what I've worked with so far): +// +// 1. Validity and blink gating, with a guard band either side of every blink edge. +// 2. Foreshortening correction against a per-eye camera axis. +// 3. Binocular fusion when both eyes are usable. +// 4. Low-pass filtering into the band where pupil responses actually live. +// 5. Rolling baseline, with subtractive and divisive normalisation. +// 6. A luminance-robust activity index. +// + +// WHAT THIS CANNOT DO!!!! +// There is no display-luminance signal available anywhere in the headset +// telemetry -- exp_l/exp_r and led_status are constants describing the IR illuminator, not the screen. +// So absolute diameter can never be separated from "the scene got brighter". `activityIndex` exists +// precisely because it measures the RATE of abrupt dilations rather than their absolute level, which +// is far less sensitive to a slow luminance ramp. Treat `deltaMm`/`relative` as descriptive, and do +// not present either as a load measurement. +// +// Calibrate the camera axes with PupilCameraAxisCalibrator before trusting stage 2; the defaults are a +// starting point, not a fit for your headset. +// +// Dependency-free so it can be exercised off-device; see psvr2_pupillometry_test. +//Update made by LukeTheProtogen :PPP + +#include // std::nth_element +#include +#include +#include +#include +#include + +struct PupilEyeSample { + float diameterMm = 0.0f; + bool diameterValid = false; + bool blinking = false; + + + float gazeDirX = 0.0f; + float gazeDirY = 0.0f; + float gazeDirZ = 0.0f; + bool gazeValid = false; +}; + +struct PupilFrame { + int64_t timestampUs = 0; + PupilEyeSample left; + PupilEyeSample right; +}; + +struct PupillometryConfig { + float leftCameraAzimuthDeg = 0.0f; + float leftCameraElevationDeg = 0.0f; + float rightCameraAzimuthDeg = 0.0f; + float rightCameraElevationDeg = 0.0f; + // amplifies noise faster than it removes bias + float minCorrectionCosine = 0.5f; + + // Samples within this of a blink edge are discarded, on both sides. Measured blink artifacts on real + // data reached 0.27 mm, which is the size of the entire effect being looked for -- this is the single + // most important stage here, and it is why results are delayed (see Update). + int64_t blinkGuardUs = 100000; + + // Rolling baseline length. Long enough to average out the light reflex, short enough to track drift. + float baselineWindowSeconds = 20.0f; + + // Pupil responses live below ~2 Hz; anything above this is sensor noise. + float lowPassHz = 4.0f; + + // Window the activity index is computed over. + float activityWindowSeconds = 4.0f; + + // Plausibility gate on raw diameter, in millimetres. + float minPlausibleMm = 1.0f; + float maxPlausibleMm = 9.0f; +}; + +struct PupillometryResult { + int64_t timestampUs = 0; + + // True when this carries an artifact-free, corrected measurement. + bool valid = false; + + float correctedMm = 0.0f; // foreshortening-corrected, binocular where both eyes were usable + float smoothedMm = 0.0f; // after the low-pass + float baselineMm = 0.0f; + float deltaMm = 0.0f; // smoothed - baseline + float relative = 0.0f; // (smoothed - baseline) / baseline + + // Abrupt dilations per second. Luminance-robust; this is the number to prefer. + float activityIndex = 0.0f; + + bool baselineReady = false; + bool activityReady = false; + int contributingEyes = 0; // 0, 1 or 2 +}; + +namespace pupillometry_detail { + +// Builds a unit axis from azimuth/elevation, in the headset's +Z-forward convention. +inline void AxisFromAngles(float azimuthDeg, float elevationDeg, float &x, float &y, float &z) { + const float az = azimuthDeg * (3.14159265358979323846f / 180.0f); + const float el = elevationDeg * (3.14159265358979323846f / 180.0f); + x = std::sin(az) * std::cos(el); + y = std::sin(el); + z = std::cos(az) * std::cos(el); +} + +// Cosine of the angle between the gaze direction and the camera axis, 1.0 when the eye looks straight +// down the camera. Both vectors point away from the eye in the same convention, so this is a plain dot +// product -- no negation. +inline float CosineToAxis(const PupilEyeSample &eye, float ax, float ay, float az) { + const float n = std::sqrt(eye.gazeDirX * eye.gazeDirX + eye.gazeDirY * eye.gazeDirY + eye.gazeDirZ * eye.gazeDirZ); + if (n < 1e-6f || !std::isfinite(n)) { + return 0.0f; + } + const float c = (eye.gazeDirX / n) * ax + (eye.gazeDirY / n) * ay + (eye.gazeDirZ / n) * az; + return c > 1.0f ? 1.0f : (c < -1.0f ? -1.0f : c); +} + +inline float Median(std::vector values) { + if (values.empty()) { + return 0.0f; + } + const size_t mid = values.size() / 2; + std::nth_element(values.begin(), values.begin() + mid, values.end()); + return values[mid]; +} + +} // namespace pupillometry_detail + +class PupillometryProcessor { +public: + void Configure(const PupillometryConfig &config) { m_config = config; } + const PupillometryConfig &Config() const { return m_config; } + + void Reset() { + m_pending.clear(); + m_baseline.clear(); + m_activity.clear(); + m_haveSmoothed = false; + m_lastEmitUs = 0; + m_lastBlinkUs = 0; + } + + // Feeds one frame and reports whether a processed result is ready. + // Results lag the input by blinkGuardUs, because a sample can only be cleared once it is known that + // no blink starts shortly after it. That delay is the price of not emitting blink artifacts, and at + // the default 100 ms it is irrelevant for anything pupil responses are used for. + bool Update(const PupilFrame &frame, PupillometryResult &out) { + if (frame.left.blinking || frame.right.blinking) { + m_lastBlinkUs = frame.timestampUs; + } + + m_pending.push_back(frame); + + // Keep the buffer bounded even if timestamps misbehave. + while (m_pending.size() > k_maxPending) { + m_pending.pop_front(); + } + + const int64_t now = frame.timestampUs; + if (m_pending.empty() || (now - m_pending.front().timestampUs) < m_config.blinkGuardUs) { + return false; + } + + const PupilFrame candidate = m_pending.front(); + m_pending.pop_front(); + + // Non-monotonic input invalidates the windows we accumulate. + if (m_lastEmitUs != 0 && candidate.timestampUs <= m_lastEmitUs) { + return false; + } + + out = Process(candidate, now); + m_lastEmitUs = candidate.timestampUs; + return true; + } + +private: + static constexpr size_t k_maxPending = 512; + + // A sample is usable only if it is far enough from any blink edge on BOTH sides. Looking forward alone is not enough. The lid retracting after a blink partially occludes the pupil + // for a while, and that recovery artifact is at least as large as the approach to the blink measured 0.27 mm swing spans both. Forward coverage comes from the pending buffer; backwardd coverage needs the timestamp of the last blink seen. + bool NearBlink(int64_t timestampUs) const { + if (m_lastBlinkUs != 0 && timestampUs > m_lastBlinkUs && (timestampUs - m_lastBlinkUs) <= m_config.blinkGuardUs) { + return true; + } + + for (const PupilFrame &f : m_pending) { + if (!f.left.blinking && !f.right.blinking) { + continue; + } + const int64_t dt = f.timestampUs - timestampUs; + if (dt >= 0 && dt <= m_config.blinkGuardUs) { + return true; + } + } + return false; + } + + bool CorrectEye(const PupilEyeSample &eye, float azimuthDeg, float elevationDeg, float &outMm) const { + if (!eye.diameterValid || eye.blinking || !eye.gazeValid) { + return false; + } + if (!std::isfinite(eye.diameterMm) || eye.diameterMm < m_config.minPlausibleMm || eye.diameterMm > m_config.maxPlausibleMm) { + return false; + } + + float ax, ay, az; + pupillometry_detail::AxisFromAngles(azimuthDeg, elevationDeg, ax, ay, az); + + const float cosine = pupillometry_detail::CosineToAxis(eye, ax, ay, az); + if (cosine < m_config.minCorrectionCosine) { + return false; + } + + outMm = eye.diameterMm / cosine; + return std::isfinite(outMm); + } + + PupillometryResult Process(const PupilFrame &frame, int64_t nowUs) { + PupillometryResult out; + out.timestampUs = frame.timestampUs; + + if (NearBlink(frame.timestampUs)) { + return out; + } + + float leftMm = 0.0f, rightMm = 0.0f; + const bool haveLeft = CorrectEye(frame.left, m_config.leftCameraAzimuthDeg, m_config.leftCameraElevationDeg, leftMm); + const bool haveRight = CorrectEye(frame.right, m_config.rightCameraAzimuthDeg, m_config.rightCameraElevationDeg, rightMm); + + if (!haveLeft && !haveRight) { + return out; + } + + // Averaging both eyes cuts high-frequency noise by roughly a third on real data, and the two are + // physiologically yoked so there is no signal lost by doing it. + out.contributingEyes = (haveLeft ? 1 : 0) + (haveRight ? 1 : 0); + out.correctedMm = (haveLeft && haveRight) ? (leftMm + rightMm) * 0.5f : (haveLeft ? leftMm : rightMm); + out.valid = true; + + // low-pass (DON'T TOUCH the baseline or activity index with this, they are already slow enough) + if (!m_haveSmoothed) { + m_smoothed = out.correctedMm; + m_haveSmoothed = true; + m_smoothedAtUs = frame.timestampUs; + } else { + const float dt = static_cast(frame.timestampUs - m_smoothedAtUs) / 1e6f; + if (dt > 0.0f) { + const float tau = 1.0f / (6.2831853f * (m_config.lowPassHz > 0.0f ? m_config.lowPassHz : 1.0f)); + const float alpha = dt / (dt + tau); + m_smoothed += (out.correctedMm - m_smoothed) * alpha; + m_smoothedAtUs = frame.timestampUs; + } + } + out.smoothedMm = m_smoothed; + + //rolling baseline + m_baseline.push_back({frame.timestampUs, m_smoothed}); + const int64_t baselineSpanUs = static_cast(m_config.baselineWindowSeconds * 1e6f); + while (!m_baseline.empty() && (frame.timestampUs - m_baseline.front().timestampUs) > baselineSpanUs) { + m_baseline.pop_front(); + } + + // A median rather than a mean: it ignores the slow light-reflex excursions that would otherwise + // drag the reference around. + if ((frame.timestampUs - m_baseline.front().timestampUs) >= baselineSpanUs / 2) { + std::vector values; + values.reserve(m_baseline.size()); + for (const Entry &e : m_baseline) { + values.push_back(e.value); + } + out.baselineMm = pupillometry_detail::Median(std::move(values)); + out.baselineReady = true; + out.deltaMm = m_smoothed - out.baselineMm; + out.relative = out.baselineMm > 1e-3f ? out.deltaMm / out.baselineMm : 0.0f; + } + + // activity index + m_activity.push_back({frame.timestampUs, out.correctedMm}); + const int64_t activitySpanUs = static_cast(m_config.activityWindowSeconds * 1e6f); + while (!m_activity.empty() && (frame.timestampUs - m_activity.front().timestampUs) > activitySpanUs) { + m_activity.pop_front(); + } + + if (m_activity.size() >= 32 && (frame.timestampUs - m_activity.front().timestampUs) >= activitySpanUs / 2) { + out.activityIndex = ComputeActivityIndex(); + out.activityReady = true; + } + + return out; + } + + // Counts abrupt dilations per second. + // This follows the shape of Duchowski et al.'s Index of Pupillary Activity: take a wavelet transform + // of the diameter signal, threshold the detail coefficients against a noise estimate, and count what + // survives. A Haar basis stands in for the paper's Symlet-16 -- coarser, but it keeps the property + // that matters here, which is responding to the rate of sudden changes rather than the absolute + // level, and therefore riding out slow luminance ramps that would swamp raw diameter. + float ComputeActivityIndex() const { + if (m_activity.size() < 16) { + return 0.0f; + } + + const size_t n = m_activity.size() - 1; + std::vector detail; + detail.reserve(n); + for (size_t i = 0; i < n; ++i) { + detail.push_back((m_activity[i].value - m_activity[i + 1].value) * 0.70710678f); + } + + // Robust noise estimate: MAD scaled to a standard deviation. + std::vector magnitudes; + magnitudes.reserve(detail.size()); + for (float d : detail) { + magnitudes.push_back(std::fabs(d)); + } + const float sigma = pupillometry_detail::Median(std::move(magnitudes)) / 0.6745f; + if (!(sigma > 0.0f)) { + return 0.0f; + } + + // Universal threshold. + const float threshold = sigma * std::sqrt(2.0f * std::log(static_cast(detail.size()))); + + int count = 0; + for (float d : detail) { + if (std::fabs(d) > threshold) { + ++count; + } + } + + const float spanSeconds = static_cast(m_activity.back().timestampUs - m_activity.front().timestampUs) / 1e6f; + return spanSeconds > 0.0f ? static_cast(count) / spanSeconds : 0.0f; + } + + struct Entry { + int64_t timestampUs; + float value; + }; + + PupillometryConfig m_config; + std::deque m_pending; + std::deque m_baseline; + std::deque m_activity; + + float m_smoothed = 0.0f; + int64_t m_smoothedAtUs = 0; + bool m_haveSmoothed = false; + int64_t m_lastEmitUs = 0; + int64_t m_lastBlinkUs = 0; +}; + +// Fits the camera axis for one eye. +// +// The foreshortening correction is only as good as the axis it is told about, and that axis is a +// property of how the camera sits in a particular headset. Rather than hardcoding a guess, accumulate +// samples while the wearer looks around and solve for the axis that leaves no residual relationship +// between gaze angle and apparent diameter -- if the correction is right, the two become independent. +class PupilCameraAxisCalibrator { +public: + // Roughly two minutes at 120 Hz. Far more than the fit needs, and it stops a forgotten calibration + // from growing without bound. + static constexpr size_t k_maxSamples = 16000; + + void Add(const PupilEyeSample &eye, float minPlausibleMm = 1.0f, float maxPlausibleMm = 9.0f) { + if (m_samples.size() >= k_maxSamples) { + return; + } + if (!eye.diameterValid || eye.blinking || !eye.gazeValid) { + return; + } + if (!std::isfinite(eye.diameterMm) || eye.diameterMm < minPlausibleMm || eye.diameterMm > maxPlausibleMm) { + return; + } + m_samples.push_back(eye); + } + + size_t Count() const { return m_samples.size(); } + void Reset() { m_samples.clear(); } + + // Grid search over plausible camera placements. Returns false if there is not enough data, or if the + // wearer never looked around enough for the fit to mean anything. + // + // outResidual is the coefficient of variation of the corrected diameter at the chosen axis: lower is + // better, and it will not reach zero on real data because pupil size genuinely varies. Compare it + // against the uncorrected CV to judge whether the fit bought anything. + bool Solve(float &outAzimuthDeg, float &outElevationDeg, float &outResidual, float &outGazeSpreadDeg) const { + if (m_samples.size() < 200) { + return false; + } + + // outGazeSpreadDeg is the smaller of the yaw and pitch spreads, so this rejects both "never looked + // around" and "only looked along one axis". + outGazeSpreadDeg = GazeSpreadDegrees(); + if (outGazeSpreadDeg < 8.0f) { + return false; + } + + bool found = false; + float best = 0.0f; + + for (int az = -40; az <= 40; az += 2) { + for (int el = -40; el <= 40; el += 2) { + float residual = 0.0f; + if (!ResidualFor(static_cast(az), static_cast(el), residual)) { + continue; + } + if (!found || residual < best) { + found = true; + best = residual; + outAzimuthDeg = static_cast(az); + outElevationDeg = static_cast(el); + } + } + } + + outResidual = best; + return found; + } + +private: + // Returns the SMALLER of the yaw and pitch spreads, in degrees. + float GazeSpreadDegrees() const { + constexpr float k_radToDeg = 180.0f / 3.14159265358979323846f; + float minYaw = 1e9f, maxYaw = -1e9f, minPitch = 1e9f, maxPitch = -1e9f; + + for (const PupilEyeSample &s : m_samples) { + const float n = std::sqrt(s.gazeDirX * s.gazeDirX + s.gazeDirY * s.gazeDirY + s.gazeDirZ * s.gazeDirZ); + if (n < 1e-6f) { + continue; + } + const float yaw = std::atan2(s.gazeDirX / n, s.gazeDirZ / n) * k_radToDeg; + // Clamped with ternaries rather than std::min/max: this header gets included in translation units + // that pull in windows.h without NOMINMAX, where those names are macros and will not compile. + const float sinPitch = s.gazeDirY / n; + const float sinPitchClamped = sinPitch > 1.0f ? 1.0f : (sinPitch < -1.0f ? -1.0f : sinPitch); + const float pitch = std::asin(sinPitchClamped) * k_radToDeg; + + minYaw = yaw < minYaw ? yaw : minYaw; + maxYaw = yaw > maxYaw ? yaw : maxYaw; + minPitch = pitch < minPitch ? pitch : minPitch; + maxPitch = pitch > maxPitch ? pitch : maxPitch; + } + + const float yawSpread = maxYaw - minYaw; + const float pitchSpread = maxPitch - minPitch; + return yawSpread < pitchSpread ? yawSpread : pitchSpread; + } + + // Scores a candidate axis by how CONSTANT it makes the corrected diameter, as a coefficient of variation. + bool ResidualFor(float azimuthDeg, float elevationDeg, float &outResidual) const { + float ax, ay, az; + pupillometry_detail::AxisFromAngles(azimuthDeg, elevationDeg, ax, ay, az); + + double sum = 0.0, sumSq = 0.0; + size_t n = 0; + + for (const PupilEyeSample &s : m_samples) { + const float c = pupillometry_detail::CosineToAxis(s, ax, ay, az); + if (c < 0.5f) { + return false; // this axis puts samples implausibly far off-axis + } + const double corrected = static_cast(s.diameterMm) / c; + sum += corrected; + sumSq += corrected * corrected; + ++n; + } + + if (n < 2) { + return false; + } + + const double mean = sum / static_cast(n); + if (mean <= 1e-6) { + return false; + } + + const double rawVariance = sumSq / static_cast(n) - mean * mean; + const double variance = rawVariance > 0.0 ? rawVariance : 0.0; + outResidual = static_cast(std::sqrt(variance) / mean); + return true; + } + + std::vector m_samples; +}; diff --git a/projects/libcustomshare/custom_share_manager.cpp b/projects/libcustomshare/custom_share_manager.cpp index b0440ea..5065b2c 100644 --- a/projects/libcustomshare/custom_share_manager.cpp +++ b/projects/libcustomshare/custom_share_manager.cpp @@ -1,9 +1,11 @@ #include "custom_share_manager.h" +#include #include #include #include #include +#include #include #include #include "util.h" @@ -15,10 +17,12 @@ void GazeStatus::set(const hmd2_gaze_status_t *pGazeStatus) { std::memcpy(&data, void GazeStatus::get(hmd2_gaze_status_t *pGazeStatus) const { std::memcpy(pGazeStatus, &data, sizeof(data)); } -void GazeImage::pushToCircularBuffer(const unsigned char *pGazeImage) { - int index = counter % 8; - // TODO: size shouldn't be 0x200100? - std::memcpy(&images[0x200100 * index], pGazeImage, 0x200100); +void GazeImage::pushToCircularBuffer(const unsigned char *pGazeImage, uint32_t size) { + const uint32_t copySize = (size == 0 || size > k_gazeImageSlotSize) ? k_gazeImageSlotSize : size; + const int index = counter % k_gazeImageSlots; + + std::memcpy(&images[k_gazeImageSlotSize * index], pGazeImage, copySize); + sizes[index] = copySize; counter++; } @@ -27,19 +31,40 @@ int GazeImage::getFromCircularBuffer(unsigned char **gazeImageBuffer) { *gazeImageBuffer = nullptr; return -1; } - int index = (counter - 1) % 8; - // TODO: size shouldn't be 0x200100? - *gazeImageBuffer = &images[0x200100 * index]; + int index = (counter - 1) % k_gazeImageSlots; + *gazeImageBuffer = &images[k_gazeImageSlotSize * index]; return index; } +bool GazeImage::copyLatest(unsigned char *pDest, uint32_t destSize, uint32_t *pOutSize) const { + if (counter == 0) { + if (pOutSize) { + *pOutSize = 0; + } + return false; + } + + const int index = (counter - 1) % k_gazeImageSlots; + const uint32_t available = (sizes[index] == 0 || sizes[index] > k_gazeImageSlotSize) ? k_gazeImageSlotSize : sizes[index]; + const uint32_t copySize = available < destSize ? available : destSize; + + std::memcpy(pDest, &images[k_gazeImageSlotSize * index], copySize); + if (pOutSize) { + *pOutSize = copySize; + } + + // Report a short read rather than pretending the caller got a whole frame. + return copySize == available; +} + DriverCommand *CommandBuffer::push(const DriverCommand &command) { int next_head = (head + 1) % 256; if (next_head == tail) return nullptr; DriverCommand *ptr = &commands[head]; *ptr = command; - ptr->isFulfilled = false; + // Release: the payload copied above must be visible before a consumer can observe this slot. + std::atomic_ref(ptr->isFulfilled).store(false, std::memory_order_release); head = next_head; return ptr; } @@ -82,28 +107,28 @@ CustomShareManager *CustomShareManager::getSingleton() { } void CustomShareManager::initialize() { - m_gazeStatusBroadcast = CreateIpcBroadcast("CUSTOM_SHARE_VRT2_WIN_GAZE_STATUS_BCAST"); - m_gazeStatusMutex = CreateIpcMutex("CUSTOM_SHARE_VRT2_WIN_GAZE_STATUS_MTX"); + m_gazeStatusBroadcast = CreateIpcBroadcast(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_GAZE_STATUS_BCAST")); + m_gazeStatusMutex = CreateIpcMutex(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_GAZE_STATUS_MTX")); - m_gazeImageBroadcast = CreateIpcBroadcast("CUSTOM_SHARE_VRT2_WIN_GAZE_IMAGE_BCAST"); - m_gazeImageMutex = CreateIpcMutex("CUSTOM_SHARE_VRT2_WIN_GAZE_IMAGE_MTX"); + m_gazeImageBroadcast = CreateIpcBroadcast(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_GAZE_IMAGE_BCAST")); + m_gazeImageMutex = CreateIpcMutex(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_GAZE_IMAGE_MTX")); for (int i = 0; i < k_maxSlots; i++) { char name[128]; - snprintf(name, sizeof(name), "CUSTOM_SHARE_VRT2_WIN_SLOT_OWNER_MTX_%d", i); + snprintf(name, sizeof(name), CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_SLOT_OWNER_MTX") "_%d", i); m_slotOwnerMutex[i] = CreateIpcMutex(name); } - m_pcmBroadcast = CreateIpcBroadcast("CUSTOM_SHARE_VRT2_WIN_PCM_BCAST"); - m_commandBroadcast = CreateIpcBroadcast("CUSTOM_SHARE_VRT2_WIN_CMD_BCAST"); + m_pcmBroadcast = CreateIpcBroadcast(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_PCM_BCAST")); + m_commandBroadcast = CreateIpcBroadcast(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_CMD_BCAST")); - m_commandMutex = CreateIpcMutex("CUSTOM_SHARE_VRT2_WIN_COMMAND_MTX"); + m_commandMutex = CreateIpcMutex(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN_COMMAND_MTX")); - m_sharedMemory = CreateIpcSharedMemory("CUSTOM_SHARE_VRT2_WIN", sizeof(BufferData)); + m_sharedMemory = CreateIpcSharedMemory(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_WIN"), sizeof(BufferData)); m_pBufferData = static_cast(IpcSharedMemory_Map(m_sharedMemory)); - m_driverActiveMutex = CreateIpcMutex("CUSTOM_SHARE_VRT2_DRIVER_ACTIVE_MTX"); - m_driverActiveGuardMutex = CreateIpcMutex("CUSTOM_SHARE_VRT2_DRIVER_ACTIVE_GUARD_MTX"); + m_driverActiveMutex = CreateIpcMutex(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_DRIVER_ACTIVE_MTX")); + m_driverActiveGuardMutex = CreateIpcMutex(CUSTOM_SHARE_NAME("CUSTOM_SHARE_VRT2_DRIVER_ACTIVE_GUARD_MTX")); } #ifdef _WIN32 @@ -151,20 +176,26 @@ bool CustomShareManager::getDriverActive() { return active; } +// Returns true once this process owns the driver-active mutex, false if it could not be taken within the timeout (which means another driver instance is holding it bool CustomShareManager::claimDriverMutex() { IpcMutex_Lock(m_driverActiveGuardMutex); - auto now = std::chrono::steady_clock::now(); + // Compare against a fixed deadline. Re-reading the clock into the same variable it is compared + // against made the old condition unconditionally true, so there was no timeout and no backoff. + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(5000); bool held = false; - while (!held && now < now + std::chrono::milliseconds(5000)) { - // Spin until we hold it or 5 second timeout + while (!held && std::chrono::steady_clock::now() < deadline) { held = IpcMutex_TryLock(m_driverActiveMutex); + if (!held) { + // Back off rather than spinning a core flat for the whole timeout. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } } IpcMutex_Unlock(m_driverActiveGuardMutex); - return !held; + return held; } void CustomShareManager::releaseDriverMutex() { @@ -213,13 +244,49 @@ bool CustomShareManager::getGazeStatus(hmd2_gaze_status_t *pGazeStatus, int *las } } -void CustomShareManager::setGazeImage(const unsigned char *pGazeImage) { +void CustomShareManager::setGazeImage(const unsigned char *pGazeImage, uint32_t size) { IpcMutex_Lock(m_gazeImageMutex); - m_pBufferData->gazeImage.pushToCircularBuffer(pGazeImage); + m_pBufferData->gazeImage.pushToCircularBuffer(pGazeImage, size); IpcMutex_Unlock(m_gazeImageMutex); IpcBroadcast_NotifyAll(m_gazeImageBroadcast); } +bool CustomShareManager::getGazeImageCopy(unsigned char *pDest, uint32_t destSize, uint32_t *pOutSize, int *lastCounter, uint32_t timeoutMs) { + auto start = std::chrono::steady_clock::now(); + while (true) { + IpcMutex_Lock(m_gazeImageMutex); + int currentCounter = m_pBufferData->gazeImage.counter; + if (!lastCounter || *lastCounter != currentCounter) { + // Copy while still holding the lock: that is the whole point of this entry point over + // getGazeImageBuffer, which hands out a pointer the producer can recycle underneath the caller. + const bool complete = m_pBufferData->gazeImage.copyLatest(pDest, destSize, pOutSize); + if (lastCounter) { + *lastCounter = currentCounter; + } + IpcMutex_Unlock(m_gazeImageMutex); + return complete; + } + IpcMutex_Unlock(m_gazeImageMutex); + + if (timeoutMs == 0) { + if (pOutSize) { + *pOutSize = 0; + } + return false; + } + + auto now = std::chrono::steady_clock::now(); + uint32_t elapsed = static_cast(std::chrono::duration_cast(now - start).count()); + if (elapsed >= timeoutMs) { + if (pOutSize) { + *pOutSize = 0; + } + return false; + } + IpcBroadcast_Wait(m_gazeImageBroadcast, timeoutMs - elapsed); + } +} + bool CustomShareManager::getGazeImageBuffer(unsigned char **gazeImageBuffer, int *lastCounter, uint32_t timeoutMs) { auto start = std::chrono::steady_clock::now(); while (true) { @@ -320,7 +387,11 @@ bool CustomShareManager::submitCommand(DriverCommand &command) { auto start = std::chrono::steady_clock::now(); - while (!ptr->isFulfilled) { + // Acquire, so that whatever the driver wrote into the payload is visible once the flag is observed. + // This is read without holding m_commandMutex, which is why it needs ordering at all. + std::atomic_ref fulfilledFlag(ptr->isFulfilled); + + while (!fulfilledFlag.load(std::memory_order_acquire)) { // Allow up to 5 seconds for the command to be fulfilled IpcBroadcast_Wait(m_commandBroadcast, 5000); @@ -330,9 +401,10 @@ bool CustomShareManager::submitCommand(DriverCommand &command) { } } + const bool fulfilled = fulfilledFlag.load(std::memory_order_acquire); command = *ptr; - return command.isFulfilled; + return fulfilled; } DriverCommand *CustomShareManager::popCommand(uint32_t timeoutMs) { @@ -354,6 +426,7 @@ DriverCommand *CustomShareManager::popCommand(uint32_t timeoutMs) { } void CustomShareManager::fulfillCommand(DriverCommand *command) { - command->isFulfilled = true; + // Release: pair with the acquire in submitCommand so the caller sees the payload we just wrote. + std::atomic_ref(command->isFulfilled).store(true, std::memory_order_release); IpcBroadcast_NotifyAll(m_commandBroadcast); } diff --git a/projects/libcustomshare/custom_share_manager.h b/projects/libcustomshare/custom_share_manager.h index ba33701..c1957d6 100644 --- a/projects/libcustomshare/custom_share_manager.h +++ b/projects/libcustomshare/custom_share_manager.h @@ -8,6 +8,18 @@ constexpr int k_maxSlots = 8; +// Layout/protocol generation for everything in this file. +// +// BufferData is mapped by name into several processes that are built and shipped together. If one of +// them is stale -- an old psvr2_toolkit_capi.dll left behind by a partial install, say -- the two sides +// disagree about field offsets while happily sharing the same memory, and the corruption is silent. +// Suffixing every IPC object name means a mismatched build simply sees nothing instead: the driver +// reads as inactive, which is a symptom someone can actually diagnose. +// +// Bump this whenever BufferData's layout or the command protocol changes. +#define CUSTOM_SHARE_IPC_GENERATION "2" +#define CUSTOM_SHARE_NAME(base) base "_G" CUSTOM_SHARE_IPC_GENERATION + struct GazeStatus { hmd2_gaze_status_t data; int counter; @@ -16,12 +28,20 @@ struct GazeStatus { void get(hmd2_gaze_status_t *pGazeStatus) const; }; +// Slot size: 0x100 header + 0x200000 payload. +constexpr uint32_t k_gazeImageSlotSize = 0x200100; +constexpr int k_gazeImageSlots = 8; + struct GazeImage { int counter; - unsigned char images[0x200100 * 8]; + // Valid byte count per slot. The headset does not always fill the whole slot, and consumers need to + // know how much of it is real. + uint32_t sizes[k_gazeImageSlots]; + unsigned char images[k_gazeImageSlotSize * k_gazeImageSlots]; - void pushToCircularBuffer(const unsigned char *pGazeImage); + void pushToCircularBuffer(const unsigned char *pGazeImage, uint32_t size); int getFromCircularBuffer(unsigned char **gazeImageBuffer); + bool copyLatest(unsigned char *pDest, uint32_t destSize, uint32_t *pOutSize) const; }; struct CommandBuffer { @@ -57,10 +77,15 @@ class CustomShareManager { void setGazeStatus(const hmd2_gaze_status_t *pGazeStatus); bool getGazeStatus(hmd2_gaze_status_t *pGazeStatus, int *lastCounter = nullptr, uint32_t timeoutMs = 0); - void setGazeImage(const unsigned char *pGazeImage); + void setGazeImage(const unsigned char *pGazeImage, uint32_t size); + // Deprecated: returns a pointer into shared memory after releasing the lock, so the producer can + // recycle the slot while the caller is still reading it. Kept for existing clients. bool getGazeImageBuffer(unsigned char **gazeImageBuffer, int *lastCounter = nullptr, uint32_t timeoutMs = 0); + // Copies the newest frame out under the lock. pOutSize receives the valid byte count and may be null. + bool getGazeImageCopy(unsigned char *pDest, uint32_t destSize, uint32_t *pOutSize, int *lastCounter = nullptr, uint32_t timeoutMs = 0); + void signalPcmUpdate(); void readPcm(int slot, unsigned char *pcmLeft, unsigned char *pcmRight); diff --git a/projects/psvr2_gaze_filter_test/CMakeLists.txt b/projects/psvr2_gaze_filter_test/CMakeLists.txt new file mode 100644 index 0000000..a61cfd5 --- /dev/null +++ b/projects/psvr2_gaze_filter_test/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(psvr2_gaze_filter_test main.cpp) + +target_link_libraries(psvr2_gaze_filter_test PRIVATE common) + +add_test(NAME gaze_filter COMMAND psvr2_gaze_filter_test) diff --git a/projects/psvr2_gaze_filter_test/main.cpp b/projects/psvr2_gaze_filter_test/main.cpp new file mode 100644 index 0000000..a5fd137 --- /dev/null +++ b/projects/psvr2_gaze_filter_test/main.cpp @@ -0,0 +1,262 @@ +// Off-device tests for GazeFilter (common/gaze_filter.h). +// +// The point of this filter is to settle fixations WITHOUT adding lag to saccades, so the tests measure +// both properties rather than just checking it runs. See EYE_TRACKING_PLAN.md 5.1. + +#include "gaze_filter.h" + +#include +#include +#include +#include + +namespace { + +int g_failures = 0; + +void Check(bool condition, const std::string &name) { + if (condition) { + std::printf(" pass %s\n", name.c_str()); + } else { + std::printf(" FAIL %s\n", name.c_str()); + ++g_failures; + } +} + +void CheckNear(float actual, float expected, float tolerance, const std::string &name) { + const bool ok = std::fabs(actual - expected) <= tolerance; + if (ok) { + std::printf(" pass %s (%.4f)\n", name.c_str(), actual); + } else { + std::printf(" FAIL %s: got %.4f, expected %.4f +/- %.4f\n", name.c_str(), actual, expected, tolerance); + ++g_failures; + } +} + +constexpr int64_t k_frameIntervalUs = 8333; // ~120 Hz + +// A direction `yawDeg` off straight ahead, in the horizontal plane. +GazeFilterVec3 DirectionAtYaw(float yawDeg) { + const float rad = yawDeg * (3.14159265358979323846f / 180.0f); + return GazeFilterVec3{std::sin(rad), 0.0f, -std::cos(rad)}; +} + +float AngleBetweenDeg(const GazeFilterVec3 &a, const GazeFilterVec3 &b) { + float dot = a.x * b.x + a.y * b.y + a.z * b.z; + dot = dot > 1.0f ? 1.0f : (dot < -1.0f ? -1.0f : dot); + return std::acos(dot) * (180.0f / 3.14159265358979323846f); +} + +// Deterministic pseudo-noise so the test does not depend on a random engine's implementation. +struct Jitter { + uint32_t state = 0x12345678u; + float Next(float amplitudeDeg) { + state = state * 1664525u + 1013904223u; + const float unit = static_cast((state >> 8) & 0xFFFF) / 65535.0f; // [0,1] + return (unit * 2.0f - 1.0f) * amplitudeDeg; + } +}; + +} // namespace + +int main() { + std::printf("GazeFilter\n"); + + // --- Basic behaviour ----------------------------------------------------------------------- + + { + GazeFilter filter; + const GazeFilterOutput out = filter.Update(DirectionAtYaw(0.0f), true, 1000); + Check(out.valid && !out.held, "first sample is reported immediately"); + CheckNear(AngleBetweenDeg(out.direction, DirectionAtYaw(0.0f)), 0.0f, 0.001f, "first sample passes through unchanged"); + } + + { + GazeFilter filter; + const GazeFilterOutput out = filter.Update(DirectionAtYaw(0.0f), false, 1000); + Check(!out.valid, "invalid input with no history reports invalid"); + } + + { + GazeFilter filter; + int64_t t = 0; + for (int i = 0; i < 50; ++i) { + t += k_frameIntervalUs; + filter.Update(DirectionAtYaw(0.0f), true, t); + } + const GazeFilterOutput out = filter.Update(DirectionAtYaw(0.0f), true, t + k_frameIntervalUs); + CheckNear(AngleBetweenDeg(out.direction, DirectionAtYaw(0.0f)), 0.0f, 0.01f, "a perfectly steady input is not dragged off target"); + Check(out.state == GazeMotionState::Fixation, "steady input classifies as fixation"); + } + + // --- Fixation: the filter must actually remove jitter -------------------------------------- + + { + GazeFilter filter; + Jitter noiseSource; + Jitter referenceSource; + + int64_t t = 0; + double filteredError = 0.0; + double rawError = 0.0; + int samples = 0; + + // Warm up first so the comparison is of steady-state behaviour. + for (int i = 0; i < 30; ++i) { + t += k_frameIntervalUs; + filter.Update(DirectionAtYaw(noiseSource.Next(0.3f)), true, t); + referenceSource.Next(0.3f); + } + + for (int i = 0; i < 200; ++i) { + t += k_frameIntervalUs; + const float noiseDeg = noiseSource.Next(0.3f); + const GazeFilterOutput out = filter.Update(DirectionAtYaw(noiseDeg), true, t); + + filteredError += AngleBetweenDeg(out.direction, DirectionAtYaw(0.0f)); + rawError += std::fabs(noiseDeg); + ++samples; + } + + const double filteredMean = filteredError / samples; + const double rawMean = rawError / samples; + + std::printf(" fixation: raw mean error %.4f deg, filtered %.4f deg\n", rawMean, filteredMean); + Check(filteredMean < rawMean * 0.5, "fixation jitter is at least halved"); + } + + // --- Saccades: the filter must NOT add lag -------------------------------------------------- + + { + GazeFilter filter; + int64_t t = 0; + + for (int i = 0; i < 30; ++i) { + t += k_frameIntervalUs; + filter.Update(DirectionAtYaw(0.0f), true, t); + } + + // A 20 degree jump in one frame is ~2400 deg/s, unambiguously a saccade. + t += k_frameIntervalUs; + const GazeFilterOutput out = filter.Update(DirectionAtYaw(20.0f), true, t); + + Check(out.state == GazeMotionState::Saccade, "a fast jump classifies as a saccade"); + CheckNear(AngleBetweenDeg(out.direction, DirectionAtYaw(20.0f)), 0.0f, 0.001f, "saccade target is reached in one frame (no added lag)"); + } + + { + // Slow drift must NOT be treated as a saccade: 5 deg/s is well under the threshold. + GazeFilter filter; + int64_t t = 0; + float yaw = 0.0f; + GazeMotionState state = GazeMotionState::Unknown; + + for (int i = 0; i < 60; ++i) { + t += k_frameIntervalUs; + yaw += 5.0f * (static_cast(k_frameIntervalUs) / 1e6f); + state = filter.Update(DirectionAtYaw(yaw), true, t).state; + } + + Check(state == GazeMotionState::Fixation, "slow drift is not misclassified as a saccade"); + } + + { + // Smooth pursuit at 25 deg/s stays under the default 30 deg/s threshold and should track closely + // rather than lagging far behind. + GazeFilter filter; + int64_t t = 0; + float yaw = 0.0f; + GazeFilterOutput out; + + for (int i = 0; i < 120; ++i) { + t += k_frameIntervalUs; + yaw += 25.0f * (static_cast(k_frameIntervalUs) / 1e6f); + out = filter.Update(DirectionAtYaw(yaw), true, t); + } + + const float lagDeg = AngleBetweenDeg(out.direction, DirectionAtYaw(yaw)); + std::printf(" pursuit: steady-state lag %.4f deg\n", lagDeg); + Check(lagDeg < 3.0f, "smooth pursuit lag stays under 3 degrees"); + } + + // --- Blink hold ----------------------------------------------------------------------------- + + { + GazeFilter filter; + int64_t t = 0; + for (int i = 0; i < 30; ++i) { + t += k_frameIntervalUs; + filter.Update(DirectionAtYaw(10.0f), true, t); + } + + // 100 ms of dropout, inside the default 200 ms hold. + t += 100000; + const GazeFilterOutput held = filter.Update(GazeFilterVec3{0, 0, 0}, false, t); + Check(held.valid && held.held, "direction is held through a short dropout"); + CheckNear(AngleBetweenDeg(held.direction, DirectionAtYaw(10.0f)), 0.0f, 0.5f, "held direction is the last good one"); + + // 300 ms total, past the hold window. + t += 200000; + const GazeFilterOutput dropped = filter.Update(GazeFilterVec3{0, 0, 0}, false, t); + Check(!dropped.valid, "past the hold window the filter reports invalid"); + } + + { + // Recovery after a long gap must not smooth the new sample against stale history. + GazeFilter filter; + int64_t t = 0; + for (int i = 0; i < 30; ++i) { + t += k_frameIntervalUs; + filter.Update(DirectionAtYaw(0.0f), true, t); + } + + t += 5000000; // 5 s + const GazeFilterOutput out = filter.Update(DirectionAtYaw(30.0f), true, t); + CheckNear(AngleBetweenDeg(out.direction, DirectionAtYaw(30.0f)), 0.0f, 0.001f, "sample after a long gap is not blended with stale history"); + } + + // --- Robustness ------------------------------------------------------------------------------ + + { + GazeFilter filter; + const GazeFilterOutput out = filter.Update(GazeFilterVec3{0.0f, 0.0f, 0.0f}, true, 1000); + Check(!out.valid, "a zero-length direction is rejected"); + } + + { + GazeFilter filter; + const float nan = std::nanf(""); + const GazeFilterOutput out = filter.Update(GazeFilterVec3{nan, nan, nan}, true, 1000); + Check(!out.valid, "a non-finite direction is rejected"); + } + + { + // Non-monotonic timestamps must not blow up or produce garbage. + GazeFilter filter; + filter.Update(DirectionAtYaw(0.0f), true, 100000); + const GazeFilterOutput out = filter.Update(DirectionAtYaw(1.0f), true, 50000); + Check(out.valid, "a backwards timestamp is handled without producing an invalid sample"); + } + + { + // Output must stay unit length, since it becomes a ray direction downstream. + GazeFilter filter; + Jitter noiseSource; + int64_t t = 0; + float worstDeviation = 0.0f; + + for (int i = 0; i < 200; ++i) { + t += k_frameIntervalUs; + const GazeFilterOutput out = filter.Update(DirectionAtYaw(noiseSource.Next(5.0f)), true, t); + if (out.valid) { + const float length = std::sqrt(out.direction.x * out.direction.x + out.direction.y * out.direction.y + out.direction.z * out.direction.z); + worstDeviation = std::fmax(worstDeviation, std::fabs(length - 1.0f)); + } + } + + CheckNear(worstDeviation, 0.0f, 0.001f, "output stays normalised"); + } + + std::printf("\n%s (%d failure%s)\n", g_failures == 0 ? "PASSED" : "FAILED", g_failures, g_failures == 1 ? "" : "s"); + return g_failures == 0 ? 0 : 1; +} diff --git a/projects/psvr2_gen_data_parser_test/CMakeLists.txt b/projects/psvr2_gen_data_parser_test/CMakeLists.txt new file mode 100644 index 0000000..6d72b2e --- /dev/null +++ b/projects/psvr2_gen_data_parser_test/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(psvr2_gen_data_parser_test main.cpp) + +target_link_libraries(psvr2_gen_data_parser_test PRIVATE common) + +add_test(NAME gen_data_parser COMMAND psvr2_gen_data_parser_test) diff --git a/projects/psvr2_gen_data_parser_test/main.cpp b/projects/psvr2_gen_data_parser_test/main.cpp new file mode 100644 index 0000000..d961bdf --- /dev/null +++ b/projects/psvr2_gen_data_parser_test/main.cpp @@ -0,0 +1,235 @@ +// Off-device tests for Hmd2ParseGenData (common/hmd2_gen_data.h). +// +// This payload arrives straight off the USB general-data pipe, so the parser is the driver's trust +// boundary for it. The cases below pin the three bounds defects fixed in EYE_TRACKING_PLAN.md 1.2: +// a block size of zero spinning the walk forever, an item table read past the end of its block, and +// an item extent checked against the wrong base (and able to wrap a uint32_t). + +#include "hmd2_gen_data.h" + +#include +#include +#include +#include + +namespace { + +int g_failures = 0; + +void Check(bool condition, const std::string &name) { + if (condition) { + std::printf(" pass %s\n", name.c_str()); + } else { + std::printf(" FAIL %s\n", name.c_str()); + ++g_failures; + } +} + +constexpr uint32_t k_blockSize = 0x200; + +struct ItemSpec { + uint16_t id = k_hmd2GenDataItemIdGazeCalib; + uint32_t offset = 0; + uint32_t size = 0; +}; + +// Builds one 0x200-byte "VD" block with the given item table. Fields a test wants to corrupt are +// patched afterwards through HeaderAt. +std::vector MakeBlock(const std::vector &items) { + std::vector block(k_blockSize, 0); + + Hmd2GenDataHeader header = {}; + header.magic[0] = 'V'; + header.magic[1] = 'D'; + header.minSize = static_cast(k_hmd2GenDataBlockGranularity); + header.size = k_blockSize; + header.numItems = static_cast(items.size()); + std::memcpy(block.data(), &header, sizeof(header)); + + for (size_t i = 0; i < items.size(); ++i) { + Hmd2GenDataItem item = {}; + item.id = items[i].id; + item.size = items[i].size; + item.offset = items[i].offset; + std::memcpy(block.data() + sizeof(Hmd2GenDataHeader) + i * sizeof(Hmd2GenDataItem), &item, sizeof(item)); + } + + return block; +} + +// Writes a calibration header of the shape the driver would accept into the block at the given offset. +void PlaceValidCalibPayload(std::vector &block, uint32_t offset) { + Hmd2GazeCalibHeader calib = {}; + calib.magic[0] = 'V'; + calib.magic[1] = 'C'; + calib.calib_id = 42; + calib.calib_eye = HMD2_GAZE_ENABLED_EYE_BOTH; + std::memcpy(block.data() + offset, &calib, sizeof(calib)); +} + +Hmd2GenDataHeader *HeaderAt(std::vector &buffer, size_t blockIndex) { + return reinterpret_cast(buffer.data() + blockIndex * k_blockSize); +} + +struct ParseOutcome { + int result = 0; + int calibItems = 0; + uint32_t lastSize = 0; +}; + +ParseOutcome Parse(std::vector &buffer) { + ParseOutcome outcome; + outcome.result = Hmd2ParseGenData(buffer.data(), static_cast(buffer.size()), [&](char *, uint32_t size) { + ++outcome.calibItems; + outcome.lastSize = size; + }); + return outcome; +} + +} // namespace + +int main() { + std::printf("Hmd2ParseGenData\n"); + + // --- Well-formed input --------------------------------------------------------------------- + + { + std::vector empty; + ParseOutcome outcome = Parse(empty); + Check(outcome.result == 0 && outcome.calibItems == 0, "empty payload parses as a no-op"); + } + + { + std::vector buffer = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x100, 0x40}}); + PlaceValidCalibPayload(buffer, 0x100); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == 0 && outcome.calibItems == 1 && outcome.lastSize == 0x40, "single block with one calibration item"); + } + + { + std::vector buffer = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x100, 0x40}}); + std::vector second = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x120, 0x30}}); + buffer.insert(buffer.end(), second.begin(), second.end()); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == 0 && outcome.calibItems == 2, "two blocks both walked"); + } + + { + std::vector buffer = MakeBlock({{0x1, 0x100, 0x40}}); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == 0 && outcome.calibItems == 0, "non-calibration item ids are skipped"); + } + + // --- Forward progress ---------------------------------------------------------------------- + // A zero-size block previously advanced the walk by nothing, hanging the USB data thread. + + { + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->size = 0; + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "block with size 0 is rejected (does not hang)"); + } + + { + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->size = sizeof(Hmd2GenDataHeader) - 1; + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "block too small to contain its own header is rejected"); + } + + // --- Item table bounds --------------------------------------------------------------------- + // numItems was previously trusted, so the table could be read far past the end of its block. + + { + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->numItems = 0xFFFF; + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "item table larger than its block is rejected"); + } + + { + // (0x200 - 32) / 12 == 40 entries fit exactly; 41 does not. + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->numItems = 41; + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "item table one entry past the block end is rejected"); + } + + { + // The other side of that boundary: 40 entries fit exactly and must still be accepted. + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->numItems = 40; + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == 0, "item table of exactly 40 entries is accepted"); + } + + // --- Item extent bounds -------------------------------------------------------------------- + + { + std::vector buffer = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x1F0, 0x40}}); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "item extending past its block end is rejected"); + } + + { + // offset + size wraps a uint32_t, which slipped through the old addition-based check. + std::vector buffer = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0xFFFFFFF0u, 0x20}}); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "item offset + size overflow is rejected"); + } + + { + // The regression that motivated the fix: item offsets are block-relative, but the old check + // compared them against the whole payload size. In the second block that was too permissive by + // exactly one block, letting a read run off the end of the buffer. + std::vector buffer = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x100, 0x40}}); + std::vector second = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x1F0, 0x20}}); + buffer.insert(buffer.end(), second.begin(), second.end()); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == -1, "second block item is bounded by its own block, not the payload"); + } + + { + // Same shape, but the item genuinely fits. This must still be accepted. + std::vector buffer = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x100, 0x40}}); + std::vector second = MakeBlock({{k_hmd2GenDataItemIdGazeCalib, 0x1C0, 0x40}}); + buffer.insert(buffer.end(), second.begin(), second.end()); + ParseOutcome outcome = Parse(buffer); + Check(outcome.result == 0 && outcome.calibItems == 2, "second block item ending exactly at the block end is accepted"); + } + + // --- Header validation --------------------------------------------------------------------- + + { + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->magic[1] = 'X'; + Check(Parse(buffer).result == -1, "bad magic is rejected"); + } + + { + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->minSize = 0x100; + Check(Parse(buffer).result == -1, "bad minSize is rejected"); + } + + { + std::vector buffer = MakeBlock({}); + HeaderAt(buffer, 0)->size = k_blockSize * 2; + Check(Parse(buffer).result == -1, "block claiming more bytes than remain is rejected"); + } + + { + std::vector buffer(0x100, 0); + Check(Parse(buffer).result == -1, "payload shorter than one block granularity is rejected"); + } + + { + // A trailing partial block after a valid one is an error, not a silent truncation. + std::vector buffer = MakeBlock({}); + buffer.resize(k_blockSize + 0x80, 0); + Check(Parse(buffer).result == -1, "trailing partial block is rejected"); + } + + std::printf("\n%s (%d failure%s)\n", g_failures == 0 ? "PASSED" : "FAILED", g_failures, g_failures == 1 ? "" : "s"); + return g_failures == 0 ? 0 : 1; +} diff --git a/projects/psvr2_openvr_driver_ex/command_thread.cpp b/projects/psvr2_openvr_driver_ex/command_thread.cpp index 685d9b7..2bc6af3 100644 --- a/projects/psvr2_openvr_driver_ex/command_thread.cpp +++ b/projects/psvr2_openvr_driver_ex/command_thread.cpp @@ -5,6 +5,7 @@ #include "trigger_effect_manager.h" #include "command_thread.h" +#include "util.h" namespace psvr2_toolkit { std::atomic CommandThread::m_running{false}; @@ -29,7 +30,15 @@ void CommandThread::Stop() { void CommandThread::ThreadLoop() { CustomShareManager *customShareManager = CustomShareManager::getSingleton(); - customShareManager->claimDriverMutex(); + // Losing this race must not take the thread down with it. The mutex only backs the "driver active" + // flag that clients poll, whereas TriggerEffectManager::Update below drives controller trigger + // effects every ~10 ms and has to keep running either way. The original code span here until it won + // and never reported failure, so bailing out on a 5 second timeout would have been a regression. + const bool holdsDriverMutex = customShareManager->claimDriverMutex(); + if (!holdsDriverMutex) { + Util::DriverLog("[CommandThread] Could not claim the driver-active mutex within the timeout. Clients will not " + "see the driver as active, but trigger effects will still run."); + } while (m_running) { DriverCommand *command = customShareManager->popCommand(10); @@ -75,6 +84,10 @@ void CommandThread::ThreadLoop() { } } - customShareManager->releaseDriverMutex(); + // Only release what we actually took: unlocking a mutex this thread never owned would hand it away + // from whichever process does own it. + if (holdsDriverMutex) { + customShareManager->releaseDriverMutex(); + } } } // namespace psvr2_toolkit \ No newline at end of file diff --git a/projects/psvr2_openvr_driver_ex/driver_hooks/caesar_manager_hooks.cpp b/projects/psvr2_openvr_driver_ex/driver_hooks/caesar_manager_hooks.cpp index bf9b2bc..5cf27ac 100644 --- a/projects/psvr2_openvr_driver_ex/driver_hooks/caesar_manager_hooks.cpp +++ b/projects/psvr2_openvr_driver_ex/driver_hooks/caesar_manager_hooks.cpp @@ -14,21 +14,40 @@ void *(*Framework__Thread__start)(void *thisptr) = nullptr; void *(*CaesarManager__initialize)(CaesarManager *, void *, void *) = nullptr; void *CaesarManager__initializeHook(CaesarManager *thisptr, void *arg1, void *arg2) { + Util::DriverLog("[Gaze] CaesarManager::initialize hook fired."); void *result = CaesarManager__initialize(thisptr, arg1, arg2); caesarUsbThreadGaze.Start(0); Framework__Thread__start(&caesarUsbThreadGaze); + + // Firmware-state reset, restored from the disabled setupManager hook below. + // + // That hook was installed at THIS function's address, so PolyHook chained the two: setupManagerHook + // wrapped this one and cleared these fields after initialize returned. It ran on every initialize, + // just not where its name suggested. Disabling it in isolation therefore removed a reset that was + // genuinely happening -- and these two lines are the only place in the toolkit that writes them. + // + // Reproduced here in the same order (after the original call) so behaviour is unchanged. + thisptr->firmwareLoaded = false; + thisptr->firmwareVersion = 0; + return result; } void (*CaesarManager__shutdown)(void *) = nullptr; void CaesarManager__shutdownHook(CaesarManager *thisptr) { + Util::DriverLog("[Gaze] CaesarManager::shutdown hook fired."); caesarUsbThreadGaze.JoinThread(); CaesarManager__shutdown(thisptr); } +// Currently unreferenced: see the TODO in InstallHooks below. Kept so that re-enabling the hook is a +// one-line change once the correct RVA is known. Note that its body is duplicated into +// CaesarManager__initializeHook above, which is where that work actually used to happen -- drop the +// duplicate there if this hook is ever pointed at the real setupManager. void *(*CaesarManager__setupManager)(CaesarManager *, void *, void *) = nullptr; void *CaesarManager__setupManagerHook(CaesarManager *thisptr, void *arg1, void *arg2) { + Util::DriverLog("[Gaze] CaesarManager::setupManager hook fired."); void *result = CaesarManager__setupManager(thisptr, arg1, arg2); thisptr->firmwareLoaded = false; @@ -42,7 +61,7 @@ void CaesarManagerHooks::InstallHooks() { Framework__Thread__start = decltype(Framework__Thread__start)(pHmdDriverLoader->GetBaseAddress() + 0x16B660); - if (!DriverSettings::GetBool(STEAMVR_SETTINGS_DISABLE_GAZE, SETTING_DISABLE_GAZE_DEFAULT_VALUE)) { + if (DriverSettings::IsGazeEnabled()) { Util::DriverLog("Enabling PSVR2 gaze tracking..."); // CaesarManager::initialize HookLib::InstallHook(reinterpret_cast(pHmdDriverLoader->GetBaseAddress() + 0x123130), reinterpret_cast(CaesarManager__initializeHook), @@ -52,9 +71,20 @@ void CaesarManagerHooks::InstallHooks() { HookLib::InstallHook(reinterpret_cast(pHmdDriverLoader->GetBaseAddress() + 0x128320), reinterpret_cast(CaesarManager__shutdownHook), reinterpret_cast(&CaesarManager__shutdown)); - // CaesarManager::setupManager - HookLib::InstallHook(reinterpret_cast(pHmdDriverLoader->GetBaseAddress() + 0x123130), reinterpret_cast(CaesarManager__setupManagerHook), - reinterpret_cast(&CaesarManager__setupManager)); + // TODO(EYE_TRACKING_PLAN.md 1.1): CaesarManager::setupManager + // + // This was installed at 0x123130 -- the same address as CaesarManager::initialize above -- which + // is a copy-paste of the RVA on the line above it. The effect was that the firmware-state reset in + // CaesarManager__setupManagerHook never ran where it was meant to, and PolyHook was asked to detour + // an already-detoured address. + // + // The hook is disabled rather than left pointing at the wrong function: an absent hook is honest, + // a mis-addressed one is not. Re-enable once the real setupManager RVA has been derived from the + // shipped driver binary. + // + // HookLib::InstallHook(reinterpret_cast(pHmdDriverLoader->GetBaseAddress() + 0x), + // reinterpret_cast(CaesarManager__setupManagerHook), + // reinterpret_cast(&CaesarManager__setupManager)); } } diff --git a/projects/psvr2_openvr_driver_ex/driver_hooks/hmd_device_hooks.cpp b/projects/psvr2_openvr_driver_ex/driver_hooks/hmd_device_hooks.cpp index e531e1e..6370c88 100644 --- a/projects/psvr2_openvr_driver_ex/driver_hooks/hmd_device_hooks.cpp +++ b/projects/psvr2_openvr_driver_ex/driver_hooks/hmd_device_hooks.cpp @@ -1,6 +1,7 @@ #include "driver_interface/caesar_manager.h" #include "driver_interface/share_manager.h" #include "driver_host_proxy.h" +#include "gaze_filter.h" #include "hmd2_gaze.h" #include "hmd_device_camera.h" #include "hmd_device_hooks.h" @@ -210,15 +211,24 @@ vr::EVRInitError sie__psvr2__HmdDevice__ActivateHook(void *thisptr, uint32_t unO // Tell SteamVR our dashboard scale. vr::VRProperties()->SetFloatProperty(ulPropertyContainer, vr::Prop_DashboardScale_Float, .9f); - vr::VRProperties()->SetBoolProperty(ulPropertyContainer, vr::Prop_SupportsXrEyeGazeInteraction_Bool, true); - - if (vr::VRDriverInput()) { - vr::EVRInputError result = (vr::VRDriverInput())->CreateEyeTrackingComponent(ulPropertyContainer, "/eyetracking", &eyeTrackingComponent); - if (result != vr::VRInputError_None) { - vr::VRDriverLog()->Log("Failed to create eye tracking component."); + // Only advertise eye gaze when the pipeline feeding it is actually running. With disableGaze set, + // CaesarManagerHooks never installs the gaze hooks, so the component would exist and never update -- + // applications would light up gaze-driven UI that then never moves, which is worse for them than + // being told there is no eye tracking at all. + if (DriverSettings::IsGazeEnabled()) { + vr::VRProperties()->SetBoolProperty(ulPropertyContainer, vr::Prop_SupportsXrEyeGazeInteraction_Bool, true); + + if (vr::VRDriverInput()) { + // Deliberately not named 'result': that would shadow the EVRInitError this function returns. + vr::EVRInputError eyeTrackingError = (vr::VRDriverInput())->CreateEyeTrackingComponent(ulPropertyContainer, "/eyetracking", &eyeTrackingComponent); + if (eyeTrackingError != vr::VRInputError_None) { + vr::VRDriverLog()->Log("Failed to create eye tracking component."); + } + } else { + vr::VRDriverLog()->Log("Failed to get driver input interface. Are you on the latest version of SteamVR?"); } } else { - vr::VRDriverLog()->Log("Failed to get driver input interface. Are you on the latest version of SteamVR?"); + Util::DriverLog("[Gaze] disableGaze is set; eye tracking will not be advertised."); } return result; @@ -253,6 +263,63 @@ inline const int64_t GetHostTimestamp() { return static_cast((static_cast(now.QuadPart) / static_cast(frequency.QuadPart)) * 1e6); } +// Smoothing for the SteamVR gaze path only. The C API and the legacy IPC server keep delivering raw +// samples: clients doing their own processing need the unfiltered stream, and silently filtering it +// would be a breaking behavioural change for them. +// +// UpdateGaze is only ever called from the single gaze USB thread, so this needs no locking. +static GazeFilter s_gazeFilter; + +static bool IsGazeFilterEnabled() { + static const bool enabled = DriverSettings::GetBool(STEAMVR_SETTINGS_GAZE_FILTER_ENABLED, SETTING_GAZE_FILTER_ENABLED_DEFAULT_VALUE); + return enabled; +} + +// Reads the filter tuning once, on the first gaze frame. Settings are not re-read afterwards, matching +// how the rest of the driver treats them. +static GazeFilter &ConfiguredGazeFilter() { + static const bool configured = [] { + GazeFilterConfig config; + config.minCutoffHz = DriverSettings::GetFloat(STEAMVR_SETTINGS_GAZE_FILTER_MIN_CUTOFF, SETTING_GAZE_FILTER_MIN_CUTOFF_DEFAULT_VALUE); + config.beta = DriverSettings::GetFloat(STEAMVR_SETTINGS_GAZE_FILTER_BETA, SETTING_GAZE_FILTER_BETA_DEFAULT_VALUE); + config.saccadeVelocityDegPerSec = + DriverSettings::GetFloat(STEAMVR_SETTINGS_GAZE_FILTER_SACCADE_DEG_PER_SEC, SETTING_GAZE_FILTER_SACCADE_DEG_PER_SEC_DEFAULT_VALUE); + config.blinkHoldUs = static_cast(DriverSettings::GetInt32(STEAMVR_SETTINGS_GAZE_BLINK_HOLD_MS, SETTING_GAZE_BLINK_HOLD_MS_DEFAULT_VALUE)) * 1000; + + s_gazeFilter.Configure(config); + + Util::DriverLog("[Gaze] Filter enabled: minCutoff={} Hz, beta={} Hz per deg/s, saccade={} deg/s, blinkHold={} us.", config.minCutoffHz, config.beta, + config.saccadeVelocityDegPerSec, config.blinkHoldUs); + return true; + }(); + (void)configured; + return s_gazeFilter; +} + +// Distance at which to place the fixation point, in metres. +// +// The headset reports convergence distance in the foveated block, while the validity flag for it lives +// in the wearable block. Falls back to a configured default when it is unusable, and clamps so that a +// bad reading cannot put the fixation point behind the viewer or at infinity. +static float GetFixationDistanceMeters(const hmd2_gaze_status_t &gazeState) { + static const float s_defaultDistanceM = + DriverSettings::GetFloat(STEAMVR_SETTINGS_GAZE_DEFAULT_FIXATION_DISTANCE_M, SETTING_GAZE_DEFAULT_FIXATION_DISTANCE_M_DEFAULT_VALUE); + + constexpr float k_minFixationDistanceM = 0.1f; + constexpr float k_maxFixationDistanceM = 20.0f; + + if (gazeState.wearable.is_convergence_distance_valid != HMD2_GAZE_BOOL_TRUE) { + return s_defaultDistanceM; + } + + const float distanceM = gazeState.foveated.convergence_distance_mm / 1000.0f; + if (!std::isfinite(distanceM) || distanceM < k_minFixationDistanceM || distanceM > k_maxFixationDistanceM) { + return s_defaultDistanceM; + } + + return distanceM; +} + void HmdDeviceHooks::UpdateGaze(void *pData, size_t dwSize) { if (eyeTrackingComponent == vr::k_ulInvalidInputComponentHandle) { return; @@ -261,17 +328,55 @@ void HmdDeviceHooks::UpdateGaze(void *pData, size_t dwSize) { hmd2_gaze_status_t *pGazeState = reinterpret_cast(pData); vr::VREyeTrackingData_t eyeTrackingData{}; - bool valid = pGazeState->wearable.is_gaze_dir_combined_valid; - - eyeTrackingData.bActive = valid; - eyeTrackingData.bTracked = valid; - eyeTrackingData.bValid = valid; + const bool dirValid = pGazeState->wearable.is_gaze_dir_combined_valid == HMD2_GAZE_BOOL_TRUE; + const bool originValid = pGazeState->wearable.is_gaze_origin_combined_valid == HMD2_GAZE_BOOL_TRUE; auto &origin = pGazeState->wearable.gaze_origin_combined_mm; auto &direction = pGazeState->wearable.gaze_dir_combined_norm; - eyeTrackingData.vGazeOrigin = vr::HmdVector3_t{-origin.x / 1000.0f, origin.y / 1000.0f, -origin.z / 1000.0f}; - eyeTrackingData.vGazeTarget = vr::HmdVector3_t{-direction.x, direction.y, -direction.z}; + // Smooth in headset space, before the axis flip, so the filter sees the same frame the hardware + // reports in. The blink hold can keep this valid for a short while after dirValid has gone false. + GazeFilterVec3 filteredDirection{direction.x, direction.y, direction.z}; + bool sampleValid = dirValid; + + if (IsGazeFilterEnabled()) { + const GazeFilterOutput filtered = ConfiguredGazeFilter().Update(filteredDirection, dirValid, static_cast(pGazeState->wearable.timestamp)); + filteredDirection = filtered.direction; + sampleValid = filtered.valid; + } + + // The eyeball centre barely moves, so holding the last good origin across a blink is what makes the + // direction hold above usable: without it the ray would be valid but originless. + static vr::HmdVector3_t s_lastValidOriginMeters{}; + static bool s_haveValidOrigin = false; + + vr::HmdVector3_t originMeters{-origin.x / 1000.0f, origin.y / 1000.0f, -origin.z / 1000.0f}; + if (originValid) { + s_lastValidOriginMeters = originMeters; + s_haveValidOrigin = true; + } else if (s_haveValidOrigin) { + originMeters = s_lastValidOriginMeters; + } + + // bActive means "the tracker is running", which stays true across a momentary dropout. bTracked and + // bValid describe this particular sample, and a sample is only usable if both halves of the ray are: + // the origin feeds the target below, so a bad origin would corrupt both fields. + eyeTrackingData.bActive = dirValid; + eyeTrackingData.bTracked = sampleValid && s_haveValidOrigin; + eyeTrackingData.bValid = sampleValid && s_haveValidOrigin; + + // SteamVR's x and z run opposite to the headset's. + const vr::HmdVector3_t directionMeters{-filteredDirection.x, filteredDirection.y, -filteredDirection.z}; + + // vGazeTarget is a fixation POINT in the same space as vGazeOrigin, not a direction. Writing the + // unit direction vector here left every consumer computing normalize(target - origin) with a ray + // skewed by the ~30 mm eye offset against a 1 m vector -- on the order of two degrees of error. + const float fixationDistanceM = GetFixationDistanceMeters(*pGazeState); + + eyeTrackingData.vGazeOrigin = originMeters; + eyeTrackingData.vGazeTarget = + vr::HmdVector3_t{originMeters.v[0] + directionMeters.v[0] * fixationDistanceM, originMeters.v[1] + directionMeters.v[1] * fixationDistanceM, + originMeters.v[2] + directionMeters.v[2] * fixationDistanceM}; int64_t hmdToHostOffset; diff --git a/projects/psvr2_openvr_driver_ex/driver_hooks/usb_thread_hooks.cpp b/projects/psvr2_openvr_driver_ex/driver_hooks/usb_thread_hooks.cpp index d45cb73..7479350 100644 --- a/projects/psvr2_openvr_driver_ex/driver_hooks/usb_thread_hooks.cpp +++ b/projects/psvr2_openvr_driver_ex/driver_hooks/usb_thread_hooks.cpp @@ -2,15 +2,20 @@ #include "../driver_interface/caesar_manager.h" #include "hmd2_gaze.h" +#include "hmd2_gen_data.h" #include "hmd_device_camera.h" #include "hmd_driver_loader.h" #include "utils/hook_lib.h" +#include "utils/driver_settings.h" #include "util.h" #include "custom_share_manager.h" #include +#include #include #include +#include +#include namespace psvr2_toolkit { struct image_data { @@ -33,107 +38,99 @@ struct CaesarUsbThreadImage { image_data image_data; }; -#pragma pack(push, 1) -struct Hmd2GenDataHeader { - char magic[2]; - uint16_t minSize; - uint32_t size; - uint16_t numItems; - uint8_t padding[22]; -}; - -struct Hmd2GenDataItem { - uint16_t id; - uint16_t unk1; - uint32_t size; - uint32_t offset; -}; - -struct Hmd2GazeCalibHeader { - char magic[2]; - uint16_t struct_version; - uint16_t data_version; - uint16_t data_id; - uint32_t payload_size; - uint32_t calib_id; - hmd2_gaze_enabled_eye_t calib_eye; - uint8_t padding[12]; -}; -#pragma pack(pop) +// Resolves the SteamVR user config directory and appends the calibration blob filename. +static bool GetGazeCalibBlobPath(std::filesystem::path &outPath) { + vr::ETrackedPropertyError err; + vr::PropertyContainerHandle_t container = vr::VRDriverHandle(); + uint32_t propSize = vr::VRProperties()->GetStringProperty(container, vr::Prop_UserConfigPath_String, nullptr, 0, &err); -int customHandleData(char *buffer, uint32_t bufferSize) { - uint32_t actualBufferSize = bufferSize; - - if (!bufferSize) { - return 0; + if (propSize == 0) { + return false; } - while (actualBufferSize > 0) { - if (actualBufferSize < 0x200) { - return -1; - } - - Hmd2GenDataHeader *header = reinterpret_cast(buffer); + std::string configPath(propSize - 1, '\0'); + vr::VRProperties()->GetStringProperty(container, vr::Prop_UserConfigPath_String, configPath.data(), propSize, &err); - if (header->magic[0] != 'V' || header->magic[1] != 'D' || header->size > actualBufferSize || header->minSize != 0x200) { - return -1; - } - - if (header->numItems != 0) { - Hmd2GenDataItem *items = reinterpret_cast(buffer + sizeof(Hmd2GenDataHeader)); + if (err != vr::TrackedProp_Success) { + return false; + } - for (uint16_t i = 0; i < header->numItems; ++i) { - Hmd2GenDataItem *item = &items[i]; + outPath = std::filesystem::path(configPath) / std::string(HMD2_GAZE_CALIB_BLOB_FILENAME); + return true; +} - if (item->offset + item->size > bufferSize) { - return -1; - } +// True when the file already holds exactly these bytes. +static bool GazeCalibBlobMatchesDisk(const std::filesystem::path &filePath, const char *data, uint32_t size) { + std::error_code ec; + const auto existingSize = std::filesystem::file_size(filePath, ec); + if (ec || existingSize != size) { + return false; + } - if (item->id == 0x3) - do { - char *data = buffer + item->offset; - Hmd2GazeCalibHeader *calibHeader = reinterpret_cast(data); + std::ifstream inFile(filePath, std::ios::binary); + if (!inFile.is_open()) { + return false; + } - // Avoid saving any blank or invalid calibration data - if (calibHeader->calib_eye != hmd2_gaze_enabled_eye_t::HMD2_GAZE_ENABLED_EYE_BOTH || calibHeader->calib_id == 0) { - break; - } + std::vector existing(size); + if (!inFile.read(existing.data(), size)) { + return false; + } - vr::ETrackedPropertyError err; - vr::PropertyContainerHandle_t container = vr::VRDriverHandle(); - uint32_t propSize = vr::VRProperties()->GetStringProperty(container, vr::Prop_UserConfigPath_String, nullptr, 0, &err); + return std::memcmp(existing.data(), data, size) == 0; +} - if (propSize == 0) { - break; - } +// Persists the gaze calibration blob the headset hands us so it can be replayed on the next connect. +// The block walking and bounds checking live in Hmd2ParseGenData (common/hmd2_gen_data.h) so that they +// can be tested without a headset; everything specific to this driver stays here. +int customHandleData(char *buffer, uint32_t bufferSize) { + return Hmd2ParseGenData(buffer, bufferSize, [](char *data, uint32_t size) { + // Avoid saving any blank or invalid calibration data. + if (!Hmd2IsGazeCalibBlobValid(data, size)) { + return; + } - std::string configPath(propSize - 1, '\0'); - vr::VRProperties()->GetStringProperty(container, vr::Prop_UserConfigPath_String, configPath.data(), propSize, &err); + std::filesystem::path filePath; + if (!GetGazeCalibBlobPath(filePath)) { + return; + } - if (err != vr::TrackedProp_Success) { - break; - } + // The headset re-sends this blob on every connect, so skip the write unless it actually changed. + // That removes essentially all of the I/O this hook would otherwise do on the USB data thread. + if (GazeCalibBlobMatchesDisk(filePath, data, size)) { + return; + } - std::filesystem::path dir(configPath); - std::filesystem::path filePath = dir / std::string("gaze_calibration_blob.bin"); + // Write to a temporary and rename over the target. Truncating the real file in place means a crash + // or power loss between truncate and write leaves the user with no usable calibration. + std::filesystem::path tempPath = filePath; + tempPath += ".tmp"; - std::ofstream outFile(filePath, std::ios::binary); - if (!outFile.is_open()) { - break; - } + { + std::ofstream outFile(tempPath, std::ios::binary | std::ios::trunc); + if (!outFile.is_open()) { + Util::DriverLog("[Gaze] Could not open {} for writing.", tempPath.string()); + return; + } - outFile.write(data, item->size); - outFile.close(); - } while (false); + outFile.write(data, size); + outFile.flush(); + if (!outFile.good()) { + Util::DriverLog("[Gaze] Failed writing calibration blob to {}.", tempPath.string()); + return; } } - uint32_t size = header->size; - buffer += size; - actualBufferSize -= size; - } + std::error_code ec; + std::filesystem::rename(tempPath, filePath, ec); + if (ec) { + Util::DriverLog("[Gaze] Could not replace {}: {}", filePath.string(), ec.message()); + std::filesystem::remove(tempPath, ec); + return; + } - return 0; + Util::DriverLog("[Gaze] Saved calibration blob ({} bytes).", size); + }); } int (*CaesarUsbThreadGenData__handleData)(void *, char *, uint32_t) = nullptr; @@ -143,6 +140,39 @@ int CaesarUsbThreadGenData__handleDataHook(void *thisptr, char *buffer, uint32_t return result; } +static bool IsGazeImageStreamEnabled() { + // Function-local so the lookup happens on the first frame rather than at static init, by which point + // VRSettings is guaranteed to be up. + static const bool enabled = DriverSettings::GetBool(STEAMVR_SETTINGS_GAZE_IMAGE_STREAM_ENABLED, SETTING_GAZE_IMAGE_STREAM_ENABLED_DEFAULT_VALUE); + return enabled; +} + +// How much of the fixed-size image slot is actually worth copying. This runs on the USB thread while +// holding the share mutex, so copying 2 MB when the frame is smaller is not free. +// +// The precise meaning of total_size has NOT been confirmed against hardware yet, so anything outside a +// sane range falls back to copying the whole slot -- a wasted copy is recoverable, a truncated IR frame +// silently corrupts the eyelid work in phase 4.1. The first value observed is logged so that the +// assumption can be checked on-device. +static uint32_t GetGazeImageCopySize(const image_data &img) { + constexpr uint32_t k_headerSize = 0x100; + + static bool s_logged = false; + if (!s_logged) { + s_logged = true; + const uint32_t observedTotalSize = img.total_size; + const uint32_t observedCustomDataSize = img.custom_data_size; + Util::DriverLog("[Gaze] First gaze image: total_size={}, custom_data_size={}, slot={} bytes.", observedTotalSize, observedCustomDataSize, + k_gazeImageSlotSize); + } + + if (img.total_size <= k_headerSize || img.total_size > k_gazeImageSlotSize) { + return k_gazeImageSlotSize; + } + + return img.total_size; +} + int (*CaesarUsbThreadImage__poll)(void *thisptr) = nullptr; int CaesarUsbThreadImage__pollHook(void *thisptr) { int result = CaesarUsbThreadImage__poll(thisptr); @@ -151,7 +181,9 @@ int CaesarUsbThreadImage__pollHook(void *thisptr) { CaesarUsbThreadImage *a1 = (CaesarUsbThreadImage *)thisptr; if (a1->image_data.magic[0] == 'V' && a1->image_data.magic[1] == 'I') { if (a1->image_data.image_type == 6) { - CustomShareManager::getSingleton()->setGazeImage((unsigned char *)&a1->image_data); + if (IsGazeImageStreamEnabled()) { + CustomShareManager::getSingleton()->setGazeImage((unsigned char *)&a1->image_data, GetGazeImageCopySize(a1->image_data)); + } } else if (a1->image_data.image_type == 11) { static HmdDeviceCamera *pHmdDeviceCamera = HmdDeviceCamera::Instance(); diff --git a/projects/psvr2_openvr_driver_ex/usb_thread_gaze.cpp b/projects/psvr2_openvr_driver_ex/usb_thread_gaze.cpp index ac9ed4c..71b7675 100644 --- a/projects/psvr2_openvr_driver_ex/usb_thread_gaze.cpp +++ b/projects/psvr2_openvr_driver_ex/usb_thread_gaze.cpp @@ -1,7 +1,9 @@ #include "driver_hooks/hmd_device_hooks.h" #include "driver_interface/caesar_manager.h" #include "custom_share_manager.h" +#include "hmd2_gen_data.h" #include "usb_thread_gaze.h" +#include "util.h" #include #include @@ -32,16 +34,28 @@ void CaesarUsbThreadGaze::OnConnected() { if (err == vr::TrackedProp_Success) { std::filesystem::path dir(configPath); - std::filesystem::path filePath = dir / "gaze_calibration_blob.bin"; + std::filesystem::path filePath = dir / HMD2_GAZE_CALIB_BLOB_FILENAME; std::ifstream inFile(filePath, std::ios::binary | std::ios::ate); if (inFile.is_open()) { std::streamsize size = inFile.tellg(); inFile.seekg(0, std::ios::beg); - std::vector buffer(size); - if (inFile.read(buffer.data(), size)) { - this->TransferPipe(5, buffer.data(), size); + // Nothing validated this file before uploading it to the headset: no size cap, no header check, + // and no check of the transfer result. A truncated or corrupt blob could be sent straight in. + if (size < static_cast(sizeof(Hmd2GazeCalibHeader)) || size > static_cast(k_hmd2GazeCalibMaxBlobSize)) { + Util::DriverLog("[Gaze] Calibration blob size {} is out of range; ignoring it.", static_cast(size)); + } else { + std::vector buffer(static_cast(size)); + if (!inFile.read(buffer.data(), size)) { + Util::DriverLog("[Gaze] Could not read the calibration blob."); + } else if (!Hmd2IsGazeCalibBlobValid(buffer.data(), static_cast(size))) { + Util::DriverLog("[Gaze] Calibration blob failed validation; ignoring it."); + } else if (this->TransferPipe(5, buffer.data(), buffer.size()) < 0) { + Util::DriverLog("[Gaze] Failed to upload the calibration blob to the headset."); + } else { + Util::DriverLog("[Gaze] Uploaded calibration blob ({} bytes).", static_cast(size)); + } } } } @@ -52,7 +66,11 @@ void CaesarUsbThreadGaze::OnConnected() { } int CaesarUsbThreadGaze::PollAndProcess() { - static hmd2_gaze_status_t state; + static constexpr size_t k_gazeStatusSize = sizeof(hmd2_gaze_status_t); + + // Deliberately not static: a partial transfer would otherwise leave the untouched tail holding the + // previous frame, which is exactly what makes a short read look like a valid sample. + hmd2_gaze_status_t state{}; int result = this->TransferPipe(GetEndpoint(), reinterpret_cast(&state), sizeof(state), 500); if (result == 0) { @@ -66,6 +84,18 @@ int CaesarUsbThreadGaze::PollAndProcess() { return -1; } + // TransferPipe returns the byte count, and a timed-out-but-partial transfer reports a positive value + // smaller than the struct. Publishing that would hand downstream a half-filled frame carrying a valid + // magic and a plausible timestamp, which nothing further down the pipeline could detect. + if (result < static_cast(k_gazeStatusSize)) { + static uint32_t s_shortTransferCount = 0; + ++s_shortTransferCount; + if (s_shortTransferCount == 1 || (s_shortTransferCount % 100) == 0) { + Util::DriverLog("[Gaze] Short transfer: {} of {} bytes, discarding frame (occurrence {}).", result, k_gazeStatusSize, s_shortTransferCount); + } + return 0; + } + if (state.magic[0] == GAZE_MAGIC_0 && state.magic[1] == GAZE_MAGIC_1_STATE) { HmdDeviceHooks::UpdateGaze(&state, sizeof(hmd2_gaze_status_t)); CustomShareManager *pShareManager = CustomShareManager::getSingleton(); diff --git a/projects/psvr2_openvr_driver_ex/utils/driver_settings.h b/projects/psvr2_openvr_driver_ex/utils/driver_settings.h index 39177dc..57b3af0 100644 --- a/projects/psvr2_openvr_driver_ex/utils/driver_settings.h +++ b/projects/psvr2_openvr_driver_ex/utils/driver_settings.h @@ -11,6 +11,16 @@ #define STEAMVR_SETTINGS_USE_ENHANCED_HAPTICS "useEnhancedHaptics" #define STEAMVR_SETTINGS_DISABLE_HIDDEN_AREA_MESH "disableHiddenAreaMesh" +// Eye tracking. See EYE_TRACKING_PLAN.md 2.4. +#define STEAMVR_SETTINGS_GAZE_DEFAULT_FIXATION_DISTANCE_M "gazeDefaultFixationDistanceM" +#define STEAMVR_SETTINGS_GAZE_FILTER_ENABLED "gazeFilterEnabled" +#define STEAMVR_SETTINGS_GAZE_FILTER_MIN_CUTOFF "gazeFilterMinCutoff" +#define STEAMVR_SETTINGS_GAZE_FILTER_BETA "gazeFilterBeta" +#define STEAMVR_SETTINGS_GAZE_FILTER_SACCADE_DEG_PER_SEC "gazeFilterSaccadeDegPerSec" +#define STEAMVR_SETTINGS_GAZE_BLINK_HOLD_MS "gazeBlinkHoldMs" +#define STEAMVR_SETTINGS_GAZE_FIT_GUIDANCE_ENABLED "gazeFitGuidanceEnabled" +#define STEAMVR_SETTINGS_GAZE_IMAGE_STREAM_ENABLED "gazeImageStreamEnabled" + #define SETTING_DISABLE_CHAPERONE_DEFAULT_VALUE false #define SETTING_DISABLE_SENSE_DEFAULT_VALUE false #define SETTING_DISABLE_GAZE_DEFAULT_VALUE false @@ -18,6 +28,19 @@ #define SETTING_USE_ENHANCED_HAPTICS_DEFAULT_VALUE true #define SETTING_DISABLE_HIDDEN_AREA_MESH_DEFAULT_VALUE false +// Fallback fixation distance when the headset reports no usable convergence distance. +#define SETTING_GAZE_DEFAULT_FIXATION_DISTANCE_M_DEFAULT_VALUE 2.0f +#define SETTING_GAZE_FILTER_ENABLED_DEFAULT_VALUE true +// One-euro filter tuning. Raising minCutoff reduces lag at the cost of jitter; beta is expressed in Hz +// of cutoff per deg/s of gaze velocity, so raising it opens the filter up sooner as the eye moves. +// The saccade threshold must stay above the tracker's noise floor -- see gaze_filter.h. +#define SETTING_GAZE_FILTER_MIN_CUTOFF_DEFAULT_VALUE 1.0f +#define SETTING_GAZE_FILTER_BETA_DEFAULT_VALUE 0.12f +#define SETTING_GAZE_FILTER_SACCADE_DEG_PER_SEC_DEFAULT_VALUE 120.0f +#define SETTING_GAZE_BLINK_HOLD_MS_DEFAULT_VALUE 200 +#define SETTING_GAZE_FIT_GUIDANCE_ENABLED_DEFAULT_VALUE true +#define SETTING_GAZE_IMAGE_STREAM_ENABLED_DEFAULT_VALUE true + namespace psvr2_toolkit { class DriverSettings { @@ -39,6 +62,20 @@ class DriverSettings { } return value; } + + static float GetFloat(const char *pchSettingsKey, float defaultValue) { + vr::EVRSettingsError error; + float value = vr::VRSettings()->GetFloat(STEAMVR_SETTINGS_SECTION_PLAYSTATION_VR2_EX, pchSettingsKey, &error); + if (error != vr::EVRSettingsError::VRSettingsError_None) { + value = defaultValue; + } + return value; + } + + // Single source of truth for whether the gaze pipeline is running. The hooks that feed it and the + // properties that advertise it are installed in different translation units, and they must agree: + // advertising eye tracking that never updates is worse for applications than reporting none. + static bool IsGazeEnabled() { return !GetBool(STEAMVR_SETTINGS_DISABLE_GAZE, SETTING_DISABLE_GAZE_DEFAULT_VALUE); } }; } // namespace psvr2_toolkit diff --git a/projects/psvr2_openvr_driver_ex/utils/hook_lib.h b/projects/psvr2_openvr_driver_ex/utils/hook_lib.h index 2a47e03..ba44558 100644 --- a/projects/psvr2_openvr_driver_ex/utils/hook_lib.h +++ b/projects/psvr2_openvr_driver_ex/utils/hook_lib.h @@ -2,6 +2,8 @@ #include "polyhook2/Detour/NatDetour.hpp" +#include "util.h" + #define INSTALL_STUB(pTarget) psvr2_toolkit::HookLib::InstallStub(pTarget) #define INSTALL_STUB_ORIGINAL(pTarget, ppOriginal) psvr2_toolkit::HookLib::InstallStub(pTarget, ppOriginal) @@ -17,15 +19,30 @@ class HookLib { static __int64 StubRet0() { return 0; } public: - static void InstallHook(void *pTarget, void *pDetour, void **ppOriginal = nullptr) { - uint64_t original = 0; // Only used if ppOriginal is null. - PLH::NatDetour *detour = new PLH::NatDetour((uint64_t)pTarget, (uint64_t)pDetour, ppOriginal ? (uint64_t *)ppOriginal : &original); - detour->hook(); + // Returns false if the detour could not be installed. A failed hook is otherwise indistinguishable + // from a hook whose target simply never runs, so the log line here is the only signal callers get. + static bool InstallHook(void *pTarget, void *pDetour, void **ppOriginal = nullptr) { + // The detour outlives this call and writes the trampoline through this pointer, so it must not + // be a local. Only used if ppOriginal is null. + static uint64_t s_discardedOriginal = 0; + + const uint64_t targetAddress = reinterpret_cast(pTarget); + PLH::NatDetour *detour = new PLH::NatDetour(targetAddress, (uint64_t)pDetour, ppOriginal ? (uint64_t *)ppOriginal : &s_discardedOriginal); + + if (!detour->hook()) { + Util::DriverLog("[HookLib] Failed to install hook at {:#x}", targetAddress); + // Deliberately not deleted. A failed hook may have already patched bytes, and letting the + // destructor decide what to unwind is riskier than leaking one small object during startup -- + // which is what this code did for every detour, successful or not, before logging was added. + return false; + } + + return true; } - static void InstallStub(void *pTarget, void **ppOriginal = nullptr) { InstallHook(pTarget, reinterpret_cast(Stub), ppOriginal); } + static bool InstallStub(void *pTarget, void **ppOriginal = nullptr) { return InstallHook(pTarget, reinterpret_cast(Stub), ppOriginal); } - static void InstallStubRet0(void *pTarget, void **ppOriginal = nullptr) { InstallHook(pTarget, reinterpret_cast(StubRet0), ppOriginal); } + static bool InstallStubRet0(void *pTarget, void **ppOriginal = nullptr) { return InstallHook(pTarget, reinterpret_cast(StubRet0), ppOriginal); } static bool SetInstructionNOPAtAddress(void *pTarget, size_t length) { DWORD oldProtect; diff --git a/projects/psvr2_pupillometry_test/CMakeLists.txt b/projects/psvr2_pupillometry_test/CMakeLists.txt new file mode 100644 index 0000000..7003db3 --- /dev/null +++ b/projects/psvr2_pupillometry_test/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(psvr2_pupillometry_test main.cpp) + +target_link_libraries(psvr2_pupillometry_test PRIVATE common) + +add_test(NAME pupillometry COMMAND psvr2_pupillometry_test) diff --git a/projects/psvr2_pupillometry_test/main.cpp b/projects/psvr2_pupillometry_test/main.cpp new file mode 100644 index 0000000..363e429 --- /dev/null +++ b/projects/psvr2_pupillometry_test/main.cpp @@ -0,0 +1,358 @@ +// Off-device tests for the pupillometry pipeline (common/pupillometry.h). +// +// Signal characteristics below come from a real 120 Hz capture (EYE_TRACKING_PLAN.md 5.3): ~3.0 mm +// mean diameter, 0.019 mm sample-to-sample noise, 0.27 mm blink artifacts, and a measurable +// gaze-angle dependence that a fitted camera axis removes almost entirely. The tests reproduce those +// conditions rather than inventing convenient ones. + +#include "pupillometry.h" + +#include +#include +#include +#include +#include + +namespace { + +int g_failures = 0; + +void Check(bool condition, const std::string &name) { + std::printf(" %s %s\n", condition ? "pass" : "FAIL", name.c_str()); + if (!condition) { + ++g_failures; + } +} + +void CheckNear(float actual, float expected, float tolerance, const std::string &name) { + const bool ok = std::fabs(actual - expected) <= tolerance; + if (ok) { + std::printf(" pass %s (%.4f)\n", name.c_str(), actual); + } else { + std::printf(" FAIL %s: got %.4f, expected %.4f +/- %.4f\n", name.c_str(), actual, expected, tolerance); + ++g_failures; + } +} + +constexpr int64_t k_frameUs = 8333; // 120 Hz, matching the hardware + +struct Noise { + uint32_t state = 0x9E3779B9u; + float Next(float amplitude) { + state = state * 1664525u + 1013904223u; + const float unit = static_cast((state >> 8) & 0xFFFF) / 65535.0f; + return (unit * 2.0f - 1.0f) * amplitude; + } +}; + +PupilEyeSample MakeEye(float diameterMm, float yawDeg = 0.0f, bool blinking = false, bool valid = true) { + PupilEyeSample e; + e.diameterMm = diameterMm; + e.diameterValid = valid; + e.blinking = blinking; + e.gazeValid = true; + // +Z forward, matching what the headset reports. + const float rad = yawDeg * (3.14159265358979323846f / 180.0f); + e.gazeDirX = std::sin(rad); + e.gazeDirY = 0.0f; + e.gazeDirZ = std::cos(rad); + return e; +} + +PupilFrame MakeFrame(int64_t t, float diameterMm, float yawDeg = 0.0f, bool blinking = false, bool valid = true) { + PupilFrame f; + f.timestampUs = t; + f.left = MakeEye(diameterMm, yawDeg, blinking, valid); + f.right = MakeEye(diameterMm, yawDeg, blinking, valid); + return f; +} + +} // namespace + +int main() { + std::printf("Pupillometry\n"); + + // --- basic plumbing -------------------------------------------------------------------------- + + { + PupillometryProcessor p; + PupillometryResult r; + // Nothing should come out until the blink guard band has been covered. + const bool early = p.Update(MakeFrame(1000, 3.0f), r); + Check(!early, "no result before the blink guard band elapses"); + } + + { + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + bool got = false; + for (int i = 0; i < 60; ++i) { + t += k_frameUs; + got = p.Update(MakeFrame(t, 3.0f), r) || got; + } + Check(got, "results start once the guard band is covered"); + Check(r.valid && r.contributingEyes == 2, "both eyes contribute when both are usable"); + CheckNear(r.correctedMm, 3.0f, 0.001f, "on-axis diameter passes through uncorrected"); + } + + // --- rejection ------------------------------------------------------------------------------- + + { + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + int valid = 0, total = 0; + for (int i = 0; i < 200; ++i) { + t += k_frameUs; + if (p.Update(MakeFrame(t, 3.0f, 0.0f, false, false), r)) { + ++total; + if (r.valid) { + ++valid; + } + } + } + Check(total > 0 && valid == 0, "samples flagged invalid never produce a measurement"); + } + + { + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + int emittedNearBlink = 0; + + // 0.27 mm artifact around a blink, matching what the real capture shows. + for (int i = 0; i < 300; ++i) { + t += k_frameUs; + // Artifact spans the guard band either side of the blink: 100 ms is 12 frames at 120 Hz. + const bool blinking = (i >= 150 && i < 162); + const bool nearBlink = (i >= 138 && i < 174); + const float d = nearBlink ? 3.27f : 3.0f; + if (p.Update(MakeFrame(t, d, 0.0f, blinking), r) && r.valid) { + if (std::fabs(r.correctedMm - 3.0f) > 0.05f) { + ++emittedNearBlink; + } + } + } + Check(emittedNearBlink == 0, "blink artifacts are rejected, not emitted"); + } + + { + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + int implausible = 0; + for (int i = 0; i < 100; ++i) { + t += k_frameUs; + if (p.Update(MakeFrame(t, 25.0f), r) && r.valid) { + ++implausible; + } + } + Check(implausible == 0, "implausible diameters are rejected"); + } + + // --- foreshortening correction ---------------------------------------------------------------- + + { + // A pupil viewed 30 degrees off the camera axis appears cos(30) smaller. Correction must undo it. + PupillometryConfig cfg; + PupillometryProcessor p; + p.Configure(cfg); + + PupillometryResult r; + int64_t t = 0; + const float trueDiameter = 3.0f; + const float apparent = trueDiameter * std::cos(30.0f * 3.14159265f / 180.0f); + + for (int i = 0; i < 60; ++i) { + t += k_frameUs; + p.Update(MakeFrame(t, apparent, 30.0f), r); + } + CheckNear(r.correctedMm, trueDiameter, 0.01f, "off-axis foreshortening is corrected"); + } + + { + // Beyond the cosine floor the correction would amplify noise; the sample must be dropped instead. + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + int emitted = 0; + for (int i = 0; i < 100; ++i) { + t += k_frameUs; + if (p.Update(MakeFrame(t, 3.0f, 75.0f), r) && r.valid) { + ++emitted; + } + } + Check(emitted == 0, "samples beyond the correction limit are dropped"); + } + + // --- binocular fusion -------------------------------------------------------------------------- + + { + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + for (int i = 0; i < 60; ++i) { + t += k_frameUs; + PupilFrame f = MakeFrame(t, 3.0f); + f.right.diameterValid = false; + p.Update(f, r); + } + Check(r.valid && r.contributingEyes == 1, "falls back to one eye when the other is unusable"); + } + + { + PupillometryProcessor p; + PupillometryResult r; + int64_t t = 0; + for (int i = 0; i < 60; ++i) { + t += k_frameUs; + PupilFrame f = MakeFrame(t, 0.0f); + f.left.diameterMm = 2.8f; + f.right.diameterMm = 3.2f; + p.Update(f, r); + } + CheckNear(r.correctedMm, 3.0f, 0.001f, "both eyes are averaged"); + } + + // --- baseline ------------------------------------------------------------------------------------ + + { + PupillometryConfig cfg; + cfg.baselineWindowSeconds = 4.0f; + PupillometryProcessor p; + p.Configure(cfg); + + PupillometryResult r; + int64_t t = 0; + Noise noise; + + // Two seconds is half the window, which is when the baseline becomes available. + for (int i = 0; i < 600; ++i) { + t += k_frameUs; + p.Update(MakeFrame(t, 3.0f + noise.Next(0.02f)), r); + } + Check(r.baselineReady, "baseline becomes available after half a window"); + CheckNear(r.baselineMm, 3.0f, 0.05f, "baseline tracks a steady diameter"); + CheckNear(r.deltaMm, 0.0f, 0.05f, "delta is near zero with no change"); + + // Now dilate by 0.4 mm and check the delta follows. + for (int i = 0; i < 200; ++i) { + t += k_frameUs; + p.Update(MakeFrame(t, 3.4f + noise.Next(0.02f)), r); + } + Check(r.deltaMm > 0.15f, "a real dilation shows up as a positive delta"); + Check(r.relative > 0.05f, "relative change is reported"); + } + + // --- activity index ------------------------------------------------------------------------------ + + { + // A slow luminance-style ramp must NOT look like activity: that is the whole point of the index. + PupillometryConfig cfg; + cfg.activityWindowSeconds = 3.0f; + PupillometryProcessor p; + p.Configure(cfg); + + PupillometryResult r; + int64_t t = 0; + Noise noise; + float rampActivity = 0.0f; + + for (int i = 0; i < 900; ++i) { + t += k_frameUs; + const float ramp = 3.0f + 1.5f * (static_cast(i) / 900.0f); // 1.5 mm over 7.5 s + if (p.Update(MakeFrame(t, ramp + noise.Next(0.02f)), r) && r.activityReady) { + rampActivity = r.activityIndex; + } + } + + // Abrupt steps, in contrast, must register. + PupillometryProcessor p2; + p2.Configure(cfg); + PupillometryResult r2; + int64_t t2 = 0; + Noise noise2; + float stepActivity = 0.0f; + + for (int i = 0; i < 900; ++i) { + t2 += k_frameUs; + const float step = 3.0f + ((i / 30) % 2 ? 0.25f : 0.0f); // a jump every 0.25 s + if (p2.Update(MakeFrame(t2, step + noise2.Next(0.02f)), r2) && r2.activityReady) { + stepActivity = r2.activityIndex; + } + } + + std::printf(" slow ramp activity = %.2f/s, abrupt steps = %.2f/s\n", rampActivity, stepActivity); + Check(stepActivity > rampActivity * 2.0f, "abrupt dilations register far above a slow luminance ramp"); + } + + // --- camera axis calibration ---------------------------------------------------------------------- + + { + // Synthesise an eye whose camera sits 20 degrees to the side, then check the fit recovers it. + PupilCameraAxisCalibrator cal; + Noise noise; + const float trueAz = -20.0f; + float ax, ay, az; + pupillometry_detail::AxisFromAngles(trueAz, 0.0f, ax, ay, az); + + // Sweep BOTH axes. Yaw-only data cannot constrain the camera elevation -- see GazeSpreadDegrees. + for (int i = 0; i < 800; ++i) { + const float yaw = -25.0f + 50.0f * (static_cast(i % 200) / 199.0f); + const float pitch = -15.0f + 30.0f * (static_cast((i * 7) % 200) / 199.0f); + + PupilEyeSample e; + e.diameterValid = true; + e.gazeValid = true; + const float yr = yaw * (3.14159265358979323846f / 180.0f); + const float pr = pitch * (3.14159265358979323846f / 180.0f); + e.gazeDirX = std::sin(yr) * std::cos(pr); + e.gazeDirY = std::sin(pr); + e.gazeDirZ = std::cos(yr) * std::cos(pr); + + const float cosine = pupillometry_detail::CosineToAxis(e, ax, ay, az); + e.diameterMm = 3.0f * cosine + noise.Next(0.01f); // what the camera would actually see + cal.Add(e); + } + + float fitAz = 0.0f, fitEl = 0.0f, residual = 1.0f, spread = 0.0f; + const bool solved = cal.Solve(fitAz, fitEl, residual, spread); + Check(solved, "calibrator solves with enough spread"); + if (solved) { + std::printf(" fitted az=%.0f (true %.0f), el=%.0f, residual=%.3f, spread=%.1f deg\n", fitAz, trueAz, fitEl, residual, spread); + CheckNear(fitAz, trueAz, 4.0f, "recovers the camera azimuth"); + Check(residual < 0.15f, "fit leaves little gaze-angle dependence"); + } + } + + { + // Looking in one direction the whole time cannot constrain the fit, and must be refused. + PupilCameraAxisCalibrator cal; + for (int i = 0; i < 800; ++i) { + cal.Add(MakeEye(3.0f, 0.0f)); + } + float a = 0, e = 0, res = 0, spread = 0; + Check(!cal.Solve(a, e, res, spread), "refuses to fit when the wearer never looked around"); + } + + { + PupilCameraAxisCalibrator cal; + for (int i = 0; i < 50; ++i) { + cal.Add(MakeEye(3.0f, static_cast(i % 30) - 15.0f)); + } + float a = 0, e = 0, res = 0, spread = 0; + Check(!cal.Solve(a, e, res, spread), "refuses to fit on too few samples"); + } + + { + PupilCameraAxisCalibrator cal; + cal.Add(MakeEye(3.0f, 0.0f, true)); // blinking + cal.Add(MakeEye(3.0f, 0.0f, false, false)); // invalid + cal.Add(MakeEye(25.0f, 0.0f)); // implausible + Check(cal.Count() == 0, "calibrator ignores blinking, invalid and implausible samples"); + } + + std::printf("\n%s (%d failure%s)\n", g_failures == 0 ? "PASSED" : "FAILED", g_failures, g_failures == 1 ? "" : "s"); + return g_failures == 0 ? 0 : 1; +} diff --git a/projects/psvr2_toolkit_capi/psvr2tk_capi.cpp b/projects/psvr2_toolkit_capi/psvr2tk_capi.cpp index de94180..2cd6b9f 100644 --- a/projects/psvr2_toolkit_capi/psvr2tk_capi.cpp +++ b/projects/psvr2_toolkit_capi/psvr2tk_capi.cpp @@ -2,11 +2,26 @@ #include "common.h" #include "psvr2tk_capi_private.h" #include "custom_share_manager.h" +#include "pupillometry.h" +#include "util.h" + +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#endif extern "C" { static int g_slot = -1; -static int g_lastGazeStatusCounter = -1; -static int g_lastGazeImageCounter = -1; + +// Per-thread, not global: these track "last frame this caller saw". Sharing them across threads makes +// two pollers steal each other's new-frame edges, so each would silently miss roughly half the stream. +static thread_local int g_lastGazeStatusCounter = -1; +static thread_local int g_lastGazeImageCounter = -1; int psvr2_toolkit_init() { CustomShareManager::createSingleton(); @@ -26,14 +41,35 @@ void psvr2_toolkit_deinit() { bool psvr2_toolkit_get_driver_active() { return CustomShareManager::getSingleton()->getDriverActive(); } +// Note: unlike the command-submitting entry points below, the streaming reads deliberately do not call +// getDriverActive(). That check costs two named-mutex operations, and on a path polled at the gaze +// frame rate it buys nothing the blocking timeout does not already report. bool psvr2_toolkit_gaze_status(hmd2_gaze_status_t *pGazeStatus, uint32_t timeoutMs) { + if (!pGazeStatus) { + return false; + } + return CustomShareManager::getSingleton()->getGazeStatus(pGazeStatus, &g_lastGazeStatusCounter, timeoutMs); } +// Deprecated: hands back a pointer into shared memory that the producer may recycle while the caller +// is still reading it. Prefer psvr2_toolkit_gaze_image_copy, which copies under the lock. bool psvr2_toolkit_gaze_image(unsigned char **pGazeImage, uint32_t timeoutMs) { + if (!pGazeImage) { + return false; + } + return CustomShareManager::getSingleton()->getGazeImageBuffer(pGazeImage, &g_lastGazeImageCounter, timeoutMs); } +bool psvr2_toolkit_gaze_image_copy(unsigned char *pDest, uint32_t destSize, uint32_t *pOutSize, uint32_t timeoutMs) { + if (!pDest || destSize == 0) { + return false; + } + + return CustomShareManager::getSingleton()->getGazeImageCopy(pDest, destSize, pOutSize, &g_lastGazeImageCounter, timeoutMs); +} + int psvr2_toolkit_write_pcm(VRControllerType controllerType, const unsigned char *pcm) { if (!CustomShareManager::getSingleton()->getDriverActive()) { return PSVR2TK_RESULT_DRIVER_INACTIVE; @@ -84,6 +120,205 @@ int psvr2_toolkit_set_trigger_effect(VRControllerType controllerType, const SceP } } +// --- Pupillometry ------------------------------------------------------------------------------ +// +// The processing runs HERE, in the client's own process, rather than in the driver. That keeps the +// driver, the shared-memory layout and the IPC protocol untouched, and it is also more correct: the +// baseline and activity windows are per-consumer state, and two applications watching the same headset +// legitimately want their own. +// +// One processor per thread, because the pipeline is a continuous filter over the sample stream and two +// callers sharing one would tear each other's history apart. Each thread also keeps its own "last frame +// seen" counter, independent of the one psvr2_toolkit_gaze_status uses, so an application can poll both +// without either stealing the other's frames. + +static std::mutex g_pupilAxesMutex; +static bool g_pupilAxesLoaded = false; +static float g_pupilAxes[4] = {0.0f, 0.0f, 0.0f, 0.0f}; // leftAz, leftEl, rightAz, rightEl + +// Camera axes live next to psvr2_toolkit_capi.dll, so a calibration performed in one application is +// picked up by every other one on the machine. +static bool GetPupilAxesPath(std::filesystem::path &outPath) { +#ifdef _WIN32 + HMODULE hModule = NULL; + if (!GetModuleHandleExA(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS | GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT, (LPCSTR)&GetPupilAxesPath, &hModule)) { + return false; + } + + char path[MAX_PATH] = {}; + if (GetModuleFileNameA(hModule, path, MAX_PATH) == 0) { + return false; + } + + outPath = std::filesystem::path(path).parent_path() / "pupil_camera_axes.txt"; + return true; +#else + (void)outPath; + return false; +#endif +} + +static void LoadPupilAxesOnce() { + std::lock_guard lock(g_pupilAxesMutex); + if (g_pupilAxesLoaded) { + return; + } + g_pupilAxesLoaded = true; + + std::filesystem::path path; + if (!GetPupilAxesPath(path)) { + return; + } + + std::ifstream in(path); + if (!in.is_open()) { + return; // uncalibrated; the straight-ahead default still runs, just less accurately + } + + float values[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + if (in >> values[0] >> values[1] >> values[2] >> values[3]) { + for (int i = 0; i < 4; ++i) { + g_pupilAxes[i] = values[i]; + } + } +} + +static PupillometryProcessor &ThreadPupillometry() { + static thread_local PupillometryProcessor processor; + static thread_local bool configured = false; + static thread_local float appliedAxes[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + + LoadPupilAxesOnce(); + + float axes[4]; + { + std::lock_guard lock(g_pupilAxesMutex); + for (int i = 0; i < 4; ++i) { + axes[i] = g_pupilAxes[i]; + } + } + + // Reconfigure only when the axes actually change, since that resets the filter history. + bool changed = !configured; + for (int i = 0; i < 4 && !changed; ++i) { + changed = appliedAxes[i] != axes[i]; + } + + if (changed) { + PupillometryConfig config; + config.leftCameraAzimuthDeg = axes[0]; + config.leftCameraElevationDeg = axes[1]; + config.rightCameraAzimuthDeg = axes[2]; + config.rightCameraElevationDeg = axes[3]; + processor.Configure(config); + processor.Reset(); + + for (int i = 0; i < 4; ++i) { + appliedAxes[i] = axes[i]; + } + configured = true; + } + + return processor; +} + +static PupilEyeSample ToPupilEyeSample(const hmd2_gaze_wearable_eye_t &eye) { + PupilEyeSample out; + out.diameterMm = eye.pupil_dia_mm; + out.diameterValid = eye.is_pupil_dia_valid == HMD2_GAZE_BOOL_TRUE; + out.blinking = eye.is_blink_valid == HMD2_GAZE_BOOL_TRUE && eye.blink == HMD2_GAZE_BOOL_TRUE; + // Headset-space direction, +Z forward, exactly as the device reports it. + out.gazeDirX = eye.gaze_dir_norm.x; + out.gazeDirY = eye.gaze_dir_norm.y; + out.gazeDirZ = eye.gaze_dir_norm.z; + out.gazeValid = eye.is_gaze_dir_valid == HMD2_GAZE_BOOL_TRUE; + return out; +} + +int psvr2_toolkit_pupillometry(Psvr2tkPupillometry *pOut, uint32_t timeoutMs) { + if (!pOut) { + return PSVR2TK_RESULT_INVALID_PARAMETER; + } + + static thread_local int lastCounter = -1; + + hmd2_gaze_status_t status = {}; + if (!CustomShareManager::getSingleton()->getGazeStatus(&status, &lastCounter, timeoutMs)) { + return PSVR2TK_RESULT_TIMEOUT; + } + + PupilFrame frame; + frame.timestampUs = status.wearable.timestamp; + frame.left = ToPupilEyeSample(status.wearable.left); + frame.right = ToPupilEyeSample(status.wearable.right); + + PupillometryResult result; + if (!ThreadPupillometry().Update(frame, result)) { + // Still inside the blink guard band; no verdict on this frame yet. + return PSVR2TK_RESULT_TIMEOUT; + } + + *pOut = {}; + pOut->timestamp = result.timestampUs; + pOut->isValid = result.valid ? 1u : 0u; + pOut->isBaselineReady = result.baselineReady ? 1u : 0u; + pOut->isActivityReady = result.activityReady ? 1u : 0u; + pOut->contributingEyes = static_cast(result.contributingEyes); + pOut->correctedMm = result.correctedMm; + pOut->smoothedMm = result.smoothedMm; + pOut->baselineMm = result.baselineMm; + pOut->deltaMm = result.deltaMm; + pOut->relative = result.relative; + pOut->activityIndex = result.activityIndex; + + return PSVR2TK_RESULT_OK; +} + +int psvr2_toolkit_pupillometry_set_camera_axes(float leftAzimuthDeg, float leftElevationDeg, float rightAzimuthDeg, float rightElevationDeg) { + // A camera more than 60 degrees off the eye's forward axis is not a plausible fit result. + const float candidates[4] = {leftAzimuthDeg, leftElevationDeg, rightAzimuthDeg, rightElevationDeg}; + for (float v : candidates) { + if (!(v > -60.0f && v < 60.0f)) { + return PSVR2TK_RESULT_INVALID_PARAMETER; + } + } + + { + std::lock_guard lock(g_pupilAxesMutex); + g_pupilAxesLoaded = true; // an explicit set supersedes whatever is on disk + for (int i = 0; i < 4; ++i) { + g_pupilAxes[i] = candidates[i]; + } + } + + // Persist so other applications pick the calibration up. Failing to write is not fatal: the axes are + // already live for this process. + std::filesystem::path path; + if (GetPupilAxesPath(path)) { + std::ofstream out(path, std::ios::trunc); + if (out.is_open()) { + out << candidates[0] << " " << candidates[1] << " " << candidates[2] << " " << candidates[3] << "\n"; + } + } + + return PSVR2TK_RESULT_OK; +} + +int psvr2_toolkit_pupillometry_get_camera_axes(float *pLeftAzimuthDeg, float *pLeftElevationDeg, float *pRightAzimuthDeg, float *pRightElevationDeg) { + if (!pLeftAzimuthDeg || !pLeftElevationDeg || !pRightAzimuthDeg || !pRightElevationDeg) { + return PSVR2TK_RESULT_INVALID_PARAMETER; + } + + LoadPupilAxesOnce(); + + std::lock_guard lock(g_pupilAxesMutex); + *pLeftAzimuthDeg = g_pupilAxes[0]; + *pLeftElevationDeg = g_pupilAxes[1]; + *pRightAzimuthDeg = g_pupilAxes[2]; + *pRightElevationDeg = g_pupilAxes[3]; + return PSVR2TK_RESULT_OK; +} + int psvr2_toolkit_set_hmd_rumble(uint8_t rumbleHz) { if (!CustomShareManager::getSingleton()->getDriverActive()) { return PSVR2TK_RESULT_DRIVER_INACTIVE; @@ -104,20 +339,43 @@ int psvr2_toolkit_set_hmd_rumble(uint8_t rumbleHz) { return success ? PSVR2TK_RESULT_OK : PSVR2TK_RESULT_TIMEOUT; } -GazeCalibrationCommand psvr2_toolkit_private_send_gaze_set_command(GazeCalibrationCommand command) { +static int SendGazeCalibrationCommand(DriverCommandType type, GazeCalibrationCommand *pCommand) { + if (!pCommand) { + return PSVR2TK_RESULT_INVALID_PARAMETER; + } + + if (!CustomShareManager::getSingleton()->getDriverActive()) { + return PSVR2TK_RESULT_DRIVER_INACTIVE; + } + DriverCommand drvCmd = {}; - drvCmd.type = DriverCommandType::GazeCalibrationSet; - drvCmd.gazeCalibration = command; - CustomShareManager::getSingleton()->submitCommand(drvCmd); - return drvCmd.gazeCalibration; + drvCmd.type = type; + drvCmd.gazeCalibration = *pCommand; + + const bool fulfilled = CustomShareManager::getSingleton()->submitCommand(drvCmd); + *pCommand = drvCmd.gazeCalibration; + + return fulfilled ? PSVR2TK_RESULT_OK : PSVR2TK_RESULT_TIMEOUT; +} + +// Deprecated: discards submitCommand's result, so a five-second timeout is indistinguishable from a +// successful calibration step. Prefer the _checked variants below. +GazeCalibrationCommand psvr2_toolkit_private_send_gaze_set_command(GazeCalibrationCommand command) { + SendGazeCalibrationCommand(DriverCommandType::GazeCalibrationSet, &command); + return command; } GazeCalibrationCommand psvr2_toolkit_private_send_gaze_get_command(GazeCalibrationCommand command) { - DriverCommand drvCmd = {}; - drvCmd.type = DriverCommandType::GazeCalibrationGet; - drvCmd.gazeCalibration = command; - CustomShareManager::getSingleton()->submitCommand(drvCmd); - return drvCmd.gazeCalibration; + SendGazeCalibrationCommand(DriverCommandType::GazeCalibrationGet, &command); + return command; +} + +int psvr2_toolkit_private_send_gaze_set_command_checked(GazeCalibrationCommand *pCommand) { + return SendGazeCalibrationCommand(DriverCommandType::GazeCalibrationSet, pCommand); +} + +int psvr2_toolkit_private_send_gaze_get_command_checked(GazeCalibrationCommand *pCommand) { + return SendGazeCalibrationCommand(DriverCommandType::GazeCalibrationGet, pCommand); } int psvr2_toolkit_private_set_usb_connection_state(bool connected) { diff --git a/projects/psvr2_toolkit_capi/psvr2tk_capi.h b/projects/psvr2_toolkit_capi/psvr2tk_capi.h index 6c1cccf..83d0474 100644 --- a/projects/psvr2_toolkit_capi/psvr2tk_capi.h +++ b/projects/psvr2_toolkit_capi/psvr2tk_capi.h @@ -14,10 +14,17 @@ X(bool, psvr2_toolkit_get_driver_active, (), ()) \ X(bool, psvr2_toolkit_gaze_status, (hmd2_gaze_status_t * pGazeStatus, uint32_t timeoutMs), (pGazeStatus, timeoutMs)) \ X(bool, psvr2_toolkit_gaze_image, (unsigned char **pGazeImage, uint32_t timeoutMs), (pGazeImage, timeoutMs)) \ + X(bool, psvr2_toolkit_gaze_image_copy, (unsigned char *pDest, uint32_t destSize, uint32_t *pOutSize, uint32_t timeoutMs), \ + (pDest, destSize, pOutSize, timeoutMs)) \ X(int, psvr2_toolkit_write_pcm, (VRControllerType controllerType, const unsigned char *pcm), (controllerType, pcm)) \ X(int, psvr2_toolkit_wait_for_pcm, (), ()) \ X(int, psvr2_toolkit_set_trigger_effect, (VRControllerType controllerType, const ScePadTriggerEffectCommand &command), (controllerType, command)) \ - X(int, psvr2_toolkit_set_hmd_rumble, (uint8_t rumbleHz), (rumbleHz)) + X(int, psvr2_toolkit_set_hmd_rumble, (uint8_t rumbleHz), (rumbleHz)) \ + X(int, psvr2_toolkit_pupillometry, (Psvr2tkPupillometry * pOut, uint32_t timeoutMs), (pOut, timeoutMs)) \ + X(int, psvr2_toolkit_pupillometry_set_camera_axes, (float leftAzimuthDeg, float leftElevationDeg, float rightAzimuthDeg, float rightElevationDeg), \ + (leftAzimuthDeg, leftElevationDeg, rightAzimuthDeg, rightElevationDeg)) \ + X(int, psvr2_toolkit_pupillometry_get_camera_axes, (float *pLeftAzimuthDeg, float *pLeftElevationDeg, float *pRightAzimuthDeg, float *pRightElevationDeg), \ + (pLeftAzimuthDeg, pLeftElevationDeg, pRightAzimuthDeg, pRightElevationDeg)) #ifdef __cplusplus extern "C" { diff --git a/projects/psvr2_toolkit_capi/psvr2tk_capi_private.h b/projects/psvr2_toolkit_capi/psvr2tk_capi_private.h index bf32eb5..20bb6bd 100644 --- a/projects/psvr2_toolkit_capi/psvr2tk_capi_private.h +++ b/projects/psvr2_toolkit_capi/psvr2tk_capi_private.h @@ -5,6 +5,8 @@ #define PSVR2TK_CAPI_PRIVATE_FUNCTIONS(X) \ X(GazeCalibrationCommand, psvr2_toolkit_private_send_gaze_set_command, (GazeCalibrationCommand command), (command)) \ X(GazeCalibrationCommand, psvr2_toolkit_private_send_gaze_get_command, (GazeCalibrationCommand command), (command)) \ + X(int, psvr2_toolkit_private_send_gaze_set_command_checked, (GazeCalibrationCommand * pCommand), (pCommand)) \ + X(int, psvr2_toolkit_private_send_gaze_get_command_checked, (GazeCalibrationCommand * pCommand), (pCommand)) \ X(int, psvr2_toolkit_private_set_usb_connection_state, (bool connected), (connected)) #ifdef __cplusplus diff --git a/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader.h b/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader.h index 7cb73b2..7a930bd 100644 --- a/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader.h +++ b/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader.h @@ -16,16 +16,33 @@ PSVR2TK_CAPI_FUNCTIONS(PSVR2TK_INLINE_HELPER) #undef PSVR2TK_INLINE_HELPER -inline void psvr2_toolkit_loader_init_functions(void *handle) { +// Resolves every entry point and returns how many could NOT be resolved. +// +// Check the result. Every function above is an inline shim that jumps through its p_ pointer, so an +// unresolved symbol is not a benign missing feature -- it is a null pointer that the first caller +// executes, crashing with no module name and no message. The usual cause is a version mismatch: an +// application built against a newer header talking to an older psvr2_toolkit_capi.dll left behind by a +// partial install. +inline int psvr2_toolkit_loader_init_functions(void *handle) { + int missing = 0; + #ifdef _WIN32 -#define PSVR2TK_LOAD_FUNC(ret, name, args_decl, args_pass) p_##name = reinterpret_cast(GetProcAddress(static_cast(handle), #name)); +#define PSVR2TK_LOAD_FUNC(ret, name, args_decl, args_pass) \ + p_##name = handle ? reinterpret_cast(GetProcAddress(static_cast(handle), #name)) : nullptr; \ + if (!p_##name) \ + ++missing; #else -#define PSVR2TK_LOAD_FUNC(ret, name, args_decl, args_pass) p_##name = reinterpret_cast(dlsym(handle, #name)); +#define PSVR2TK_LOAD_FUNC(ret, name, args_decl, args_pass) \ + p_##name = handle ? reinterpret_cast(dlsym(handle, #name)) : nullptr; \ + if (!p_##name) \ + ++missing; #endif PSVR2TK_CAPI_FUNCTIONS(PSVR2TK_LOAD_FUNC) #undef PSVR2TK_LOAD_FUNC + + return missing; } #ifdef __cplusplus diff --git a/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader_private.h b/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader_private.h index 46e9c6e..cae54f1 100644 --- a/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader_private.h +++ b/projects/psvr2_toolkit_capi_loader/psvr2tk_capi_loader_private.h @@ -16,15 +16,26 @@ PSVR2TK_CAPI_PRIVATE_FUNCTIONS(PSVR2TK_INLINE_PRIVATE_HELPER) #undef PSVR2TK_INLINE_PRIVATE_HELPER -inline void psvr2_toolkit_private_loader_init_functions(void *handle) { +// Returns how many private entry points could not be resolved. See the note on the public loader in +// psvr2tk_capi_loader.h -- an unresolved symbol here is a null pointer waiting to be called. +inline int psvr2_toolkit_private_loader_init_functions(void *handle) { + int missing = 0; + #ifdef _WIN32 #define PSVR2TK_LOAD_PRIVATE_FUNC(ret, name, args_decl, args_pass) \ - p_##name = reinterpret_cast(GetProcAddress(static_cast(handle), #name)); + p_##name = handle ? reinterpret_cast(GetProcAddress(static_cast(handle), #name)) : nullptr; \ + if (!p_##name) \ + ++missing; #else -#define PSVR2TK_LOAD_PRIVATE_FUNC(ret, name, args_decl, args_pass) p_##name = reinterpret_cast(dlsym(handle, #name)); +#define PSVR2TK_LOAD_PRIVATE_FUNC(ret, name, args_decl, args_pass) \ + p_##name = handle ? reinterpret_cast(dlsym(handle, #name)) : nullptr; \ + if (!p_##name) \ + ++missing; #endif PSVR2TK_CAPI_PRIVATE_FUNCTIONS(PSVR2TK_LOAD_PRIVATE_FUNC) #undef PSVR2TK_LOAD_PRIVATE_FUNC + + return missing; } diff --git a/projects/psvr2_toolkit_capi_test/gaze_diagnostics.h b/projects/psvr2_toolkit_capi_test/gaze_diagnostics.h new file mode 100644 index 0000000..6860886 --- /dev/null +++ b/projects/psvr2_toolkit_capi_test/gaze_diagnostics.h @@ -0,0 +1,405 @@ +#pragma once + +// Read-only derived measurements over the gaze stream, for evaluating the quality of the tracker and the user's fit. This is not a driver-facing API +// Nothing here feeds back into the driver. The point is to answer, with a headset on, some questions that I wanted to test it out: +// +// what pos_guide magnitude actually corresponds to a badly fitted headset? +// what IPD do the gaze origins measure, and does it agree with what SteamVR already reports? +// what are this user's blink rate and duration? + +#include "hmd2_gaze.h" +#include "pupillometry.h" + +#include +#include +#include + +namespace gaze_capture { + +// Rolling mean/min/max over a bounded window. +class RollingStat { +public: + explicit RollingStat(size_t capacity = 600) : m_capacity(capacity) {} + + void Add(float value) { + if (!std::isfinite(value)) { + return; + } + m_samples.push_back(value); + m_sum += value; + while (m_samples.size() > m_capacity) { + m_sum -= m_samples.front(); + m_samples.pop_front(); + } + } + + bool Empty() const { return m_samples.empty(); } + size_t Count() const { return m_samples.size(); } + float Mean() const { return m_samples.empty() ? 0.0f : static_cast(m_sum / static_cast(m_samples.size())); } + + float Min() const { + float v = 0.0f; + bool first = true; + for (float s : m_samples) { + if (first || s < v) { + v = s; + first = false; + } + } + return v; + } + + float Max() const { + float v = 0.0f; + bool first = true; + for (float s : m_samples) { + if (first || s > v) { + v = s; + first = false; + } + } + return v; + } + +private: + size_t m_capacity; + std::deque m_samples; + double m_sum = 0.0; +}; + +struct BlinkStats { + uint32_t blinks = 0; + float lastDurationMs = 0.0f; + float meanDurationMs = 0.0f; + float blinksPerMinute = 0.0f; + // Fraction of the session with at least one eye closed. The basis of PERCLOS-style drowsiness work. + float closedFraction = 0.0f; +}; + +// Compact, copyable view of everything the UI displays. +// +// The UI used to copy the whole GazeDiagnostics under the lock each frame, which was already dragging +// several rolling buffers along and would now pull the calibrator's sample store too. Snapshotting the +// scalars keeps the locked section short and the per-frame cost flat. +struct GazeDiagnosticsSnapshot { + uint64_t samples = 0; + + bool hasIpd = false; + float ipdMeanMm = 0, ipdMinMm = 0, ipdMaxMm = 0; + size_t ipdCount = 0; + + bool hasFit = false; + float fitLeftMean = 0, fitRightMean = 0, fitLeftMax = 0, fitRightMax = 0, fitWorstNow = 0; + + BlinkStats blinks; + + bool hasConvergence = false; + float convMeanMm = 0, convMinMm = 0, convMaxMm = 0; + + PupillometryResult pupil; + PupillometryConfig pupilConfig; + uint64_t pupilAccepted = 0, pupilRejected = 0; + + bool calibrating = false, calSolved = false; + size_t calSamples = 0; + float calLeftAz = 0, calLeftEl = 0, calRightAz = 0, calRightEl = 0; + float calResidual = 0, calSpread = 0; +}; + +class GazeDiagnostics { +public: + // Called once per new gaze frame, from the recorder thread. + void Update(const hmd2_gaze_status_t &s, int64_t hostRecvUs) { + if (m_firstSampleUs == 0) { + m_firstSampleUs = hostRecvUs; + } + m_lastSampleUs = hostRecvUs; + ++m_sampleCount; + + UpdateIpd(s); + UpdateFit(s); + UpdateBlink(s, hostRecvUs); + UpdateConvergence(s); + UpdatePupillometry(s); + } + + // --- 4.3: measured IPD ----------------------------------------------------------------------- + // The gaze origins are eyeball rotation centres, so their separation is the user's interpupillary + // distance, measured rather than dialled in. + bool HasIpd() const { return !m_ipdMm.Empty(); } + float IpdMeanMm() const { return m_ipdMm.Mean(); } + float IpdMinMm() const { return m_ipdMm.Min(); } + float IpdMaxMm() const { return m_ipdMm.Max(); } + size_t IpdSampleCount() const { return m_ipdMm.Count(); } + + // --- 4.2: headset fit ------------------------------------------------------------------------ + bool HasFit() const { return !m_fitLeft.Empty() || !m_fitRight.Empty(); } + float FitLeftMean() const { return m_fitLeft.Mean(); } + float FitRightMean() const { return m_fitRight.Mean(); } + float FitLeftMax() const { return m_fitLeft.Max(); } + float FitRightMax() const { return m_fitRight.Max(); } + float FitWorstNow() const { return m_fitWorstNow; } + + // --- 5.2: blink dynamics --------------------------------------------------------------------- + const BlinkStats &Blinks() const { return m_blinkStats; } + + // --- 4.4: convergence ------------------------------------------------------------------------ + bool HasConvergence() const { return !m_convergenceMm.Empty(); } + float ConvergenceMeanMm() const { return m_convergenceMm.Mean(); } + float ConvergenceMinMm() const { return m_convergenceMm.Min(); } + float ConvergenceMaxMm() const { return m_convergenceMm.Max(); } + + uint64_t SampleCount() const { return m_sampleCount; } + + // --- 5.3: pupillometry ------------------------------------------------------------------------- + const PupillometryResult &Pupil() const { return m_pupilResult; } + uint64_t PupilAccepted() const { return m_pupilAccepted; } + uint64_t PupilRejected() const { return m_pupilRejected; } + + void SetPupilConfig(const PupillometryConfig &config) { + m_pupilConfig = config; + m_pupil.Configure(config); + m_pupil.Reset(); + } + const PupillometryConfig &PupilConfig() const { return m_pupilConfig; } + + // Camera-axis calibration. Collect while the wearer looks around, then solve. + void StartCalibration() { + m_calLeft.Reset(); + m_calRight.Reset(); + m_calibrating = true; + m_calSolved = false; + } + void StopCalibration() { m_calibrating = false; } + bool IsCalibrating() const { return m_calibrating; } + size_t CalibrationSamples() const { return m_calLeft.Count(); } + bool CalibrationSolved() const { return m_calSolved; } + float CalLeftAz() const { return m_calLeftAz; } + float CalLeftEl() const { return m_calLeftEl; } + float CalRightAz() const { return m_calRightAz; } + float CalRightEl() const { return m_calRightEl; } + float CalResidual() const { return m_calResidual; } + float CalSpreadDeg() const { return m_calSpread; } + + // Returns false when there is not enough 2D gaze spread to identify the axes. + bool SolveCalibration() { + float lr = 0.0f, ls = 0.0f, rr = 0.0f, rs = 0.0f; + const bool okL = m_calLeft.Solve(m_calLeftAz, m_calLeftEl, lr, ls); + const bool okR = m_calRight.Solve(m_calRightAz, m_calRightEl, rr, rs); + + if (!okL || !okR) { + m_calSolved = false; + m_calSpread = ls < rs ? ls : rs; + return false; + } + + m_calResidual = (lr + rr) * 0.5f; + m_calSpread = ls < rs ? ls : rs; + m_calSolved = true; + m_calibrating = false; + + m_pupilConfig.leftCameraAzimuthDeg = m_calLeftAz; + m_pupilConfig.leftCameraElevationDeg = m_calLeftEl; + m_pupilConfig.rightCameraAzimuthDeg = m_calRightAz; + m_pupilConfig.rightCameraElevationDeg = m_calRightEl; + m_pupil.Configure(m_pupilConfig); + m_pupil.Reset(); + return true; + } + + GazeDiagnosticsSnapshot Snapshot() const { + GazeDiagnosticsSnapshot s; + s.samples = m_sampleCount; + + s.hasIpd = HasIpd(); + s.ipdMeanMm = IpdMeanMm(); + s.ipdMinMm = IpdMinMm(); + s.ipdMaxMm = IpdMaxMm(); + s.ipdCount = IpdSampleCount(); + + s.hasFit = HasFit(); + s.fitLeftMean = FitLeftMean(); + s.fitRightMean = FitRightMean(); + s.fitLeftMax = FitLeftMax(); + s.fitRightMax = FitRightMax(); + s.fitWorstNow = FitWorstNow(); + + s.blinks = m_blinkStats; + + s.hasConvergence = HasConvergence(); + s.convMeanMm = ConvergenceMeanMm(); + s.convMinMm = ConvergenceMinMm(); + s.convMaxMm = ConvergenceMaxMm(); + + s.pupil = m_pupilResult; + s.pupilConfig = m_pupilConfig; + s.pupilAccepted = m_pupilAccepted; + s.pupilRejected = m_pupilRejected; + + s.calibrating = m_calibrating; + s.calSolved = m_calSolved; + s.calSamples = m_calLeft.Count(); + s.calLeftAz = m_calLeftAz; + s.calLeftEl = m_calLeftEl; + s.calRightAz = m_calRightAz; + s.calRightEl = m_calRightEl; + s.calResidual = m_calResidual; + s.calSpread = m_calSpread; + return s; + } + + void Reset() { *this = GazeDiagnostics(); } + +private: + void UpdateIpd(const hmd2_gaze_status_t &s) { + if (s.wearable.left.is_gaze_origin_valid != HMD2_GAZE_BOOL_TRUE || s.wearable.right.is_gaze_origin_valid != HMD2_GAZE_BOOL_TRUE) { + return; + } + + const float dx = s.wearable.left.gaze_origin_mm.x - s.wearable.right.gaze_origin_mm.x; + const float dy = s.wearable.left.gaze_origin_mm.y - s.wearable.right.gaze_origin_mm.y; + const float dz = s.wearable.left.gaze_origin_mm.z - s.wearable.right.gaze_origin_mm.z; + const float separationMm = std::sqrt(dx * dx + dy * dy + dz * dz); + + // Human IPD spans roughly 51-77 mm. Anything outside a generous window is a bad sample, not a face. + if (separationMm < 40.0f || separationMm > 90.0f) { + return; + } + + m_ipdMm.Add(separationMm); + } + + void UpdateFit(const hmd2_gaze_status_t &s) { + float worst = 0.0f; + + if (s.wearable.left.is_pos_guide_valid == HMD2_GAZE_BOOL_TRUE) { + const float m = std::sqrt(s.wearable.left.pos_guide.x * s.wearable.left.pos_guide.x + s.wearable.left.pos_guide.y * s.wearable.left.pos_guide.y); + m_fitLeft.Add(m); + worst = std::fmax(worst, m); + } + + if (s.wearable.right.is_pos_guide_valid == HMD2_GAZE_BOOL_TRUE) { + const float m = std::sqrt(s.wearable.right.pos_guide.x * s.wearable.right.pos_guide.x + s.wearable.right.pos_guide.y * s.wearable.right.pos_guide.y); + m_fitRight.Add(m); + worst = std::fmax(worst, m); + } + + m_fitWorstNow = worst; + } + + void UpdateBlink(const hmd2_gaze_status_t &s, int64_t hostRecvUs) { + const bool leftClosed = s.wearable.left.is_blink_valid == HMD2_GAZE_BOOL_TRUE && s.wearable.left.blink == HMD2_GAZE_BOOL_TRUE; + const bool rightClosed = s.wearable.right.is_blink_valid == HMD2_GAZE_BOOL_TRUE && s.wearable.right.blink == HMD2_GAZE_BOOL_TRUE; + const bool closed = leftClosed || rightClosed; + + if (closed) { + m_closedSamples++; + if (!m_wasClosed) { + m_blinkStartUs = hostRecvUs; + } + } else if (m_wasClosed) { + const float durationMs = static_cast(hostRecvUs - m_blinkStartUs) / 1000.0f; + + // Ignore implausibly long closures: those are the eyes simply being shut, not a blink. + if (durationMs > 0.0f && durationMs < 1000.0f) { + m_blinkStats.blinks++; + m_blinkStats.lastDurationMs = durationMs; + m_blinkDurationSumMs += durationMs; + m_blinkStats.meanDurationMs = static_cast(m_blinkDurationSumMs / m_blinkStats.blinks); + } + } + + m_wasClosed = closed; + + const double elapsedMinutes = static_cast(m_lastSampleUs - m_firstSampleUs) / 60e6; + if (elapsedMinutes > 0.01) { + m_blinkStats.blinksPerMinute = static_cast(m_blinkStats.blinks / elapsedMinutes); + } + if (m_sampleCount > 0) { + m_blinkStats.closedFraction = static_cast(static_cast(m_closedSamples) / static_cast(m_sampleCount)); + } + } + + static PupilEyeSample ToPupilSample(const hmd2_gaze_wearable_eye_t &eye) { + PupilEyeSample out; + out.diameterMm = eye.pupil_dia_mm; + out.diameterValid = eye.is_pupil_dia_valid == HMD2_GAZE_BOOL_TRUE; + out.blinking = eye.is_blink_valid == HMD2_GAZE_BOOL_TRUE && eye.blink == HMD2_GAZE_BOOL_TRUE; + // Headset-space direction, +Z forward -- exactly what pupillometry.h expects. + out.gazeDirX = eye.gaze_dir_norm.x; + out.gazeDirY = eye.gaze_dir_norm.y; + out.gazeDirZ = eye.gaze_dir_norm.z; + out.gazeValid = eye.is_gaze_dir_valid == HMD2_GAZE_BOOL_TRUE; + return out; + } + + void UpdatePupillometry(const hmd2_gaze_status_t &s) { + PupilFrame frame; + // The headset clock, not the host one: it is the timebase the samples were measured against. + frame.timestampUs = s.wearable.timestamp; + frame.left = ToPupilSample(s.wearable.left); + frame.right = ToPupilSample(s.wearable.right); + + if (m_calibrating) { + m_calLeft.Add(frame.left, m_pupilConfig.minPlausibleMm, m_pupilConfig.maxPlausibleMm); + m_calRight.Add(frame.right, m_pupilConfig.minPlausibleMm, m_pupilConfig.maxPlausibleMm); + } + + PupillometryResult result; + if (!m_pupil.Update(frame, result)) { + return; + } + + if (result.valid) { + m_pupilResult = result; + ++m_pupilAccepted; + } else { + // Mostly blink guard bands. A high rejection rate means either a lot of blinking or a fit problem. + ++m_pupilRejected; + } + } + + void UpdateConvergence(const hmd2_gaze_status_t &s) { + if (s.wearable.is_convergence_distance_valid != HMD2_GAZE_BOOL_TRUE) { + return; + } + const float mm = s.foveated.convergence_distance_mm; + if (!std::isfinite(mm) || mm <= 0.0f) { + return; + } + m_convergenceMm.Add(mm); + } + + RollingStat m_ipdMm; + RollingStat m_fitLeft; + RollingStat m_fitRight; + RollingStat m_convergenceMm; + float m_fitWorstNow = 0.0f; + + BlinkStats m_blinkStats; + bool m_wasClosed = false; + int64_t m_blinkStartUs = 0; + double m_blinkDurationSumMs = 0.0; + uint64_t m_closedSamples = 0; + + int64_t m_firstSampleUs = 0; + int64_t m_lastSampleUs = 0; + uint64_t m_sampleCount = 0; + + PupillometryConfig m_pupilConfig; + PupillometryProcessor m_pupil; + PupillometryResult m_pupilResult; + uint64_t m_pupilAccepted = 0; + uint64_t m_pupilRejected = 0; + + PupilCameraAxisCalibrator m_calLeft; + PupilCameraAxisCalibrator m_calRight; + bool m_calibrating = false; + bool m_calSolved = false; + float m_calLeftAz = 0.0f, m_calLeftEl = 0.0f; + float m_calRightAz = 0.0f, m_calRightEl = 0.0f; + float m_calResidual = 0.0f; + float m_calSpread = 0.0f; +}; + +} // namespace gaze_capture diff --git a/projects/psvr2_toolkit_capi_test/gaze_recorder.h b/projects/psvr2_toolkit_capi_test/gaze_recorder.h new file mode 100644 index 0000000..fb76690 --- /dev/null +++ b/projects/psvr2_toolkit_capi_test/gaze_recorder.h @@ -0,0 +1,322 @@ +#pragma once + +// Session recorder for the gaze stream (EYE_TRACKING_PLAN.md 0.3). +// +// Phases 2, 4 and 5 all change the numbers the tracker produces, so there has to be a way to compare +// a change against the same input. This dumps every field of hmd2_gaze_status_t to CSV, plus a host +// receive timestamp, and optionally the raw IR eye camera frames. +// +// Polling runs on its own thread rather than off the render loop: the gaze stream can outrun vsync, +// and a dropped sample would show up as a frame_counter gap in the capture. + +#include "hmd2_gaze.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gaze_capture { + +inline int64_t NowMicros() { + using namespace std::chrono; + static const steady_clock::time_point start = steady_clock::now(); + return duration_cast(steady_clock::now() - start).count(); +} + +// CSV emission. the header and the row are produced by walking the same description, so the two cannot drift apart as fields are added. Field names are only materialised by the header emitter. + +struct CsvHeaderEmitter { + std::string out; + + void Add(const char *prefix, const char *name) { + if (!out.empty()) { + out += ','; + } + out += prefix; + out += name; + } + + void operator()(const char *prefix, const char *name, double) { Add(prefix, name); } + void operator()(const char *prefix, const char *name, int64_t) { Add(prefix, name); } +}; + +struct CsvRowEmitter { + std::string out; + + void Add(const char *text) { + if (!out.empty()) { + out += ','; + } + out += text; + } + + void operator()(const char *, const char *, double value) { + char buffer[40]; + std::snprintf(buffer, sizeof(buffer), "%.9g", value); + Add(buffer); + } + + void operator()(const char *, const char *, int64_t value) { + char buffer[32]; + std::snprintf(buffer, sizeof(buffer), "%lld", static_cast(value)); + Add(buffer); + } +}; + +template void DescribeEye(const char *prefix, const hmd2_gaze_wearable_eye_t &eye, Emit &emit) { + emit(prefix, "gaze_origin_valid", static_cast(eye.is_gaze_origin_valid)); + emit(prefix, "gaze_origin_x", static_cast(eye.gaze_origin_mm.x)); + emit(prefix, "gaze_origin_y", static_cast(eye.gaze_origin_mm.y)); + emit(prefix, "gaze_origin_z", static_cast(eye.gaze_origin_mm.z)); + emit(prefix, "gaze_dir_valid", static_cast(eye.is_gaze_dir_valid)); + emit(prefix, "gaze_dir_x", static_cast(eye.gaze_dir_norm.x)); + emit(prefix, "gaze_dir_y", static_cast(eye.gaze_dir_norm.y)); + emit(prefix, "gaze_dir_z", static_cast(eye.gaze_dir_norm.z)); + emit(prefix, "pupil_dia_valid", static_cast(eye.is_pupil_dia_valid)); + emit(prefix, "pupil_dia_mm", static_cast(eye.pupil_dia_mm)); + emit(prefix, "pupil_pos_valid", static_cast(eye.is_pupil_pos_in_sensor_area_valid)); + emit(prefix, "pupil_pos_x", static_cast(eye.pupil_pos_in_sensor_area.x)); + emit(prefix, "pupil_pos_y", static_cast(eye.pupil_pos_in_sensor_area.y)); + emit(prefix, "pos_guide_valid", static_cast(eye.is_pos_guide_valid)); + emit(prefix, "pos_guide_x", static_cast(eye.pos_guide.x)); + emit(prefix, "pos_guide_y", static_cast(eye.pos_guide.y)); + emit(prefix, "blink_valid", static_cast(eye.is_blink_valid)); + emit(prefix, "blink", static_cast(eye.blink)); +} + +template void DescribeGazeStatus(const hmd2_gaze_status_t &s, int64_t hostRecvUs, Emit &emit) { + emit("", "host_recv_us", static_cast(hostRecvUs)); + + emit("", "magic0", static_cast(s.magic[0])); + emit("", "magic1", static_cast(s.magic[1])); + emit("", "version", static_cast(s.version)); + emit("", "size", static_cast(s.size)); + + emit("", "exp_l", static_cast(s.exp_l)); + emit("", "exp_r", static_cast(s.exp_r)); + emit("", "led_status", static_cast(s.led_status)); + emit("", "exp_counter_l", static_cast(s.exp_counter_l)); + emit("", "exp_counter_r", static_cast(s.exp_counter_r)); + emit("", "led_counter", static_cast(s.led_counter)); + emit("", "dsp_return_code", static_cast(s.dsp_return_code)); + + emit("", "lens_left_x", static_cast(s.lens_config.left.x)); + emit("", "lens_left_y", static_cast(s.lens_config.left.y)); + emit("", "lens_left_z", static_cast(s.lens_config.left.z)); + emit("", "lens_right_x", static_cast(s.lens_config.right.x)); + emit("", "lens_right_y", static_cast(s.lens_config.right.y)); + emit("", "lens_right_z", static_cast(s.lens_config.right.z)); + + emit("", "user_calibration_id", static_cast(s.user_calibration_id)); + emit("", "fr_gaze_origin_x", static_cast(s.fr_gaze_origin.x)); + emit("", "fr_gaze_origin_y", static_cast(s.fr_gaze_origin.y)); + emit("", "fr_gaze_origin_z", static_cast(s.fr_gaze_origin.z)); + emit("", "enabled_eye", static_cast(s.enabled_eye)); + emit("", "motor_sequence", static_cast(s.motor_sequence)); + emit("", "motor_strength", static_cast(s.motor_strength)); + + emit("", "wearable_timestamp", static_cast(s.wearable.timestamp)); + emit("", "wearable_frame_counter", static_cast(s.wearable.frame_counter)); + + DescribeEye("left_", s.wearable.left, emit); + DescribeEye("right_", s.wearable.right, emit); + + emit("", "combined_origin_valid", static_cast(s.wearable.is_gaze_origin_combined_valid)); + emit("", "combined_origin_x", static_cast(s.wearable.gaze_origin_combined_mm.x)); + emit("", "combined_origin_y", static_cast(s.wearable.gaze_origin_combined_mm.y)); + emit("", "combined_origin_z", static_cast(s.wearable.gaze_origin_combined_mm.z)); + emit("", "combined_dir_valid", static_cast(s.wearable.is_gaze_dir_combined_valid)); + emit("", "combined_dir_x", static_cast(s.wearable.gaze_dir_combined_norm.x)); + emit("", "combined_dir_y", static_cast(s.wearable.gaze_dir_combined_norm.y)); + emit("", "combined_dir_z", static_cast(s.wearable.gaze_dir_combined_norm.z)); + emit("", "convergence_valid", static_cast(s.wearable.is_convergence_distance_valid)); + + emit("", "foveated_timestamp", static_cast(s.foveated.timestamp)); + emit("", "foveated_frame_counter", static_cast(s.foveated.frame_counter)); + emit("", "foveated_tracking_state", static_cast(s.foveated.tracking_state)); + emit("", "foveated_dir_left_x", static_cast(s.foveated.gaze_dir_left_norm.x)); + emit("", "foveated_dir_left_y", static_cast(s.foveated.gaze_dir_left_norm.y)); + emit("", "foveated_dir_left_z", static_cast(s.foveated.gaze_dir_left_norm.z)); + emit("", "foveated_dir_right_x", static_cast(s.foveated.gaze_dir_right_norm.x)); + emit("", "foveated_dir_right_y", static_cast(s.foveated.gaze_dir_right_norm.y)); + emit("", "foveated_dir_right_z", static_cast(s.foveated.gaze_dir_right_norm.z)); + emit("", "foveated_dir_combined_x", static_cast(s.foveated.gaze_dir_combined_norm.x)); + emit("", "foveated_dir_combined_y", static_cast(s.foveated.gaze_dir_combined_norm.y)); + emit("", "foveated_dir_combined_z", static_cast(s.foveated.gaze_dir_combined_norm.z)); + emit("", "foveated_convergence_distance_mm", static_cast(s.foveated.convergence_distance_mm)); +} + +// Capture destination + +inline std::string MakeSessionDirectory() { + const std::time_t now = std::time(nullptr); + std::tm tm = {}; +#ifdef _WIN32 + localtime_s(&tm, &now); +#else + localtime_r(&now, &tm); +#endif + + char stamp[32]; + std::strftime(stamp, sizeof(stamp), "%Y%m%d_%H%M%S", &tm); + + std::filesystem::path dir = std::filesystem::path("psvr2tk_captures") / stamp; + std::error_code ec; + std::filesystem::create_directories(dir, ec); + return dir.string(); +} + +// Status recorder + +class GazeRecorder { +public: + bool IsActive() const { return m_active.load(); } + uint64_t Rows() const { return m_rows.load(); } + uint64_t Gaps() const { return m_gaps.load(); } + + std::string Path() { + std::scoped_lock lock(m_mutex); + return m_path; + } + + bool Start() { + std::scoped_lock lock(m_mutex); + if (m_active.load()) { + return true; + } + + const std::string dir = MakeSessionDirectory(); + m_path = (std::filesystem::path(dir) / "gaze_status.csv").string(); + + m_csv.open(m_path, std::ios::out | std::ios::trunc); + if (!m_csv.is_open()) { + m_path.clear(); + return false; + } + + hmd2_gaze_status_t blank = {}; + CsvHeaderEmitter header; + DescribeGazeStatus(blank, 0, header); + m_csv << header.out << "\n"; + + m_rows.store(0); + m_gaps.store(0); + m_haveLastFrame = false; + m_active.store(true); + return true; + } + + void Stop() { + std::scoped_lock lock(m_mutex); + if (!m_active.load()) { + return; + } + m_active.store(false); + m_csv.flush(); + m_csv.close(); + } + + void Write(const hmd2_gaze_status_t &status, int64_t hostRecvUs) { + std::scoped_lock lock(m_mutex); + if (!m_active.load() || !m_csv.is_open()) { + return; + } + + // A capture with gaps is not usable as a reference, so surface them rather than silently recording a stream that skipped frames. + if (m_haveLastFrame) { + const uint32_t expected = m_lastFrame + 1; + if (status.wearable.frame_counter != expected) { + m_gaps.fetch_add(1); + } + } + m_lastFrame = status.wearable.frame_counter; + m_haveLastFrame = true; + + CsvRowEmitter row; + DescribeGazeStatus(status, hostRecvUs, row); + m_csv << row.out << "\n"; + + m_rows.fetch_add(1); + } + +private: + std::mutex m_mutex; + std::ofstream m_csv; + std::string m_path; + std::atomic m_active{false}; + std::atomic m_rows{0}; + std::atomic m_gaps{0}; + bool m_haveLastFrame = false; + uint32_t m_lastFrame = 0; +}; + +// IR eye image dumper +class GazeImageDumper { +public: + // Full slot size: 0x100 header + 0x200000 payload. + static constexpr size_t k_frameSize = 0x200100; + + bool IsActive() const { return m_active; } + int Written() const { return m_written; } + int &Stride() { return m_stride; } + int &MaxFrames() { return m_maxFrames; } + const std::string &Directory() const { return m_dir; } + + bool Start() { + if (m_active) { + return true; + } + m_dir = MakeSessionDirectory(); + m_seen = 0; + m_written = 0; + m_active = true; + return true; + } + + void Stop() { m_active = false; } + + // Called from the render loop with a frame already copied out of shared memory by psvr2_toolkit_gaze_image_copy, so what lands on disk cannot tear. + void MaybeDump(const unsigned char *image, uint32_t size) { + if (!m_active || image == nullptr || size == 0) { + return; + } + + if (m_written >= m_maxFrames) { + m_active = false; + return; + } + + if (m_stride > 1 && (m_seen++ % m_stride) != 0) { + return; + } + + char name[64]; + std::snprintf(name, sizeof(name), "eye_%05d.bin", m_written); + + std::ofstream out(std::filesystem::path(m_dir) / name, std::ios::binary); + if (!out.is_open()) { + m_active = false; + return; + } + + out.write(reinterpret_cast(image), static_cast(size)); + ++m_written; + } + +private: + bool m_active = false; + int m_stride = 10; + int m_maxFrames = 200; + int m_seen = 0; + int m_written = 0; + std::string m_dir; +}; + +} // namespace gaze_capture diff --git a/projects/psvr2_toolkit_capi_test/main.cpp b/projects/psvr2_toolkit_capi_test/main.cpp index 279cec6..58d5899 100644 --- a/projects/psvr2_toolkit_capi_test/main.cpp +++ b/projects/psvr2_toolkit_capi_test/main.cpp @@ -16,6 +16,8 @@ #include "pad_trigger_effect.h" #include "psvr2tk_capi_loader.h" #include "psvr2tk_capi_loader_private.h" +#include "gaze_diagnostics.h" +#include "gaze_recorder.h" std::atomic g_appRunning = true; std::mutex g_hapticsMutex; @@ -24,6 +26,16 @@ bool g_playToneRight = false; float g_toneFrequency = 200.0f; float g_toneAmplitude = 0.5f; +std::mutex g_latestGazeMutex; +hmd2_gaze_status_t g_latestGaze = {}; + +gaze_capture::GazeRecorder g_gazeRecorder; +gaze_capture::GazeImageDumper g_gazeImageDumper; + +std::mutex g_diagnosticsMutex; +gaze_capture::GazeDiagnostics g_gazeDiagnostics; +std::string g_calibrationMessage; + void HapticsThreadFunc() { double phase = 0.0; unsigned char buf[k_senseChunkSize]; @@ -65,14 +77,73 @@ void HapticsThreadFunc() { } } +void GazeRecorderThreadFunc() { + hmd2_gaze_status_t status = {}; + + while (g_appRunning) { + + if (!psvr2_toolkit_gaze_status(&status, 200)) { + continue; + } + + const int64_t hostRecvUs = gaze_capture::NowMicros(); + + { + std::scoped_lock lock(g_latestGazeMutex); + g_latestGaze = status; + } + + { + std::scoped_lock lock(g_diagnosticsMutex); + g_gazeDiagnostics.Update(status, hostRecvUs); + } + + g_gazeRecorder.Write(status, hostRecvUs); + } +} + int main(int argc, char *argv[]) { // Initialize our CAPI SDL_SharedObject *handle = static_cast(psvr2_toolkit_loader_get_module_handle()); - psvr2_toolkit_loader_init_functions(handle); - psvr2_toolkit_private_loader_init_functions(handle); + + if (!handle) { + char capiPath[1024] = {}; + psvr2_toolkit_loader_get_module_path(capiPath, sizeof(capiPath)); + + std::cerr << "Could not load psvr2_toolkit_capi.dll." << std::endl; + if (capiPath[0]) { + std::cerr << " Tried: " << capiPath << std::endl; + } else { + std::cerr << " The driver has not recorded a path yet. Start SteamVR once with the toolkit installed." << std::endl; + } + + std::cout << "Press Enter to close" << std::endl; + std::cin.get(); + return -1; + } + + // Resolve everything up front and refuse to run on a partial set. Each entry point is an inline shim + // through a function pointer, so a single unresolved symbol becomes a null call on first use -- + // crashing before a window ever appears, with no module name in the report. + const int missingPublic = psvr2_toolkit_loader_init_functions(handle); + const int missingPrivate = psvr2_toolkit_private_loader_init_functions(handle); + + if (missingPublic || missingPrivate) { + char capiPath[1024] = {}; + psvr2_toolkit_loader_get_module_path(capiPath, sizeof(capiPath)); + + std::cerr << "psvr2_toolkit_capi.dll is missing " << (missingPublic + missingPrivate) << " expected function(s)." << std::endl; + std::cerr << " Loaded: " << capiPath << std::endl; + std::cerr << " This app and that DLL are from different builds. Reinstall the toolkit DLLs from the same" << std::endl; + std::cerr << " build as this executable, then try again." << std::endl; + + std::cout << "Press Enter to close" << std::endl; + std::cin.get(); + return -1; + } int result = psvr2_toolkit_init(); - if (handle && result < 0) { + if (result < 0) { std::cerr << "Failed to initialize CAPI! Result: " << result << std::endl; std::cout << "Press Enter to close" << std::endl; @@ -135,9 +206,14 @@ int main(int argc, char *argv[]) { std::thread hapticsThread(HapticsThreadFunc); hapticsThread.detach(); // Detached so if PSVR2TK goes down, the test app can still exit cleanly + std::thread gazeRecorderThread(GazeRecorderThreadFunc); + gazeRecorderThread.detach(); // Detached for the same reason as the haptics thread + // Buffers for CAPI data - hmd2_gaze_status_t gazeStatus; - unsigned char *latestGazeImage = nullptr; + hmd2_gaze_status_t gazeStatus = {}; + + std::vector gazeImage(gaze_capture::GazeImageDumper::k_frameSize, 0); + uint32_t gazeImageSize = 0; // Gaze image is 2048x1024 8-bit grayscale, with a 256-byte header. const int IMAGE_WIDTH = 400; @@ -172,16 +248,27 @@ int main(int argc, char *argv[]) { } } - // Fetch the latest data from the CAPI - psvr2_toolkit_gaze_status(&gazeStatus, 0); - psvr2_toolkit_gaze_image(&latestGazeImage, 0); + // Fetch the latest data from the CAPI. Gaze status comes from the recorder thread's copy so that + // there is only ever one caller of psvr2_toolkit_gaze_status; see the note on g_latestGazeMutex. + { + std::scoped_lock lock(g_latestGazeMutex); + gazeStatus = g_latestGaze; + } + + uint32_t receivedSize = 0; + if (psvr2_toolkit_gaze_image_copy(gazeImage.data(), static_cast(gazeImage.size()), &receivedSize, 0)) { + gazeImageSize = receivedSize; + g_gazeImageDumper.MaybeDump(gazeImage.data(), gazeImageSize); + } // Convert and upload texture data - if (latestGazeImage) { + constexpr uint32_t k_gazeImageHeaderSize = 0x100; + const uint32_t requiredSize = k_gazeImageHeaderSize + static_cast(IMAGE_WIDTH) * static_cast(IMAGE_HEIGHT); + if (gazeImageSize >= requiredSize) { void *mapped_ptr = SDL_MapGPUTransferBuffer(gpu_device, transferBuffer, false); if (mapped_ptr) { uint32_t *dst = reinterpret_cast(mapped_ptr); - const unsigned char *src = latestGazeImage + 0x100; // Skip header + const unsigned char *src = gazeImage.data() + k_gazeImageHeaderSize; // Skip header for (size_t i = 0; i < IMAGE_WIDTH * IMAGE_HEIGHT; ++i) { unsigned char val = src[i]; dst[i] = (uint32_t)val | ((uint32_t)val << 8) | ((uint32_t)val << 16) | (0xFF << 24); // Grayscale to RGBA @@ -342,6 +429,191 @@ int main(int argc, char *argv[]) { ImGui::PopID(); } + if (ImGui::CollapsingHeader("Gaze Diagnostics", ImGuiTreeNodeFlags_DefaultOpen)) { + ImGui::PushID("GazeDiagnosticsSection"); + + gaze_capture::GazeDiagnosticsSnapshot diag; + { + std::scoped_lock lock(g_diagnosticsMutex); + diag = g_gazeDiagnostics.Snapshot(); + } + + ImGui::TextWrapped("Derived measurements only -- none of this changes driver behaviour. It exists to " + "answer the questions EYE_TRACKING_PLAN.md says must be settled on hardware before " + "tasks 4.2, 4.3 and 5.2 can be implemented safely."); + ImGui::Text("Samples: %llu", static_cast(diag.samples)); + ImGui::Separator(); + + // measured IPD, for comparison against what SteamVR already reports. + ImGui::SeparatorText("Measured IPD (task 4.3)"); + if (diag.hasIpd) { + ImGui::Text("Mean: %.2f mm (min %.2f, max %.2f, n=%zu)", diag.ipdMeanMm, diag.ipdMinMm, diag.ipdMaxMm, diag.ipdCount); + ImGui::TextWrapped("Compare against the IPD SteamVR reports. If they disagree, the driver is not " + "currently sourcing IPD from the eyes and task 4.3 is worth wiring up."); + } else { + ImGui::TextDisabled("No valid pair of gaze origins yet."); + } + ImGui::Separator(); + + // fit guidance, to calibrate a threshold empirically rather than guessing one. + ImGui::SeparatorText("Headset fit / pos_guide (task 4.2)"); + if (diag.hasFit) { + ImGui::Text("Left mean %.4f max %.4f", diag.fitLeftMean, diag.fitLeftMax); + ImGui::Text("Right mean %.4f max %.4f", diag.fitRightMean, diag.fitRightMax); + ImGui::Text("Worst right now: %.4f", diag.fitWorstNow); + ImGui::TextWrapped("Note the value with the headset seated well, then deliberately misfit it and note " + "it again. The gap between those two readings is the threshold task 4.2 needs."); + } else { + ImGui::TextDisabled("No valid pos_guide samples yet."); + } + ImGui::Separator(); + + // blink dynamics from the existing binary blink flag. + ImGui::SeparatorText("Blink dynamics (task 5.2)"); + const gaze_capture::BlinkStats &blinks = diag.blinks; + ImGui::Text("Blinks: %u rate %.1f/min", blinks.blinks, blinks.blinksPerMinute); + ImGui::Text("Duration: last %.1f ms, mean %.1f ms", blinks.lastDurationMs, blinks.meanDurationMs); + ImGui::Text("Eyes closed: %.2f%% of samples (PERCLOS basis)", blinks.closedFraction * 100.0f); + ImGui::Separator(); + + // convergence distance, which task 2.1 now also uses for the fixation point. + ImGui::SeparatorText("Convergence distance (tasks 2.1 / 4.4)"); + if (diag.hasConvergence) { + ImGui::Text("Mean: %.0f mm (min %.0f, max %.0f)", diag.convMeanMm, diag.convMinMm, diag.convMaxMm); + ImGui::TextWrapped("This now drives vGazeTarget. If it looks implausible, the driver falls back to " + "gazeDefaultFixationDistanceM and the accuracy gain from 2.1 is reduced."); + } else { + ImGui::TextDisabled("No valid convergence distance yet."); + } + + ImGui::Separator(); + + // pupillometry. + ImGui::SeparatorText("Pupillometry (task 5.3)"); + + if (!diag.calSolved) { + ImGui::TextWrapped("Camera axes are NOT calibrated. Foreshortening correction is running with a " + "straight-ahead assumption, which on real data leaves most of the gaze-angle " + "bias in place. Calibrate below before trusting these numbers."); + } else { + ImGui::Text("Camera axes: left az %+.0f el %+.0f, right az %+.0f el %+.0f (CV %.4f)", diag.calLeftAz, diag.calLeftEl, diag.calRightAz, diag.calRightEl, + diag.calResidual); + } + + if (diag.calibrating) { + ImGui::Text("Collecting... %zu samples. Look slowly around the whole field of view.", diag.calSamples); + if (ImGui::Button("Solve")) { + bool solved = false; + float la = 0.0f, le = 0.0f, ra = 0.0f, re = 0.0f; + { + std::scoped_lock lock(g_diagnosticsMutex); + solved = g_gazeDiagnostics.SolveCalibration(); + la = g_gazeDiagnostics.CalLeftAz(); + le = g_gazeDiagnostics.CalLeftEl(); + ra = g_gazeDiagnostics.CalRightAz(); + re = g_gazeDiagnostics.CalRightEl(); + } + + if (!solved) { + g_calibrationMessage = "Not enough 2D gaze spread yet - keep looking around, including up and down."; + } else { + // Push the fit into the CAPI so it is persisted beside psvr2_toolkit_capi.dll. Every other + // client -- the Baballonia module, the Unity calibration app -- reads it from there, so the + // calibration only has to be done once per headset rather than once per application. + const int pushed = psvr2_toolkit_pupillometry_set_camera_axes(la, le, ra, re); + g_calibrationMessage = (pushed == PSVR2TK_RESULT_OK) ? "Solved and saved. Other PSVR2 Toolkit apps will use these axes." + : "Solved, but the axes were rejected as implausible and were not saved."; + } + } + ImGui::SameLine(); + if (ImGui::Button("Cancel")) { + std::scoped_lock lock(g_diagnosticsMutex); + g_gazeDiagnostics.StopCalibration(); + } + } else { + if (ImGui::Button("Calibrate camera axes")) { + std::scoped_lock lock(g_diagnosticsMutex); + g_gazeDiagnostics.StartCalibration(); + g_calibrationMessage.clear(); + } + } + + if (!g_calibrationMessage.empty()) { + ImGui::TextWrapped("%s", g_calibrationMessage.c_str()); + } + + ImGui::Spacing(); + + const PupillometryResult &pupil = diag.pupil; + if (pupil.valid) { + ImGui::Text("Corrected: %.3f mm (%d eye%s)", pupil.correctedMm, pupil.contributingEyes, pupil.contributingEyes == 1 ? "" : "s"); + ImGui::Text("Smoothed: %.3f mm", pupil.smoothedMm); + if (pupil.baselineReady) { + ImGui::Text("Baseline: %.3f mm delta %+.3f mm (%+.1f%%)", pupil.baselineMm, pupil.deltaMm, pupil.relative * 100.0f); + } else { + ImGui::TextDisabled("Baseline: building..."); + } + if (pupil.activityReady) { + ImGui::Text("Activity: %.2f dilations/s", pupil.activityIndex); + } else { + ImGui::TextDisabled("Activity: building..."); + } + } else { + ImGui::TextDisabled("No accepted pupil sample yet."); + } + + const uint64_t total = diag.pupilAccepted + diag.pupilRejected; + ImGui::Text("Accepted %llu / rejected %llu (%.1f%% rejected)", static_cast(diag.pupilAccepted), + static_cast(diag.pupilRejected), + total ? 100.0 * static_cast(diag.pupilRejected) / static_cast(total) : 0.0); + + ImGui::TextWrapped("Rejections are mostly blink guard bands, which are intentional -- blink artifacts " + "measured 0.27 mm on real data, the same size as the effect being looked for."); + ImGui::TextWrapped("Prefer 'Activity' over 'delta': there is no display-luminance signal anywhere in the " + "headset telemetry, so absolute diameter cannot be separated from the scene simply " + "getting brighter. Activity counts abrupt dilations and rides out slow light changes."); + + ImGui::PopID(); + } + + if (ImGui::CollapsingHeader("Gaze Capture", ImGuiTreeNodeFlags_DefaultOpen)) { + ImGui::PushID("GazeCaptureSection"); + + const bool recording = g_gazeRecorder.IsActive(); + if (ImGui::Button(recording ? "Stop Status Recording" : "Start Status Recording")) { + if (recording) { + g_gazeRecorder.Stop(); + } else { + g_gazeRecorder.Start(); + } + } + + ImGui::Text("Rows: %llu Frame gaps: %llu", static_cast(g_gazeRecorder.Rows()), + static_cast(g_gazeRecorder.Gaps())); + + const std::string recorderPath = g_gazeRecorder.Path(); + ImGui::TextWrapped("File: %s", recorderPath.empty() ? "(none)" : recorderPath.c_str()); + + ImGui::Separator(); + + ImGui::SliderInt("Dump every Nth frame", &g_gazeImageDumper.Stride(), 1, 60); + ImGui::SliderInt("Max frames", &g_gazeImageDumper.MaxFrames(), 1, 2000); + + const bool dumping = g_gazeImageDumper.IsActive(); + if (ImGui::Button(dumping ? "Stop Eye Image Dump" : "Start Eye Image Dump")) { + if (dumping) { + g_gazeImageDumper.Stop(); + } else { + g_gazeImageDumper.Start(); + } + } + + ImGui::Text("Eye frames written: %d", g_gazeImageDumper.Written()); + ImGui::TextWrapped("Folder: %s", g_gazeImageDumper.Directory().empty() ? "(none)" : g_gazeImageDumper.Directory().c_str()); + + ImGui::PopID(); + } + if (ImGui::CollapsingHeader("Gaze Status", ImGuiTreeNodeFlags_DefaultOpen)) { ImGui::Text("Magic: %c%c", gazeStatus.magic[0], gazeStatus.magic[1]); ImGui::Text("Version: %u", gazeStatus.version); @@ -440,6 +712,7 @@ int main(int argc, char *argv[]) { // Cleanup g_appRunning = false; + g_gazeRecorder.Stop(); ImGui_ImplSDLGPU3_Shutdown(); ImGui_ImplSDL3_Shutdown();