diff --git a/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsCapture.kt b/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsCapture.kt index a1c83fa7..5b54ca32 100644 --- a/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsCapture.kt +++ b/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsCapture.kt @@ -22,10 +22,10 @@ import android.view.InputDevice * * Input: parse ([DsDevice.parseState]) → typed mirror on an [GamepadRouter.ExternalPad] (buttons * diffed, axes on-change — the exit chord participates like any pad) + the rich plane (touch - * normalized to the wire's 0..65535 screen space on-change; motion forwarded per report in raw - * device units, the wire's contract). The wire slot is claimed when the capture engages, with the - * first parsed report as the fallback for a claim that found no free index, and freed on - * unplug/[stop], so indices never leak. + * normalized to the wire's 0..65535 screen space on-change; motion forwarded per report, rescaled + * into the wire's units by this pad's own calibration, read once at claim). The wire slot is + * claimed when the capture engages, with the first parsed report as the fallback for a claim that + * found no free index, and freed on unplug/[stop], so indices never leak. * * Feedback: implements [GamepadFeedback.PadFeedbackSink] — rumble / trigger / lightbar / player * LED events addressed to this pad's wire index become USB output reports on the physical pad @@ -55,6 +55,10 @@ class DsCapture( @Volatile private var model: DsDevice.Model? = null @Volatile private var pad: GamepadRouter.ExternalPad? = null + /** This pad's factory motion scale, read once per capture (see [readMotionCal]). Written on + * the claiming thread before [model], which is what the link thread's parse reads it under. */ + @Volatile private var motionCal = DsDevice.MotionCal.NOMINAL + // Typed-mirror diff state (wire units) + rich-plane on-change mirrors. Link thread only. private val state = DsDevice.State() private var wireButtons = 0 @@ -124,6 +128,9 @@ class DsCapture( if (model != null) return false val m = DsDevice.modelFor(dev.productId) ?: return false if (!usb.start(dev)) return false + // Before `model`, which is what gates the link thread's parse: a report must never be + // scaled by the previous pad's calibration, or by the fallback once the real one is known. + motionCal = readMotionCal(m) model = m for (id in InputDevice.getDeviceIds()) { val d = InputDevice.getDevice(id) ?: continue @@ -138,6 +145,34 @@ class DsCapture( return true } + /** + * Read this pad's IMU calibration, ONCE, while claiming it — the feature report that says how + * many raw counts this individual unit puts on a °/s and on a g ([DsDevice.MotionCal]). + * + * At claim time and nowhere else: the read is a blocking EP0 control transfer (bounded by the + * link's write timeout, answered in about a millisecond by a pad that is there), and the + * calibration is fixed for the life of the connection, so doing it per input report would buy + * nothing and cost the capture its latency. A pad that refuses keeps the nominal scaling + * rather than losing motion altogether. + */ + private fun readMotionCal(m: DsDevice.Model): DsDevice.MotionCal { + val blob = usb.getReport(HidUsbLink.REPORT_TYPE_FEATURE, m.calReportId, m.calReportLen) + val cal = DsDevice.MotionCal.parse(blob, m.calReportId) + // Worth a line either way: this is the number the owed on-glass check reads back — a pad + // whose blob was read declares its own resolution, the fallback declares the wire's. + if (cal === DsDevice.MotionCal.NOMINAL) { + Log.w( + TAG, + "motion calibration 0x%02x unreadable (%d/%d B) — nominal scaling (%s)".format( + m.calReportId, blob?.size ?: 0, m.calReportLen, cal, + ), + ) + } else { + Log.i(TAG, "motion calibration 0x%02x: %s".format(m.calReportId, cal)) + } + return cal + } + /** Stop the link and free the wire slot (host tears the virtual pad down). Idempotent. */ fun stop() { // Before anything touches the link: the pad-audio renderer borrows this connection's @@ -168,7 +203,7 @@ class DsCapture( private fun onReport(report: ByteArray, len: Int) { val m = model ?: return - if (!DsDevice.parseState(m, report, len, state)) return + if (!DsDevice.parseState(m, report, len, state, motionCal)) return // Normally claimed already, at capture time; this is the retry for a capture that engaged // while every wire index was taken. val p = pad ?: ensureSlot(m) ?: return // all 16 taken — drop until one frees @@ -310,8 +345,8 @@ class DsCapture( /** * The rich plane: touch contacts normalized to the wire's 0..65535 screen space, forwarded - * on change per slot; motion forwarded every report (raw device units — the wire is a unit - * passthrough into the host's virtual pad, and sensor noise makes per-report dedup pointless). + * on change per slot; motion forwarded every report (already in wire units — the parse applies + * this pad's calibration, and sensor noise makes per-report dedup pointless). */ private fun mirrorRich(p: GamepadRouter.ExternalPad, m: DsDevice.Model) { for (f in 0 until 2) { diff --git a/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsDevice.kt b/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsDevice.kt index 4aedf25c..6274636d 100644 --- a/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsDevice.kt +++ b/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/DsDevice.kt @@ -1,5 +1,7 @@ package io.unom.punktfunk.kit +import kotlin.math.abs + /** * Sony DualSense / DualSense Edge / DualShock 4 **USB** protocol constants: the input-report * parser and the output-report builders the capture link ([DsCapture]) needs. Unlike the SC2's @@ -17,11 +19,6 @@ package io.unom.punktfunk.kit * reaches this code — an uncaptured pad stays on the ordinary InputDevice path. */ object DsDevice { - /** The pads' native acceleration resolution — `hid-playstation`'s `DS_ACC_RES_PER_G`. */ - private const val DS_RAW_ACCEL_LSB_PER_G = 8192L - /** The wire's, from `punktfunk_core::input::gamepad::MOTION_ACCEL_LSB_PER_G`. */ - private const val WIRE_ACCEL_LSB_PER_G = 10000L - const val VID_SONY = 0x054C const val PID_DUALSENSE = 0x0CE6 const val PID_DUALSENSE_EDGE = 0x0DF2 @@ -33,14 +30,159 @@ object DsDevice { /** * One captured model: its `GamepadPref` wire byte (the virtual pad the host builds — matching * the physical one), its output-report size (the descriptor-declared size the firmware - * expects: DS5 48 = id + 47, Edge 64 = id + 63, DS4 32 = id + 31), and its touchpad extent + * expects: DS5 48 = id + 47, Edge 64 = id + 63, DS4 32 = id + 31), its touchpad extent * (`dualsense_proto::DS_TOUCH_W/H`, `dualshock4_proto::DS4_TOUCH_*`) for normalizing touches - * onto the wire's 0..65535 space. + * onto the wire's 0..65535 space, and the IMU-calibration feature report it answers + * ([MotionCal]): DS5/Edge `0x05` (id + 40 B), DS4 over USB `0x02` (id + 36 B). */ - enum class Model(val pref: Int, val outputSize: Int, val touchW: Int, val touchH: Int) { - DUALSENSE(Gamepad.PREF_DUALSENSE, 48, 1920, 1080), - DUALSENSE_EDGE(Gamepad.PREF_DUALSENSEEDGE, 64, 1920, 1080), - DUALSHOCK4(Gamepad.PREF_DUALSHOCK4, 32, 1920, 942), + enum class Model( + val pref: Int, + val outputSize: Int, + val touchW: Int, + val touchH: Int, + val calReportId: Int, + val calReportLen: Int, + ) { + DUALSENSE(Gamepad.PREF_DUALSENSE, 48, 1920, 1080, 0x05, 41), + DUALSENSE_EDGE(Gamepad.PREF_DUALSENSEEDGE, 64, 1920, 1080, 0x05, 41), + DUALSHOCK4(Gamepad.PREF_DUALSHOCK4, 32, 1920, 942, 0x02, 37), + } + + /** + * One pad's own IMU calibration: the factory scale factors that turn its raw motion counts + * into the wire's fixed units (`punktfunk_core::input::gamepad` — 20 LSB per °/s, 10000 LSB + * per g), read out of the calibration feature report the pad serves on EP0. + * + * **Why the pad's blob and not a constant.** Measured on glass 2026-08-07: a DualSense flat + * and face up arrived as 0.811 g where 1.000 was owed, because this path forwarded the raw + * i16s verbatim. The nominal ×10000/8192 rescale that first closed that gap ([NOMINAL]) still + * leaves that unit's factory bias — about 1 % — on acceleration, and provably cannot fix gyro + * at all: the same still-average showed this pad's gyro calibration is nowhere near identity, + * and a near-identity one would mean 1024 LSB per °/s, i.e. ±32 °/s full scale, which no + * controller has. The scale is per unit; only the pad knows it. + * + * The arithmetic is `hid-playstation`'s, and the host's contract test + * (`crates/pf-inject/tests/motion_contract.rs`, `SonyImuCalibration`) is the same math read + * from the other end — it applies it to the blobs our *virtual* pads declare and asserts they + * land on the wire constants. Per axis: gyro `raw × speed_2x × 20 / (|plus − bias| + + * |minus − bias|)`, accel `(raw − (plus − range/2)) × 20000 / range`, where `range = plus − + * minus` spans 2 g. + */ + class MotionCal private constructor( + /** Per axis: `speed_2x × 20`, over `|plus − bias| + |minus − bias|`. */ + private val gyroNumer: LongArray, + private val gyroDenom: LongArray, + /** Per axis: the raw count the pad reads at 0 g, and the raw span of 2 g. */ + private val accelBias: LongArray, + private val accelRange: LongArray, + ) { + /** Raw gyro count on [axis] (0 = pitch, 1 = yaw, 2 = roll) → the wire's 20 LSB per °/s. */ + fun gyroToWire(axis: Int, raw: Int): Int = + clampWire(raw.toLong() * gyroNumer[axis] / gyroDenom[axis]) + + /** Raw acceleration count on [axis] → the wire's 10000 LSB per g, zero point removed. */ + fun accelToWire(axis: Int, raw: Int): Int = + clampWire((raw - accelBias[axis]) * ACCEL_NUMER / accelRange[axis]) + + /** + * The derived resolutions, for the capture's one-line claim log — the number that says + * whether a pad's blob was actually read (a real DualSense declares ≈16 LSB/°·s and ≈8192 + * LSB/g; the [NOMINAL] fallback reads back as exactly 20 and 8192). + */ + override fun toString(): String = buildString { + append("gyro ") + for (i in 0 until 3) { + if (i > 0) append('/') + append(gyroDenom[i] * WIRE_GYRO_LSB_PER_DEG_S / gyroNumer[i]) + } + append(" LSB/°·s, accel ") + for (i in 0 until 3) { + if (i > 0) append('/') + append(accelRange[i] / 2) + } + append(" LSB/g at ") + append(accelBias.joinToString("/")) + } + + /** + * Both conversions are a >1 multiplier on every pad measured so far, so a real ±4 g slam + * or a fast flick near full scale would otherwise wrap the i16 and read as an impossible + * motion in the opposite direction. + */ + private fun clampWire(v: Long): Int = v.coerceIn(-32768L, 32767L).toInt() + + companion object { + /** The pads' nominal acceleration resolution — `hid-playstation`'s `DS_ACC_RES_PER_G`. */ + private const val RAW_ACCEL_LSB_PER_G = 8192L + /** `punktfunk_core::input::gamepad::MOTION_GYRO_LSB_PER_DEG_S`. */ + private const val WIRE_GYRO_LSB_PER_DEG_S = 20L + /** `MOTION_ACCEL_LSB_PER_G`, doubled — the declared accel range spans 2 g, not 1. */ + private const val ACCEL_NUMER = 2 * 10000L + /** Bytes the layout below reads; the reports themselves are longer (41 / 37). */ + private const val MIN_LEN = 35 + + /** + * What an unreadable pad gets: gyro straight through and accel on the nominal 8192 + * LSB/g. Wrong by that unit's factory bias, and for gyro wrong by however far its + * scale sits from the wire's 20 — but a pad whose calibration cannot be read is far + * better off slightly mis-scaled than silent, so this never zeroes motion. + */ + val NOMINAL = MotionCal( + LongArray(3) { 1 }, + LongArray(3) { 1 }, + LongArray(3), + LongArray(3) { 2 * RAW_ACCEL_LSB_PER_G }, + ) + + /** + * Parse a calibration feature report ([Model.calReportId]) — all little-endian i16: + * `[0]` report id, `[1..7)` gyro bias (pitch, yaw, roll), `[7..19)` gyro plus/minus + * INTERLEAVED (pitch+, pitch−, yaw+, yaw−, roll+, roll−), `[19..23)` the two speed + * words, `[23..35)` accel plus/minus (x+, x−, y+, y−, z+, z−). + * + * ⚠ Interleaved is the **USB** order. A Bluetooth DualShock 4 groups the three plusses + * before the three minuses and consumers switch layout on the transport — this path is + * USB-only by construction (see the file header), so do not "generalise" it. + * + * Falls back to [NOMINAL] for a failed read (null), a truncated or foreign reply, and + * per axis for a degenerate declaration — a clone or broken pad that declares zeroes + * would otherwise divide by zero (`hid-playstation` guards the same case, for the same + * reason). + */ + fun parse(blob: ByteArray?, reportId: Int): MotionCal { + if (blob == null || blob.size < MIN_LEN) return NOMINAL + if ((blob[0].toInt() and 0xFF) != reportId) return NOMINAL + val w = { o: Int -> + ((blob[o + 1].toInt() shl 8) or (blob[o].toInt() and 0xFF)).toShort().toLong() + } + val speed2x = w(19) + w(21) + val gyroNumer = LongArray(3) + val gyroDenom = LongArray(3) + val accelBias = LongArray(3) + val accelRange = LongArray(3) + for (i in 0 until 3) { + val bias = w(1 + 2 * i) + val denom = abs(w(7 + 4 * i) - bias) + abs(w(9 + 4 * i) - bias) + if (speed2x > 0 && denom > 0) { + gyroNumer[i] = speed2x * WIRE_GYRO_LSB_PER_DEG_S + gyroDenom[i] = denom + } else { + gyroNumer[i] = 1 // passthrough, as before any calibration existed + gyroDenom[i] = 1 + } + val plus = w(23 + 4 * i) + val range = plus - w(25 + 4 * i) + if (range > 0) { + accelBias[i] = plus - range / 2 + accelRange[i] = range + } else { + accelBias[i] = 0 // nominal, as NOMINAL above + accelRange[i] = 2 * RAW_ACCEL_LSB_PER_G + } + } + return MotionCal(gyroNumer, gyroDenom, accelBias, accelRange) + } + } } /** The captured [Model] for a USB PID, or null for anything we don't capture. */ @@ -55,8 +197,9 @@ object DsDevice { * The client-consumed fields of one input report. `buttons` is already the WIRE bitmask * (`Gamepad.BTN_*`) — the parse maps device bits straight to the wire, the exact inverse of * the host's `DsState::from_gamepad` (BTN_A ↔ cross, BTN_B ↔ circle, BTN_X ↔ square, - * BTN_Y ↔ triangle; positional, not glyph-order). Gyro/accel stay in raw device units — the - * wire's `Motion` is a unit passthrough into the virtual pad's report. Touch coordinates stay + * BTN_Y ↔ triangle; positional, not glyph-order). Gyro/accel arrive in WIRE units — the wire's + * `Motion` is a unit passthrough into the virtual pad's report, so the pad's raw counts are + * rescaled during the parse by the [MotionCal] handed to [parseState]. Touch coordinates stay * device-raw here; [DsCapture] normalizes against the model's extent when forwarding. */ class State { @@ -64,8 +207,8 @@ object DsDevice { var lsX = 0; var lsY = 0 // wire i16, +y = up (device is +y down — inverted in the parse) var rsX = 0; var rsY = 0 var lt = 0; var rt = 0 // 0..255 - val gyro = IntArray(3) // raw i16 units (pitch/yaw/roll) - val accel = IntArray(3) + val gyro = IntArray(3) // wire i16: 20 LSB per °/s (pitch/yaw/roll) + val accel = IntArray(3) // wire i16: 10000 LSB per g val touchActive = BooleanArray(2) val touchX = IntArray(2) // raw device coords (0..touchW-1 / 0..touchH-1) val touchY = IntArray(2) @@ -113,15 +256,25 @@ object DsDevice { * short read (the pad also emits `0x09`-family getMAC responses etc. on EP0 — those never hit * the interrupt endpoint, but be defensive). Motion/touch fields update only when the report * is long enough to carry them (it always is on glass — 64-byte interrupt transfers). + * + * [cal] is this pad's own motion calibration, read once when the capture claims it; the + * default is the nominal fallback, which is all a caller without a live pad (the tests) can + * have. */ - fun parseState(model: Model, report: ByteArray, len: Int, out: State): Boolean = + fun parseState( + model: Model, + report: ByteArray, + len: Int, + out: State, + cal: MotionCal = MotionCal.NOMINAL, + ): Boolean = if (model == Model.DUALSHOCK4) { - parseDs4(report, len, out) + parseDs4(report, len, out, cal) } else { - parseDs5(model, report, len, out) + parseDs5(model, report, len, out, cal) } - private fun parseDs5(model: Model, r: ByteArray, len: Int, out: State): Boolean { + private fun parseDs5(model: Model, r: ByteArray, len: Int, out: State, cal: MotionCal): Boolean { if (len < 11 || (r[0].toInt() and 0xFF) != DS5_INPUT_ID) return false out.lsX = stickX(u8(r, 1)) out.lsY = stickY(u8(r, 2)) @@ -157,8 +310,8 @@ object DsDevice { } out.buttons = w if (len >= 28) { - for (i in 0 until 3) out.gyro[i] = i16(r, 16 + 2 * i) - for (i in 0 until 3) out.accel[i] = accelToWire(i16(r, 22 + 2 * i)) + for (i in 0 until 3) out.gyro[i] = cal.gyroToWire(i, i16(r, 16 + 2 * i)) + for (i in 0 until 3) out.accel[i] = cal.accelToWire(i, i16(r, 22 + 2 * i)) } if (len >= 41) { unpackTouch(r, 33, out, 0) @@ -167,7 +320,7 @@ object DsDevice { return true } - private fun parseDs4(r: ByteArray, len: Int, out: State): Boolean { + private fun parseDs4(r: ByteArray, len: Int, out: State, cal: MotionCal): Boolean { if (len < 10 || (r[0].toInt() and 0xFF) != DS5_INPUT_ID) return false // DS4 shares id 0x01 out.lsX = stickX(u8(r, 1)) out.lsY = stickY(u8(r, 2)) @@ -193,8 +346,8 @@ object DsDevice { if (b7 and DS4_TOUCHPAD != 0) w = w or Gamepad.BTN_TOUCHPAD out.buttons = w if (len >= 25) { - for (i in 0 until 3) out.gyro[i] = i16(r, 13 + 2 * i) - for (i in 0 until 3) out.accel[i] = accelToWire(i16(r, 19 + 2 * i)) + for (i in 0 until 3) out.gyro[i] = cal.gyroToWire(i, i16(r, 13 + 2 * i)) + for (i in 0 until 3) out.accel[i] = cal.accelToWire(i, i16(r, 19 + 2 * i)) } if (len >= 43) { unpackTouch(r, 35, out, 0) @@ -232,28 +385,6 @@ object DsDevice { private fun i16(r: ByteArray, o: Int): Int = ((r[o + 1].toInt() shl 8) or (r[o].toInt() and 0xFF)).toShort().toInt() - /** - * Raw DualSense/DualShock 4 acceleration → the wire's units. - * - * The pad reports acceleration in its own device units; the wire is fixed at - * `MOTION_ACCEL_LSB_PER_G` = 10000 LSB per g (`punktfunk_core::input::gamepad`). Forwarding the - * raw value verbatim — which this path did until 2026-08-07 — hands the host a number ~18 % - * short, because the pad's native resolution is the 8192 LSB/g that `hid-playstation` calls - * `DS_ACC_RES_PER_G`. Measured on glass: a DualSense flat and face up arrived as 0.811 g where - * 1.000 was owed, against 8192/10000 = 0.819 predicted. - * - * The residual ~1 % is this unit's factory bias, which only its calibration feature report can - * remove — that read is still owed (it also fixes gyro, whose factory calibration is emphatically - * NOT near-identity and so cannot be corrected by a nominal constant like this one). - * - * Clamped because the rescale is a >1 multiplier: a real ±4 g slam near full scale would - * otherwise wrap the i16 and read as an impossible acceleration in the opposite direction. - */ - private fun accelToWire(raw: Int): Int = - ((raw.toLong() * WIRE_ACCEL_LSB_PER_G) / DS_RAW_ACCEL_LSB_PER_G) - .coerceIn(-32768L, 32767L) - .toInt() - // Device stick byte (0..255, centre 0x80, +y down) → wire i16 (+y up) — the exact inverse of // the host's `to_u8` mapping (`lx = to_u8(x)`, `ly = 255 - to_u8(y)`). private fun stickX(raw: Int): Int = raw * 257 - 32768 diff --git a/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/HidUsbLink.kt b/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/HidUsbLink.kt index 254c1bb8..57005e3e 100644 --- a/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/HidUsbLink.kt +++ b/clients/android/kit/src/main/kotlin/io/unom/punktfunk/kit/HidUsbLink.kt @@ -442,6 +442,42 @@ class HidUsbLink( return n >= 0 } + /** + * Read one report back OUT of the device — HID `GET_REPORT`, the EP0 mirror of [sendReport]. + * [type] is [REPORT_TYPE_FEATURE] (or output), [id] the report number, [len] the report's full + * declared size INCLUDING its leading id byte, which a numbered report echoes back in byte 0 + * (hidapi framing). Returns what arrived — truncated if the device answered short — or null + * when the device refuses the request or the link is down. + * + * ⚠ **Once, at claim time; never per input report.** EP0 is independent of the interrupt + * endpoints (see [sendReport]), so this is safe alongside the reader thread — but it BLOCKS the + * calling thread for up to [WRITE_TIMEOUT_MS], and a blocking control transfer in the report + * path would wreck capture latency. The one caller reads a Sony pad's fixed motion calibration + * when the capture engages ([DsCapture]). + */ + fun getReport(type: Int, id: Int, len: Int): ByteArray? { + if (len <= 0) return null + val conn = connection ?: return null + val ifId = (activeClaim ?: claims.firstOrNull())?.iface?.id ?: return null + val buf = ByteArray(len) + val n = runCatching { + conn.controlTransfer( + 0xA1, // device→host, class, interface + 0x01, // GET_REPORT + (type shl 8) or id, + ifId, + buf, + buf.size, + WRITE_TIMEOUT_MS, + ) + }.getOrDefault(-1) + return when { + n >= len -> buf + n > 0 -> buf.copyOf(n) + else -> null + } + } + /** * Stop the read loop and release the interfaces. Idempotent; does not fire [onClosed]. * @@ -469,12 +505,13 @@ class HidUsbLink( device = null } - private companion object { - const val READ_TIMEOUT_MS = 100L - const val WRITE_TIMEOUT_MS = 250 + companion object { + private const val READ_TIMEOUT_MS = 100L + private const val WRITE_TIMEOUT_MS = 250 /** Hard `requestWait` ERRORS (not timeouts) persisting this long = the fd is dead. */ - const val ERROR_UNPLUG_MS = 2000L - const val REPORT_TYPE_OUTPUT = 0x02 + private const val ERROR_UNPLUG_MS = 2000L + private const val REPORT_TYPE_OUTPUT = 0x02 + /** HID feature-report type — public for [getReport] callers ([writeRaw] takes a kind). */ const val REPORT_TYPE_FEATURE = 0x03 } } diff --git a/clients/android/kit/src/test/kotlin/io/unom/punktfunk/kit/DsDeviceTest.kt b/clients/android/kit/src/test/kotlin/io/unom/punktfunk/kit/DsDeviceTest.kt index ca1fb3ca..ed2d5bce 100644 --- a/clients/android/kit/src/test/kotlin/io/unom/punktfunk/kit/DsDeviceTest.kt +++ b/clients/android/kit/src/test/kotlin/io/unom/punktfunk/kit/DsDeviceTest.kt @@ -151,6 +151,185 @@ class DsDeviceTest { assertFalse(DsDevice.parseState(DsDevice.Model.DUALSHOCK4, ds4Report(), 8, s)) } + // ---- IMU calibration (the pad's own scale factors) ---- + + /** + * A calibration feature report in the pads' USB layout: report id, three gyro bias words, six + * INTERLEAVED gyro plus/minus words, the two speed words, six accel plus/minus words — all + * little-endian i16, exactly what [DsDevice.MotionCal.parse] reads and what + * `crates/pf-inject/tests/motion_contract.rs` writes from the other end. + */ + private fun calBlob( + id: Int, + gyroBias: IntArray, + gyroPlus: IntArray, + gyroMinus: IntArray, + speed: Int, + accelPlus: IntArray, + accelMinus: IntArray, + len: Int = 41, + ): ByteArray = ByteArray(len).also { b -> + fun put(o: Int, v: Int) { + b[o] = (v and 0xFF).toByte() + b[o + 1] = ((v shr 8) and 0xFF).toByte() + } + b[0] = id.toByte() + for (i in 0 until 3) { + put(1 + 2 * i, gyroBias[i]) + put(7 + 4 * i, gyroPlus[i]) + put(9 + 4 * i, gyroMinus[i]) + put(23 + 4 * i, accelPlus[i]) + put(25 + 4 * i, accelMinus[i]) + } + put(19, speed) + put(21, speed) + } + + /** + * A realistic DualSense blob: gyro measured at 512 °/s each way over ±8192 counts about a + * small factory bias — 16384/1024 = 16 raw LSB per °/s, the ≈±2000 °/s full scale a real pad + * has — and accel spanning about ±8192 counts (`DS_ACC_RES_PER_G`) about a per-axis zero point + * that is NOT zero. Both are the shape a nominal constant cannot express. + */ + private fun realisticCal(): DsDevice.MotionCal = DsDevice.MotionCal.parse( + calBlob( + id = 0x05, + gyroBias = intArrayOf(10, -6, 3), + gyroPlus = intArrayOf(10 + 8192, -6 + 8192, 3 + 8192), + gyroMinus = intArrayOf(10 - 8192, -6 - 8192, 3 - 8192), + speed = 512, // speed_plus + speed_minus = 1024 + accelPlus = intArrayOf(8300, 8200, 8000), + accelMinus = intArrayOf(-8100, -8192, -8384), + ), + 0x05, + ) + + @Test + fun calibrationRescalesRawCountsOntoTheWireUnits() { + val cal = realisticCal() + // 100 °/s at this pad's 16 LSB per °/s = 1600 raw → the wire's 20 LSB per °/s = 2000. + for (axis in 0 until 3) { + assertEquals(2000, cal.gyroToWire(axis, 1600)) + assertEquals(-2000, cal.gyroToWire(axis, -1600)) + assertEquals(0, cal.gyroToWire(axis, 0)) + } + // 1 g = the axis's zero point plus half its declared 2 g range → 10000 wire units. + val zero = intArrayOf(100, 4, -192) // plus − range/2, per axis + val oneG = intArrayOf(8300, 8200, 8000) // = accelPlus + for (axis in 0 until 3) { + assertEquals(10000, cal.accelToWire(axis, oneG[axis])) + assertEquals(0, cal.accelToWire(axis, zero[axis])) + assertEquals(-10000, cal.accelToWire(axis, zero[axis] - (oneG[axis] - zero[axis]))) + } + // Both rescales are >1 here, so full-scale raw must clamp rather than wrap the i16. + assertEquals(32767, cal.gyroToWire(0, 30000)) + assertEquals(-32768, cal.gyroToWire(0, -30000)) + assertEquals(32767, cal.accelToWire(0, 30000)) + // The capture logs this, and it is the discriminator the owed on-glass check reads: a pad + // whose blob was read declares its own resolution, the fallback declares the wire's. + assertTrue(cal.toString().startsWith("gyro 16/16/16 LSB/°·s")) + assertTrue(DsDevice.MotionCal.NOMINAL.toString().startsWith("gyro 20/20/20 LSB/°·s")) + } + + /** + * The host's own virtual pads declare `DS_FEATURE_CALIBRATION` (`dualsense_proto.rs`) — a blob + * that states the wire's units exactly. Reading it back must therefore be a passthrough: if + * this ever stops holding, the client and the host disagree about what a motion sample means. + */ + @Test + fun theHostsOwnBlobIsAPassthrough() { + val cal = DsDevice.MotionCal.parse( + calBlob( + id = 0x05, + gyroBias = intArrayOf(0, 0, 0), + gyroPlus = intArrayOf(10000, 10000, 10000), + gyroMinus = intArrayOf(-10000, -10000, -10000), + speed = 500, + accelPlus = intArrayOf(10000, 10000, 10000), + accelMinus = intArrayOf(-10000, -10000, -10000), + ), + 0x05, + ) + for (axis in 0 until 3) { + assertEquals(2000, cal.gyroToWire(axis, 2000)) // 100 °/s + assertEquals(10000, cal.accelToWire(axis, 10000)) // 1 g + assertEquals(-1234, cal.gyroToWire(axis, -1234)) + } + } + + /** + * Anything unusable keeps the pre-calibration behaviour — accel on the nominal 8192 LSB/g, + * gyro straight through. A pad with no readable calibration is better off slightly mis-scaled + * than silent, so nothing here may zero motion. + */ + @Test + fun unusableCalibrationFallsBackInsteadOfZeroing() { + val degenerate = calBlob( + id = 0x02, + gyroBias = intArrayOf(0, 0, 0), + gyroPlus = intArrayOf(0, 0, 0), + gyroMinus = intArrayOf(0, 0, 0), + speed = 0, + accelPlus = intArrayOf(0, 0, 0), + accelMinus = intArrayOf(0, 0, 0), + len = 37, + ) + val cals = listOf( + DsDevice.MotionCal.NOMINAL, + DsDevice.MotionCal.parse(null, 0x05), // the GET_REPORT failed + DsDevice.MotionCal.parse(ByteArray(8) { if (it == 0) 0x05 else 0 }, 0x05), // short reply + DsDevice.MotionCal.parse(degenerate, 0x02), // a clone pad's zeroes + DsDevice.MotionCal.parse(degenerate, 0x05), // someone else's report id + ) + for (cal in cals) { + for (axis in 0 until 3) { + assertEquals(1234, cal.gyroToWire(axis, 1234)) // passthrough + assertEquals(10000, cal.accelToWire(axis, 8192)) // 8192 raw LSB = 1 g + assertEquals(-10000, cal.accelToWire(axis, -8192)) + } + } + } + + /** The parse applies the calibration at the motion offsets, per model, and defaults to nominal. */ + @Test + fun parseStateAppliesTheCalibration() { + val cal = realisticCal() + // DS5: gyro at [16..22), accel at [22..28). Pitch = 1600 raw (100 °/s), accel z = 8000 (1 g). + val ds5 = ds5Report { + it[16] = 0x40; it[17] = 0x06 // 1600 + it[26] = 0x40; it[27] = 0x1F // 8000 + } + val five = DsDevice.State() + assertTrue(DsDevice.parseState(DsDevice.Model.DUALSENSE, ds5, 64, five, cal)) + assertEquals(2000, five.gyro[0]) + assertEquals(10000, five.accel[2]) + // DS4: gyro at [13..19), accel at [19..25). Same numbers, same answers. + val ds4 = ds4Report { + it[13] = 0x40; it[14] = 0x06 + it[23] = 0x40; it[24] = 0x1F + } + val four = DsDevice.State() + assertTrue(DsDevice.parseState(DsDevice.Model.DUALSHOCK4, ds4, 64, four, cal)) + assertEquals(2000, four.gyro[0]) + assertEquals(10000, four.accel[2]) + // No calibration argument = the nominal fallback: gyro through, accel ×10000/8192. + val nominal = DsDevice.State() + assertTrue(DsDevice.parseState(DsDevice.Model.DUALSENSE, ds5, 64, nominal)) + assertEquals(1600, nominal.gyro[0]) + assertEquals(8000L * 10000 / 8192, nominal.accel[2].toLong()) + } + + /** Each model asks for the feature report its firmware actually serves over USB. */ + @Test + fun calibrationReportIdentityPerModel() { + assertEquals(0x05, DsDevice.Model.DUALSENSE.calReportId) + assertEquals(41, DsDevice.Model.DUALSENSE.calReportLen) + assertEquals(0x05, DsDevice.Model.DUALSENSE_EDGE.calReportId) + assertEquals(41, DsDevice.Model.DUALSENSE_EDGE.calReportLen) + assertEquals(0x02, DsDevice.Model.DUALSHOCK4.calReportId) + assertEquals(37, DsDevice.Model.DUALSHOCK4.calReportLen) + } + // ---- output builders (offsets = the host parser's: `parse_ds_output` / `parse_ds4_output`) ---- @Test