WIP: checkpoint Wii native IMU and upstream IR bridge
Pause Wii pointing work with firmware 0.33-wii-trace installed. Preserve the pinned libogc IR pipeline, calibrated native IMU, software BOOTSEL, and standard camera sensitivity trial. Tracking instability, tracking loss, and ineffective vertical movement remain unresolved. Level-2 camera filtering is not hardware-qualified. Nine targeted regression tests pass; firmware and persistent storage were verified after flashing.
This commit is contained in:
parent
3040c9d294
commit
9f6dddb790
41 changed files with 7990 additions and 150 deletions
138
tools/prepare_libogc_ir.py
Executable file
138
tools/prepare_libogc_ir.py
Executable file
|
|
@ -0,0 +1,138 @@
|
|||
#!/usr/bin/env python3
|
||||
"""Extract the pinned libogc Wiiuse IR math into a build-local C source file."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
UPSTREAM_COMMIT = "a4064a86487c46d8ab76d4fdf99e8059a62c4fa2"
|
||||
IR_GIT_BLOB = "d9659f5da575861f574d3d0480887955ef8f8faf"
|
||||
COMPONENT = Path(__file__).resolve().parents[1] / "external" / "libogc_ir"
|
||||
|
||||
|
||||
class PreparationError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
def _check_sha256(data: bytes, expected: str, label: str) -> None:
|
||||
actual = hashlib.sha256(data).hexdigest()
|
||||
if actual != expected:
|
||||
raise PreparationError(
|
||||
f"{label}: SHA-256 mismatch: expected {expected}, got {actual}"
|
||||
)
|
||||
|
||||
|
||||
def _section(data: bytes, section: dict) -> bytes:
|
||||
first, last = section["first_line"], section["last_line"]
|
||||
lines = data.splitlines(keepends=True)
|
||||
label = f"{section['source']}:{first}-{last}"
|
||||
if not 1 <= first <= last <= len(lines):
|
||||
raise PreparationError(f"{label}: extraction range is outside the source")
|
||||
selected = lines[first - 1 : last]
|
||||
if (
|
||||
selected[0].decode("utf-8").rstrip("\r\n") != section["first_line_text"]
|
||||
or selected[-1].decode("utf-8").rstrip("\r\n") != section["last_line_text"]
|
||||
):
|
||||
raise PreparationError(f"{label}: extraction boundary marker mismatch")
|
||||
content = b"".join(selected)
|
||||
_check_sha256(content, section["sha256"], label)
|
||||
return content
|
||||
|
||||
|
||||
def prepare(output: Path) -> Path:
|
||||
"""Write unchanged upstream math sections; preserve mtime when bytes match.
|
||||
|
||||
Input hashes, extraction boundaries and header fidelity are strict build
|
||||
prerequisites. This does not fetch sources, invoke Git, rewrite C, or
|
||||
compile anything. The generated component retains the libogc GPL plus its
|
||||
section 18 linking exception; see external/libogc_ir/NOTICE.txt.
|
||||
"""
|
||||
output = Path(output).resolve()
|
||||
if output.is_relative_to(COMPONENT):
|
||||
raise PreparationError(
|
||||
"output must be outside the vendored libogc IR component"
|
||||
)
|
||||
|
||||
manifest = json.loads((COMPONENT / "UPSTREAM.json").read_bytes())
|
||||
if manifest["format_version"] != 1 or manifest["commit"] != UPSTREAM_COMMIT:
|
||||
raise PreparationError("unsupported libogc IR manifest version or source pin")
|
||||
if manifest["ir_git_blob_sha1"] != IR_GIT_BLOB:
|
||||
raise PreparationError("unexpected pinned ir.c Git blob")
|
||||
|
||||
sources = {}
|
||||
for name, record in manifest["sources"].items():
|
||||
data = (COMPONENT / record["local_path"]).read_bytes()
|
||||
_check_sha256(data, record["sha256"], name)
|
||||
blob = hashlib.sha1(b"blob " + str(len(data)).encode("ascii") + b"\0" + data)
|
||||
if blob.hexdigest() != record["git_blob_sha1"]:
|
||||
raise PreparationError(f"{name}: Git blob checksum mismatch")
|
||||
if len(data) != record["bytes"] or len(data.splitlines()) != record["lines"]:
|
||||
raise PreparationError(f"{name}: source size or line count mismatch")
|
||||
sources[name] = data
|
||||
ir_source = sources["wiiuse/ir.c"]
|
||||
ir_blob = hashlib.sha1(
|
||||
b"blob " + str(len(ir_source)).encode("ascii") + b"\0" + ir_source
|
||||
).hexdigest()
|
||||
if ir_blob != IR_GIT_BLOB:
|
||||
raise PreparationError("ir.c differs from the pinned upstream Git blob")
|
||||
|
||||
header = manifest["portable_header"]
|
||||
_check_sha256((COMPONENT / header["path"]).read_bytes(), header["sha256"], "ir.h")
|
||||
for section in manifest["extraction"]["header_sections"]:
|
||||
_section(sources[section["source"]], section)
|
||||
|
||||
chunks = [
|
||||
b'#include "libogc_ir/ir.h"\n#include <math.h>\n\n',
|
||||
(
|
||||
"/*\n"
|
||||
" * Generated libogc Wiiuse IR math extraction; modified 2026-09-11.\n"
|
||||
f" * Upstream commit: {UPSTREAM_COMMIT}\n"
|
||||
" * Function bodies and algorithm constants below are unchanged.\n"
|
||||
" * Only platform isolation, declarations and provenance are adapted.\n"
|
||||
" * GNU GPL with the libogc section 18 linking exception, explicitly\n"
|
||||
" * retained and extended to this modified component. See\n"
|
||||
" * external/libogc_ir/license_libogc.txt, libogc_license.txt,\n"
|
||||
" * NOTICE.txt and UPSTREAM.json. The original Wiiuse notice follows.\n"
|
||||
" */\n"
|
||||
).encode(),
|
||||
]
|
||||
for section in manifest["extraction"]["c_sections"]:
|
||||
source = section["source"]
|
||||
local_path = manifest["sources"][source]["local_path"]
|
||||
chunks.append(
|
||||
f'\n#line {section["first_line"]} "external/libogc_ir/{local_path}"\n'.encode()
|
||||
)
|
||||
chunks.append(_section(sources[source], section))
|
||||
if source == "wiiuse/definitions.h" and section["first_line"] == 31:
|
||||
# Upstream uses the GNU empty-tail form DEBUG(fmt, ...). A purely
|
||||
# variadic disabled macro has identical expansion and is valid C11.
|
||||
chunks.append(
|
||||
b"\n/* C11 portability only: logging stays disabled. */\n"
|
||||
b"#undef WIIUSE_DEBUG\n#define WIIUSE_DEBUG(...)\n"
|
||||
)
|
||||
generated = b"".join(chunks)
|
||||
|
||||
if not output.exists() or output.read_bytes() != generated:
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
output.write_bytes(generated)
|
||||
return output
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
prepare(args.output)
|
||||
except (PreparationError, OSError, ValueError, KeyError, TypeError) as exc:
|
||||
print(f"error: {exc}", file=sys.stderr)
|
||||
return 1
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
432
tools/switch2_native_imu.py
Executable file
432
tools/switch2_native_imu.py
Executable file
|
|
@ -0,0 +1,432 @@
|
|||
#!/usr/bin/env python3
|
||||
"""Offline candidate codec for native Joy-Con 2 (R) IMU blocks.
|
||||
|
||||
No device access or firmware writes. Accepts exported native_hex packet JSON,
|
||||
unfragmented decrypted BLE PCAPng (ATT handle 0x000e), or --block HEX.
|
||||
|
||||
Recovered from native captures, not copied from original-Switch report 0x30.
|
||||
The quaternion is ratio-coded: select the largest component, make it positive,
|
||||
and store the next three components in cyclic order divided by that component.
|
||||
Labelled right-Joy-Con captures support wire order (w, x, y, z), with +X toward
|
||||
the outer edge away from the rail, +Y toward R/ZR, and +Z out of the button face.
|
||||
Positive gyro rotation follows the right-hand rule about those body axes.
|
||||
Acceleration units are a g-scale candidate supported by gravity magnitudes.
|
||||
For observed packed tag 3, labelled turns support gyro degrees/second =
|
||||
raw * 500 / 2**(width - 1), correcting the initial 512-degree-range hypothesis.
|
||||
Subsample timing and cross-device tick units remain unconfirmed. The trailing
|
||||
temperature-like word is raw, not degrees; format 0x0f has no such word.
|
||||
|
||||
Reference for native report boundaries:
|
||||
https://github.com/ndeadly/switch2_controller_research/blob/master/hid_reports.md
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
import sys
|
||||
from collections import Counter
|
||||
from collections.abc import Iterable, Iterator
|
||||
from pathlib import Path
|
||||
from typing import TypedDict
|
||||
|
||||
|
||||
class AccelerationVector(TypedDict):
|
||||
bit_offset: int
|
||||
width: int
|
||||
fraction_bits_candidate: int
|
||||
raw: list[int]
|
||||
g_candidate: list[float]
|
||||
|
||||
|
||||
class RotationTriplet(TypedDict):
|
||||
bit_offset: int
|
||||
width: int
|
||||
dps_per_count_candidate: float
|
||||
raw: list[int]
|
||||
dps_candidate: list[float]
|
||||
interpretation: str
|
||||
|
||||
|
||||
class DecodedBlock(TypedDict):
|
||||
format: int
|
||||
length: int
|
||||
counter_ticks: int
|
||||
elapsed_ticks: int
|
||||
range_tag: int | None
|
||||
largest_component: int
|
||||
quaternion_width: int
|
||||
quaternion_ratios: list[float]
|
||||
quaternion_wire: list[float]
|
||||
accelerations: list[AccelerationVector]
|
||||
rotation_triplets: list[RotationTriplet]
|
||||
temperature_raw_candidate: int | None
|
||||
|
||||
|
||||
class InputRecord(TypedDict, total=False):
|
||||
native_hex: str
|
||||
block: str
|
||||
t_us: int | None
|
||||
stream: str
|
||||
|
||||
|
||||
# Quaternion width; alternating acceleration / rotation triplet widths.
|
||||
# All offsets are LSB-first within the IMU block, not the containing HID report.
|
||||
LAYOUTS = {
|
||||
0: (31, (32,)),
|
||||
1: (23, (22, 22, 22)),
|
||||
2: (21, (14, 13, 13, 14, 14)),
|
||||
3: (21, (14, 16, 13, 16, 14)),
|
||||
}
|
||||
|
||||
|
||||
def bits(word: int, start: int, width: int) -> int:
|
||||
return (word >> start) & ((1 << width) - 1)
|
||||
|
||||
|
||||
def signed(word: int, start: int, width: int) -> int:
|
||||
value = bits(word, start, width)
|
||||
return value - (1 << width) if value & (1 << (width - 1)) else value
|
||||
|
||||
|
||||
def quaternion_from_ratios(index: int, ratios: list[float]) -> list[float]:
|
||||
largest = 1.0 / math.sqrt(1.0 + sum(value * value for value in ratios))
|
||||
result = [0.0] * 4
|
||||
result[index] = largest
|
||||
for i, ratio in enumerate(ratios):
|
||||
result[(index + i + 1) & 3] = ratio * largest
|
||||
return result
|
||||
|
||||
|
||||
def decode_block(block: bytes) -> DecodedBlock:
|
||||
if len(block) not in (30, 40):
|
||||
raise ValueError(f"expected 30 or 40 IMU bytes, received {len(block)}")
|
||||
if block[3] not in (0x0C, 0x0D, 0x0E, 0x0F):
|
||||
raise ValueError(f"unsupported internal format byte 0x{block[3]:02x}")
|
||||
variant = block[3] & 3
|
||||
if len(block) != (30 if variant == 0 else 40):
|
||||
raise ValueError("internal format does not match declared IMU length")
|
||||
word = int.from_bytes(block, "little")
|
||||
quaternion_width, widths = LAYOUTS[variant]
|
||||
range_tag = None if variant == 0 else bits(word, 32, 2)
|
||||
if range_tag not in (None, 3):
|
||||
raise ValueError(f"unobserved packed range tag {range_tag}; scale unresolved")
|
||||
index_start = 32 if variant == 0 else 34
|
||||
index_tag = bits(word, index_start, 3)
|
||||
if index_tag > 3:
|
||||
raise ValueError(f"unobserved quaternion index tag {index_tag}")
|
||||
position = index_start + 3
|
||||
ratio_codes = [bits(word, position + i * quaternion_width, quaternion_width)
|
||||
for i in range(3)]
|
||||
ratios = [value / (1 << (quaternion_width - 1)) - 1.0 for value in ratio_codes]
|
||||
position += 3 * quaternion_width
|
||||
accelerations: list[AccelerationVector] = []
|
||||
rotation_triplets: list[RotationTriplet] = []
|
||||
for vector_index, width in enumerate(widths):
|
||||
values = [signed(word, position + i * width, width) for i in range(3)]
|
||||
if vector_index % 2 == 0:
|
||||
fraction_bits = 28 if variant == 0 else width - 2
|
||||
accelerations.append({
|
||||
"bit_offset": position,
|
||||
"width": width,
|
||||
"fraction_bits_candidate": fraction_bits,
|
||||
"raw": values,
|
||||
"g_candidate": [value / (1 << fraction_bits) for value in values],
|
||||
})
|
||||
else:
|
||||
dps_per_count = 500.0 / (1 << (width - 1))
|
||||
rotation_triplets.append({
|
||||
"bit_offset": position,
|
||||
"width": width,
|
||||
"dps_per_count_candidate": dps_per_count,
|
||||
"raw": values,
|
||||
"dps_candidate": [value * dps_per_count for value in values],
|
||||
"interpretation": "body-local angular velocity candidate",
|
||||
})
|
||||
position += 3 * width
|
||||
if variant == 3:
|
||||
assert position == 319
|
||||
if bits(word, 319, 1):
|
||||
raise ValueError("unobserved nonzero format-0x0f terminal bit")
|
||||
temperature = None
|
||||
else:
|
||||
assert position == len(block) * 8 - 16
|
||||
temperature = signed(word, position, 16)
|
||||
return {
|
||||
"format": block[3],
|
||||
"length": len(block),
|
||||
"counter_ticks": bits(word, 0, 12),
|
||||
"elapsed_ticks": bits(word, 12, 12),
|
||||
"range_tag": range_tag,
|
||||
"largest_component": index_tag,
|
||||
"quaternion_width": quaternion_width,
|
||||
"quaternion_ratios": ratios,
|
||||
"quaternion_wire": quaternion_from_ratios(index_tag, ratios),
|
||||
"accelerations": accelerations,
|
||||
"rotation_triplets": rotation_triplets,
|
||||
"temperature_raw_candidate": temperature,
|
||||
}
|
||||
|
||||
|
||||
def put_bits(word: int, start: int, width: int, value: int, *, is_signed: bool = False) -> int:
|
||||
minimum = -(1 << (width - 1)) if is_signed else 0
|
||||
maximum = (1 << (width - int(is_signed))) - 1
|
||||
if not minimum <= value <= maximum:
|
||||
raise ValueError(f"value {value} does not fit {'signed ' if is_signed else ''}{width} bits")
|
||||
return word | ((value & ((1 << width) - 1)) << start)
|
||||
|
||||
|
||||
def pack_quaternion(word: int, start: int, width: int,
|
||||
quaternion: list[float], index: int) -> int:
|
||||
if len(quaternion) != 4 or not all(math.isfinite(v) for v in quaternion):
|
||||
raise ValueError("quaternion must contain four finite components")
|
||||
if index not in range(4) or quaternion[index] == 0:
|
||||
raise ValueError("selected quaternion component must be nonzero")
|
||||
word = put_bits(word, start, 3, index)
|
||||
start += 3
|
||||
for i in range(3):
|
||||
ratio = quaternion[(index + i + 1) & 3] / quaternion[index]
|
||||
if abs(ratio) > 1.000000001:
|
||||
raise ValueError("selected quaternion component is not largest")
|
||||
# +1 is the top quantization boundary; do not wrap it into -1.
|
||||
code = min((1 << width) - 1,
|
||||
max(0, round((ratio + 1.0) * (1 << (width - 1)))))
|
||||
word = put_bits(word, start + i * width, width, code)
|
||||
return word
|
||||
|
||||
|
||||
def encode_mode0(counter_ticks: int, elapsed_ticks: int,
|
||||
quaternion_wire: list[float], acceleration_g: list[float],
|
||||
temperature_raw: int, *, largest_component: int | None = None) -> bytes:
|
||||
"""Encode the recovered one-sample layout; not yet a console-qualified encoder.
|
||||
|
||||
Physical axis mapping and temperature conversion are caller responsibilities.
|
||||
This deliberately takes real orientation/acceleration, not canned motion data.
|
||||
"""
|
||||
if len(quaternion_wire) != 4:
|
||||
raise ValueError("quaternion must have four components")
|
||||
if largest_component is None:
|
||||
largest_component = max(range(4), key=lambda i: abs(quaternion_wire[i]))
|
||||
if len(acceleration_g) != 3 or not all(math.isfinite(v) for v in acceleration_g):
|
||||
raise ValueError("acceleration must contain three finite components")
|
||||
word = put_bits(0, 0, 12, counter_ticks)
|
||||
word = put_bits(word, 12, 12, elapsed_ticks)
|
||||
word = put_bits(word, 24, 8, 0x0C)
|
||||
word = pack_quaternion(word, 32, 31, quaternion_wire, largest_component)
|
||||
for i, value in enumerate(acceleration_g):
|
||||
word = put_bits(word, 128 + 32 * i, 32, round(value * (1 << 28)), is_signed=True)
|
||||
word = put_bits(word, 224, 16, temperature_raw, is_signed=True)
|
||||
return word.to_bytes(30, "little")
|
||||
|
||||
|
||||
def repack_decoded(decoded: DecodedBlock) -> bytes:
|
||||
"""Rebuild fields independently; do not copy original bytes or ratio codes.
|
||||
|
||||
Rebuild gyro fields from candidate degree-per-second values as well.
|
||||
"""
|
||||
variant = decoded["format"] & 3
|
||||
quaternion_width, widths = LAYOUTS[variant]
|
||||
word = put_bits(0, 0, 12, decoded["counter_ticks"])
|
||||
word = put_bits(word, 12, 12, decoded["elapsed_ticks"])
|
||||
word = put_bits(word, 24, 8, decoded["format"])
|
||||
start = 32
|
||||
if variant:
|
||||
range_tag = decoded["range_tag"]
|
||||
if range_tag is None:
|
||||
raise ValueError("packed frame requires a range tag")
|
||||
word = put_bits(word, start, 2, range_tag)
|
||||
start += 2
|
||||
word = pack_quaternion(word, start, quaternion_width,
|
||||
decoded["quaternion_wire"], decoded["largest_component"])
|
||||
position = start + 3 + 3 * quaternion_width
|
||||
accel_index = rotation_index = 0
|
||||
for vector_index, width in enumerate(widths):
|
||||
if vector_index % 2 == 0:
|
||||
fraction_bits = 28 if variant == 0 else width - 2
|
||||
values = [round(v * (1 << fraction_bits))
|
||||
for v in decoded["accelerations"][accel_index]["g_candidate"]]
|
||||
accel_index += 1
|
||||
else:
|
||||
counts_per_dps = (1 << (width - 1)) / 500.0
|
||||
values = [round(v * counts_per_dps)
|
||||
for v in decoded["rotation_triplets"][rotation_index]["dps_candidate"]]
|
||||
rotation_index += 1
|
||||
for i, value in enumerate(values):
|
||||
word = put_bits(word, position + i * width, width, value, is_signed=True)
|
||||
position += 3 * width
|
||||
if variant != 3:
|
||||
temperature = decoded["temperature_raw_candidate"]
|
||||
if temperature is None:
|
||||
raise ValueError("this format requires the trailing temperature-like word")
|
||||
word = put_bits(word, position, 16, temperature, is_signed=True)
|
||||
return word.to_bytes(decoded["length"], "little")
|
||||
|
||||
|
||||
def pcap_native_records(path: Path) -> Iterator[InputRecord]:
|
||||
"""Read complete decrypted native-right notifications, not BLE reassembly."""
|
||||
data = path.read_bytes()
|
||||
position = 0
|
||||
endian = "<"
|
||||
interfaces: list[tuple[int, int, int]] = []
|
||||
while position + 12 <= len(data):
|
||||
if data[position:position + 4] == b"\x0a\x0d\x0d\x0a":
|
||||
magic = data[position + 8:position + 12]
|
||||
if magic not in (b"\x4d\x3c\x2b\x1a", b"\x1a\x2b\x3c\x4d"):
|
||||
raise ValueError("invalid PCAPng byte-order magic")
|
||||
endian = "<" if magic[0] == 0x4D else ">"
|
||||
interfaces = []
|
||||
kind, size = struct.unpack_from(endian + "II", data, position)
|
||||
if (size < 12 or size % 4 or position + size > len(data) or
|
||||
struct.unpack_from(endian + "I", data, position + size - 4)[0] != size):
|
||||
raise ValueError(f"invalid PCAPng block at {position}")
|
||||
if kind == 1:
|
||||
if size < 20:
|
||||
raise ValueError("truncated interface description")
|
||||
link_type = struct.unpack_from(endian + "H", data, position + 8)[0]
|
||||
resolution, offset_seconds = 1_000_000, 0
|
||||
option = position + 16
|
||||
while option + 4 <= position + size - 4:
|
||||
code, length = struct.unpack_from(endian + "HH", data, option)
|
||||
option += 4
|
||||
if option + length > position + size - 4:
|
||||
raise ValueError("truncated interface option")
|
||||
value = data[option:option + length]
|
||||
if code == 9 and length == 1:
|
||||
resolution = (2 if value[0] & 128 else 10) ** (value[0] & 127)
|
||||
elif code == 14 and length == 8:
|
||||
offset_seconds = struct.unpack(endian + "q", value)[0]
|
||||
option += (length + 3) & ~3
|
||||
if code == 0:
|
||||
break
|
||||
interfaces.append((link_type, resolution, offset_seconds))
|
||||
elif kind == 6:
|
||||
if size < 32:
|
||||
raise ValueError("truncated enhanced packet block")
|
||||
interface, high, low, captured, _ = struct.unpack_from(endian + "IIIII", data, position + 8)
|
||||
if interface >= len(interfaces) or captured > size - 32:
|
||||
raise ValueError("invalid enhanced packet metadata")
|
||||
link_type, resolution, offset_seconds = interfaces[interface]
|
||||
packet = data[position + 28:position + 28 + captured]
|
||||
ll_start = {256: 10, 251: 0}.get(link_type)
|
||||
if ll_start is not None and len(packet) >= ll_start + 6:
|
||||
ll_payload = ll_start + 6
|
||||
# LLID=2: complete first L2CAP fragment. This reader does not
|
||||
# reconstruct fragmented or encrypted notifications.
|
||||
if (packet[ll_start + 4] & 3 == 2 and packet[ll_start + 5] == 70 and
|
||||
packet[ll_payload:ll_payload + 7] == b"\x42\x00\x04\x00\x1b\x0e\x00" and
|
||||
len(packet) >= ll_payload + 70):
|
||||
timestamp = (((high << 32) | low) * 1_000_000 // resolution +
|
||||
offset_seconds * 1_000_000)
|
||||
yield {"t_us": timestamp,
|
||||
"stream": f"{interface}:{packet[ll_start:ll_start + 4].hex()}",
|
||||
"native_hex": packet[ll_payload + 7:ll_payload + 70].hex()}
|
||||
position += size
|
||||
if position != len(data):
|
||||
raise ValueError("trailing incomplete PCAPng block")
|
||||
|
||||
|
||||
def records(path: Path) -> Iterator[InputRecord]:
|
||||
if path.suffix.lower() == ".pcapng":
|
||||
yield from pcap_native_records(path)
|
||||
else:
|
||||
data = json.loads(path.read_text())
|
||||
yield from data["packets"] if isinstance(data, dict) else data
|
||||
|
||||
|
||||
def native_block(record: InputRecord) -> bytes:
|
||||
native_hex = record.get("native_hex")
|
||||
if native_hex is None:
|
||||
raise ValueError("input record requires native_hex")
|
||||
packet = bytes.fromhex(native_hex)
|
||||
if len(packet) == 64 and packet[0] == 8:
|
||||
packet = packet[1:]
|
||||
if len(packet) != 63:
|
||||
raise ValueError("expected 63 native08 payload bytes, optionally prefixed by report ID")
|
||||
length = packet[15]
|
||||
if length not in (0, 30, 40):
|
||||
raise ValueError(f"unrecognized native08 IMU length {length}")
|
||||
return packet[16:16 + length]
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("inputs", nargs="*", type=Path)
|
||||
parser.add_argument("--block", help="decode one 30/40-byte IMU block in hexadecimal")
|
||||
parser.add_argument("--summary", action="store_true", help="aggregate instead of JSON-lines output")
|
||||
parser.add_argument("--verify-roundtrip", action="store_true", help="re-encode all fields and compare bytes")
|
||||
args = parser.parse_args()
|
||||
if bool(args.block) == bool(args.inputs):
|
||||
parser.error("provide input files or --block, but not both")
|
||||
counts: Counter[str] = Counter()
|
||||
norms: list[float] = []
|
||||
decoded_count = 0
|
||||
empty_count = 0
|
||||
mismatch_count = 0
|
||||
timing_gaps = 0
|
||||
previous: dict[tuple[str, str], int] = {}
|
||||
streams: list[tuple[str, Iterable[InputRecord]]] = [("hex", [{"block": args.block}])] if args.block else [
|
||||
(str(path), records(path)) for path in args.inputs]
|
||||
try:
|
||||
for source, source_records in streams:
|
||||
for record in source_records:
|
||||
block = bytes.fromhex(record["block"]) if "block" in record else native_block(record)
|
||||
if not block:
|
||||
empty_count += 1
|
||||
continue
|
||||
decoded = decode_block(block)
|
||||
decoded_count += 1
|
||||
counts[f"0x{decoded['format']:02x}"] += 1
|
||||
for acceleration in decoded["accelerations"]:
|
||||
norms.append(math.sqrt(sum(v * v for v in acceleration["g_candidate"])))
|
||||
key = (source, record.get("stream", "default"))
|
||||
if key in previous:
|
||||
delta = (decoded["counter_ticks"] - previous[key]) & 0xFFF
|
||||
if delta != decoded["elapsed_ticks"]:
|
||||
timing_gaps += 1
|
||||
previous[key] = decoded["counter_ticks"]
|
||||
if args.verify_roundtrip:
|
||||
rebuilt = repack_decoded(decoded)
|
||||
if decoded["format"] == 0x0C:
|
||||
temperature = decoded["temperature_raw_candidate"]
|
||||
if temperature is None:
|
||||
raise ValueError("mode0 requires the trailing temperature-like word")
|
||||
rebuilt_mode0 = encode_mode0(
|
||||
decoded["counter_ticks"], decoded["elapsed_ticks"],
|
||||
decoded["quaternion_wire"], decoded["accelerations"][0]["g_candidate"],
|
||||
temperature,
|
||||
largest_component=decoded["largest_component"])
|
||||
if rebuilt_mode0 != block:
|
||||
raise ValueError("mode0 semantic encoder round trip failed")
|
||||
mismatch_count += rebuilt != block
|
||||
if not args.summary:
|
||||
print(json.dumps({"source": source, "t_us": record.get("t_us"), **decoded}))
|
||||
if decoded_count == 0:
|
||||
raise ValueError("no supported native IMU blocks found")
|
||||
if args.summary:
|
||||
norms.sort()
|
||||
print(json.dumps({
|
||||
"decoded_blocks": decoded_count,
|
||||
"empty_blocks": empty_count,
|
||||
"formats": dict(sorted(counts.items())),
|
||||
"acceleration_vectors": len(norms),
|
||||
"acceleration_norm_g_candidate": {
|
||||
name: norms[round((len(norms) - 1) * fraction)]
|
||||
for name, fraction in (("min", 0), ("p10", .1), ("median", .5), ("p90", .9), ("max", 1))
|
||||
},
|
||||
"counter_interval_mismatches": timing_gaps,
|
||||
"roundtrip_mismatches": mismatch_count if args.verify_roundtrip else None,
|
||||
"limitations": ["axis and gyro-scale qualification limited to labelled right-Joy-Con captures",
|
||||
"subsample timing and cross-device tick units unconfirmed",
|
||||
"temperature conversion unconfirmed",
|
||||
"encoder not yet console-qualified"],
|
||||
}, indent=2))
|
||||
except (ValueError, KeyError, TypeError, OSError, struct.error) as error:
|
||||
print(f"decode error: {error}", file=sys.stderr)
|
||||
return 1
|
||||
return int(mismatch_count != 0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
35
tools/switch2_usb_probe/bootsel.cpp
Normal file
35
tools/switch2_usb_probe/bootsel.cpp
Normal file
|
|
@ -0,0 +1,35 @@
|
|||
#include "bootsel.h"
|
||||
|
||||
#include "adapter/adapter_mode_controller.h"
|
||||
#include "usb/usb_configuration_management.h"
|
||||
|
||||
namespace {
|
||||
bool bootsel_accepted;
|
||||
}
|
||||
|
||||
bool probe_bootsel_vendor_control(uint8_t rhport, uint8_t stage,
|
||||
const tusb_control_request_t* request) {
|
||||
using namespace UsbConfigurationManagement;
|
||||
if (request == nullptr || request->bmRequestType != 0x40 ||
|
||||
request->bRequest != static_cast<uint8_t>(Operation::kBootselReboot) ||
|
||||
request->wValue != kRequestValue || request->wIndex != kRequestIndex ||
|
||||
request->wLength != kRequestHeaderSize) {
|
||||
return false;
|
||||
}
|
||||
// The shared handler receives the envelope at SETUP and validates and
|
||||
// dispatches it only at ACK, after the host's control transfer completes.
|
||||
const bool accepted =
|
||||
usb_configuration_management_vendor_control(rhport, stage, request);
|
||||
if (accepted && stage == CONTROL_STAGE_ACK) {
|
||||
bootsel_accepted = true;
|
||||
}
|
||||
return accepted;
|
||||
}
|
||||
|
||||
void probe_bootsel_task(uint32_t now_ms) {
|
||||
// The native bridge does not initialize ordinary adapter-mode selection.
|
||||
// A successful BOOTSEL dispatch guarantees the task takes its reboot path.
|
||||
if (bootsel_accepted) {
|
||||
adapter_mode_controller_task(now_ms);
|
||||
}
|
||||
}
|
||||
20
tools/switch2_usb_probe/bootsel.h
Normal file
20
tools/switch2_usb_probe/bootsel.h
Normal file
|
|
@ -0,0 +1,20 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "tusb.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// Core 0: expose only the existing private BOOTSEL command, not configuration.
|
||||
bool probe_bootsel_vendor_control(uint8_t rhport, uint8_t stage,
|
||||
const tusb_control_request_t* request);
|
||||
// Core 0: service the existing reboot delay only after a validated status ACK.
|
||||
void probe_bootsel_task(uint32_t now_ms);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -8,6 +8,13 @@
|
|||
#include "platform/pico/system_clock.h"
|
||||
#include "profile/controller_profile_runtime.h"
|
||||
#include "pico/stdlib.h"
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
#include <math.h>
|
||||
#include "input/wii_ir_pointer.h"
|
||||
#include "native_imu.h"
|
||||
#include "pico/time.h"
|
||||
extern "C" int probe_debug_printf(const char* format, ...);
|
||||
#endif
|
||||
|
||||
#if !SWITCH_PICO_SWITCH2_USB_BRIDGE || !SWITCH_PICO_BLUEPAD32 || \
|
||||
!SWITCH_PICO_ENABLE_BLE || !SWITCH_PICO_SWITCH2_MOUSE_CAPTURE || \
|
||||
|
|
@ -27,7 +34,323 @@ bool g_initialized;
|
|||
bool g_start_attempted;
|
||||
bool g_flash_ready;
|
||||
probe_controller_input g_input;
|
||||
#if !SWITCH2_BRIDGE_WII_INPUT
|
||||
uint32_t g_received_ms;
|
||||
#endif
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
#ifndef SWITCH2_WII_IR_SCREEN_CONFIG
|
||||
#define SWITCH2_WII_IR_SCREEN_CONFIG 660, 370, 0, -115, 1920, 1080
|
||||
#endif
|
||||
constexpr float kIrScreenConfig[] = {SWITCH2_WII_IR_SCREEN_CONFIG};
|
||||
static_assert(sizeof(kIrScreenConfig) / sizeof(kIrScreenConfig[0]) == 6);
|
||||
bool g_screen_configured;
|
||||
constexpr uint32_t kSensorDeadlineUs = 150000;
|
||||
constexpr uint32_t kOutputDeadlineUs = 100000;
|
||||
Bluepad32WiiBridgeSnapshot g_wii;
|
||||
ProbeNativeMotion g_motion;
|
||||
bool g_wii_active;
|
||||
bool g_native_stream;
|
||||
uint8_t g_native_features;
|
||||
uint32_t g_wii_generation;
|
||||
uint32_t g_orientation_requested_generation;
|
||||
int g_sensor_status = -1;
|
||||
uint16_t g_stick_center[2]{2048, 2048};
|
||||
uint16_t g_stick_positive[2]{2047, 2047};
|
||||
uint16_t g_stick_negative[2]{2048, 2048};
|
||||
uint8_t g_power_info = 0x24; // Nominal battery until source status; no USB-power flag.
|
||||
uint8_t g_report_counter;
|
||||
uint32_t g_report_serial;
|
||||
uint32_t g_pending_serial;
|
||||
uint32_t g_pending_us;
|
||||
uint32_t g_pending_generation;
|
||||
uint32_t g_pending_ticks;
|
||||
uint8_t g_pending_report[63];
|
||||
WiiIrMouseReport g_pending_pointer{};
|
||||
bool g_pending_motion_ready;
|
||||
bool g_clock_started;
|
||||
uint32_t g_clock_us;
|
||||
uint32_t g_clock_ticks;
|
||||
uint32_t g_clock_fraction;
|
||||
bool g_have_committed_ticks;
|
||||
uint32_t g_committed_ticks;
|
||||
bool g_have_submission;
|
||||
uint32_t g_submitted_us;
|
||||
uint32_t g_output_open_us;
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
uint32_t g_last_ir_trace_us;
|
||||
#endif
|
||||
|
||||
void unpack_stick_pair(const uint8_t* data, uint16_t values[2]) {
|
||||
values[0] = data[0] | (static_cast<uint16_t>(data[1] & 15) << 8);
|
||||
values[1] = (data[1] >> 4) | (static_cast<uint16_t>(data[2]) << 4);
|
||||
}
|
||||
|
||||
uint16_t calibrated_stick_axis(int16_t value, unsigned axis, bool invert) {
|
||||
const int32_t input = value;
|
||||
const bool input_positive = input >= 0;
|
||||
const bool output_positive = input_positive != invert;
|
||||
const int32_t magnitude = input_positive ? input : -input;
|
||||
const int32_t denominator = input_positive ? INT16_MAX : 32768;
|
||||
const int32_t travel = output_positive ? g_stick_positive[axis] : g_stick_negative[axis];
|
||||
const int32_t displacement = (magnitude * travel + denominator / 2) / denominator;
|
||||
return static_cast<uint16_t>(g_stick_center[axis] +
|
||||
(output_positive ? displacement : -displacement));
|
||||
}
|
||||
|
||||
void pack_wii_controls(const ControllerProfileTransformResult& mapped) {
|
||||
const ControllerState& state = mapped.state;
|
||||
g_input.buttons[0] = static_cast<uint8_t>(
|
||||
(state.button_south ? 0x01 : 0) | (state.button_east ? 0x02 : 0) |
|
||||
(state.button_west ? 0x04 : 0) | (state.button_north ? 0x08 : 0) |
|
||||
(state.button_right_shoulder ? 0x10 : 0) |
|
||||
(state.right_trigger != 0 &&
|
||||
state.right_trigger >= mapped.right_trigger_digital_threshold ? 0x20 : 0) |
|
||||
(state.button_start ? 0x40 : 0) | (state.button_right_stick ? 0x80 : 0));
|
||||
g_input.buttons[1] = static_cast<uint8_t>(
|
||||
(state.button_system ? 0x01 : 0) | ((state.extra_buttons & 1) ? 0x10 : 0) |
|
||||
((state.extra_buttons & (1u << 5)) ? 0x80 : 0) |
|
||||
((state.extra_buttons & (1u << 6)) ? 0x40 : 0));
|
||||
// One virtual right stick: honor a mapped right stick first, otherwise the
|
||||
// Nunchuk stick. Left-only buttons are not repurposed as mouse clicks.
|
||||
int16_t x = state.right_stick_x;
|
||||
int16_t y = state.right_stick_y;
|
||||
if (x == 0 && y == 0 && g_wii.layout == Bluepad32ControllerLayout::kWiiNunchuk) {
|
||||
x = state.left_stick_x;
|
||||
y = state.left_stick_y;
|
||||
}
|
||||
const uint16_t sx = calibrated_stick_axis(x, 0, false);
|
||||
const uint16_t sy = calibrated_stick_axis(y, 1, true);
|
||||
g_input.stick[0] = static_cast<uint8_t>(sx);
|
||||
g_input.stick[1] = static_cast<uint8_t>((sx >> 8) | (sy << 4));
|
||||
g_input.stick[2] = static_cast<uint8_t>(sy >> 4);
|
||||
}
|
||||
|
||||
void advance_wii_clock(uint32_t now_us) {
|
||||
if (!g_clock_started) {
|
||||
g_clock_started = true;
|
||||
g_clock_us = now_us;
|
||||
return;
|
||||
}
|
||||
const uint64_t scaled = static_cast<uint64_t>(now_us - g_clock_us) * 960u +
|
||||
g_clock_fraction;
|
||||
g_clock_us = now_us;
|
||||
g_clock_ticks += static_cast<uint32_t>(scaled / 1000000u);
|
||||
g_clock_fraction = static_cast<uint32_t>(scaled % 1000000u);
|
||||
}
|
||||
|
||||
bool wii_sensors_fresh(uint32_t now_us) {
|
||||
return g_wii.accel_valid && g_wii.gyro_valid &&
|
||||
static_cast<int32_t>(now_us - g_wii.accel_received_us) <
|
||||
static_cast<int32_t>(kSensorDeadlineUs) &&
|
||||
static_cast<int32_t>(now_us - g_wii.gyro_received_us) <
|
||||
static_cast<int32_t>(kSensorDeadlineUs);
|
||||
}
|
||||
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
void trace_wii_ir(uint32_t now_us, bool output_enabled) {
|
||||
// Ten snapshots/second keep camera diagnostics well below UART capacity.
|
||||
if (now_us - g_last_ir_trace_us < 100000) return;
|
||||
g_last_ir_trace_us = now_us;
|
||||
uint8_t data[WII_IR_MOUSE_DIAGNOSTIC_SIZE];
|
||||
if (wii_ir_pointer_diagnostics(data, sizeof(data)) != sizeof(data)) return;
|
||||
static constexpr char hex[] = "0123456789abcdef";
|
||||
char encoded[sizeof(data) * 2 + 1];
|
||||
for (size_t i = 0; i < sizeof(data); ++i) {
|
||||
encoded[2 * i] = hex[data[i] >> 4];
|
||||
encoded[2 * i + 1] = hex[data[i] & 15];
|
||||
}
|
||||
encoded[sizeof(encoded) - 1] = 0;
|
||||
// State bits: calibrated IMU, native stream, effective IR output gate.
|
||||
const unsigned state = static_cast<unsigned>(g_motion.ready()) |
|
||||
(static_cast<unsigned>(g_native_stream) << 1) |
|
||||
(static_cast<unsigned>(output_enabled) << 2);
|
||||
probe_debug_printf("[PROBE %lu] WII_IR_DIAGNOSTIC state=%u len=%u: %s\n",
|
||||
static_cast<unsigned long>(to_ms_since_boot(get_absolute_time())), state,
|
||||
static_cast<unsigned>(sizeof(data)), encoded);
|
||||
}
|
||||
#endif
|
||||
|
||||
void update_wii_ir_gate(uint32_t now_us) {
|
||||
const uint32_t last_progress = g_have_submission ? g_submitted_us : g_output_open_us;
|
||||
const bool output_fresh = static_cast<int32_t>(now_us - last_progress) <
|
||||
static_cast<int32_t>(kOutputDeadlineUs);
|
||||
const bool enabled = g_native_stream && g_wii_active &&
|
||||
g_motion.ready() && wii_sensors_fresh(now_us) && output_fresh &&
|
||||
(g_native_features & 0x10);
|
||||
wii_ir_mouse_set_output_enabled(enabled);
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
trace_wii_ir(now_us, enabled);
|
||||
#endif
|
||||
}
|
||||
|
||||
void discard_wii_output() {
|
||||
g_pending_serial = 0;
|
||||
g_have_committed_ticks = false;
|
||||
g_have_submission = false;
|
||||
g_output_open_us = time_us_32();
|
||||
wii_ir_mouse_set_output_enabled(false);
|
||||
}
|
||||
|
||||
void lose_wii_source() {
|
||||
if (g_wii_active) {
|
||||
g_motion.reset();
|
||||
discard_wii_output();
|
||||
g_sensor_status = -1;
|
||||
}
|
||||
g_wii_active = false;
|
||||
g_input = {};
|
||||
}
|
||||
|
||||
void poll_wii_source(uint32_t now_ms) {
|
||||
bluepad32_input_backend_wii_snapshot(&g_wii);
|
||||
// Read the clock after the coherent snapshot so Core1 receipt timestamps
|
||||
// cannot appear to be in the future to the motion integrator.
|
||||
const uint32_t now_us = time_us_32();
|
||||
advance_wii_clock(now_us);
|
||||
if (!g_wii.controller.active || g_wii.slot >= BLUEPAD32_INPUT_BACKEND_SLOT_COUNT ||
|
||||
static_cast<int32_t>(now_us - g_wii.received_us) >=
|
||||
static_cast<int32_t>(kInputDeadlineMs * 1000u)) {
|
||||
lose_wii_source();
|
||||
g_orientation_requested_generation = 0;
|
||||
return;
|
||||
}
|
||||
if (g_wii.layout == Bluepad32ControllerLayout::kWiiHorizontal) {
|
||||
if (g_orientation_requested_generation != g_wii.controller.connection_generation &&
|
||||
bluepad32_input_backend_set_wii_orientation(g_wii.controller.identity,
|
||||
g_wii.controller.connection_generation, true)) {
|
||||
g_orientation_requested_generation = g_wii.controller.connection_generation;
|
||||
}
|
||||
lose_wii_source();
|
||||
return; // Never emit a transient sideways mapping while Core1 switches.
|
||||
}
|
||||
if (!g_wii_active || g_wii_generation != g_wii.controller.connection_generation) {
|
||||
discard_wii_output();
|
||||
g_motion.reset();
|
||||
g_wii_generation = g_wii.controller.connection_generation;
|
||||
g_sensor_status = -1;
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
g_last_ir_trace_us = now_us;
|
||||
#endif
|
||||
g_power_info = 0x24;
|
||||
probe_debug_printf("[PROBE] Wii source active in slot %u; keep still for native motion calibration\n",
|
||||
g_wii.slot);
|
||||
}
|
||||
g_wii_active = true;
|
||||
g_input.active = true;
|
||||
g_input.serial = g_wii.state_generation;
|
||||
g_input.native_status = static_cast<uint8_t>(0x30 | ((g_native_features & 0x20) ? 8 : 0));
|
||||
const ControllerProfileTransformResult mapped = controller_profile_runtime_transform(
|
||||
g_wii.slot, g_wii.controller, now_ms, AdapterUsbMode::kSwitch);
|
||||
pack_wii_controls(mapped);
|
||||
ControllerProfileRuntimeProfileChangeEvent feedback{};
|
||||
if (controller_profile_runtime_take_initial_profile_indication(g_wii.slot, &feedback) ||
|
||||
controller_profile_runtime_take_profile_change(g_wii.slot, &feedback)) {
|
||||
bluepad32_input_backend_queue_profile_feedback(g_wii.slot,
|
||||
feedback.connection_generation, feedback.active_profile_number, feedback.policy);
|
||||
}
|
||||
if (g_wii.battery != 0) {
|
||||
const unsigned level = (static_cast<unsigned>(g_wii.battery) * 9u + 127u) / 255u;
|
||||
g_power_info = static_cast<uint8_t>(level << 2);
|
||||
}
|
||||
ProbeNativeMotionSample motion{};
|
||||
motion.accel_valid = g_wii.accel_valid;
|
||||
motion.gyro_valid = g_wii.gyro_valid;
|
||||
motion.accel_sequence = g_wii.accel_sequence;
|
||||
motion.gyro_sequence = g_wii.gyro_sequence;
|
||||
motion.accel_us = g_wii.accel_received_us;
|
||||
motion.gyro_us = g_wii.gyro_received_us;
|
||||
// SDL -> physical right frame [X,-Z,Y], then a +90-degree mouse mounting
|
||||
// rotation about forward Y. A face-up Wii becomes rail-down native +X gravity.
|
||||
motion.accel_g[0] = static_cast<float>(g_wii.accel_q13[1]) / 8192.0f;
|
||||
motion.accel_g[1] = -static_cast<float>(g_wii.accel_q13[2]) / 8192.0f;
|
||||
motion.accel_g[2] = -static_cast<float>(g_wii.accel_q13[0]) / 8192.0f;
|
||||
motion.gyro_dps[0] = static_cast<float>(g_wii.gyro_q10[1]) / 1024.0f;
|
||||
motion.gyro_dps[1] = -static_cast<float>(g_wii.gyro_q10[2]) / 1024.0f;
|
||||
motion.gyro_dps[2] = -static_cast<float>(g_wii.gyro_q10[0]) / 1024.0f;
|
||||
g_motion.update(now_us, g_wii_generation, motion);
|
||||
WiiIrMouseReport optical{};
|
||||
(void)wii_ir_mouse_peek(&optical, 0);
|
||||
// Core 1 may publish during the peek. Read the clock after the snapshot.
|
||||
g_motion.observe_optical_heading(time_us_32(), optical.generation,
|
||||
optical.optical_sequence, optical.optical_received_us, optical.optical_yaw_radians,
|
||||
optical.tracking && optical.optical_valid && optical.owner == g_wii.slot &&
|
||||
optical.connection_generation == g_wii_generation);
|
||||
const int sensor_status = !wii_sensors_fresh(now_us) ? 0 : g_motion.ready() ? 2 : 1;
|
||||
if (sensor_status != g_sensor_status) {
|
||||
g_sensor_status = sensor_status;
|
||||
if (sensor_status == 2) {
|
||||
const float* bias = g_motion.bias();
|
||||
probe_debug_printf("[PROBE] Wii native IMU ready; bias_mdeg_s=%ld,%ld,%ld\n",
|
||||
lroundf(bias[0] * 1000), lroundf(bias[1] * 1000),
|
||||
lroundf(bias[2] * 1000));
|
||||
} else {
|
||||
probe_debug_printf("[PROBE] Wii native IMU %s\n", sensor_status ?
|
||||
"calibrating: keep still" : "waiting for fresh calibrated accelerometer/MotionPlus");
|
||||
}
|
||||
}
|
||||
update_wii_ir_gate(now_us);
|
||||
}
|
||||
|
||||
uint32_t prepare_wii_report(uint8_t report[63]) {
|
||||
if (!report || !g_native_stream || !g_wii_active) return 0;
|
||||
const uint32_t now_us = time_us_32();
|
||||
if (static_cast<int32_t>(now_us - g_wii.received_us) >=
|
||||
static_cast<int32_t>(kInputDeadlineMs * 1000u)) return 0;
|
||||
advance_wii_clock(now_us);
|
||||
update_wii_ir_gate(now_us);
|
||||
WiiIrMouseReport pointer{};
|
||||
(void)wii_ir_mouse_peek(&pointer, INT16_MAX);
|
||||
const bool motion_ready = g_motion.ready() && wii_sensors_fresh(now_us);
|
||||
if (g_pending_serial &&
|
||||
(g_pending_generation != g_wii_generation ||
|
||||
g_pending_pointer.generation != pointer.generation ||
|
||||
g_pending_motion_ready != motion_ready ||
|
||||
static_cast<int32_t>(now_us - g_pending_us) >= static_cast<int32_t>(kOutputDeadlineUs))) {
|
||||
g_pending_serial = 0;
|
||||
}
|
||||
if (g_pending_serial) {
|
||||
memcpy(report, g_pending_report, sizeof(g_pending_report));
|
||||
return g_pending_serial;
|
||||
}
|
||||
if (g_report_serial == UINT32_MAX) return 0; // Never reuse a submission token.
|
||||
memset(g_pending_report, 0, sizeof(g_pending_report));
|
||||
g_pending_report[0] = g_report_counter;
|
||||
g_pending_report[1] = g_power_info;
|
||||
memcpy(g_pending_report + 2, g_input.buttons, sizeof(g_input.buttons));
|
||||
g_pending_report[4] = 7;
|
||||
memcpy(g_pending_report + 5, g_input.stick, sizeof(g_input.stick));
|
||||
g_pending_report[8] = g_input.native_status;
|
||||
g_pending_report[13] = 0xff; // Observed no-surface value.
|
||||
g_pending_ticks = g_clock_ticks;
|
||||
const uint32_t elapsed = g_have_committed_ticks ? g_pending_ticks - g_committed_ticks : 1;
|
||||
const uint16_t wire_elapsed = static_cast<uint16_t>(elapsed <= 0xfff ? elapsed : 1);
|
||||
const bool have_motion = g_motion.ready() && wii_sensors_fresh(now_us) &&
|
||||
probe_native_imu_pack(g_motion.quaternion(), g_motion.acceleration(),
|
||||
static_cast<uint16_t>(g_pending_ticks & 0xfff), wire_elapsed, 0,
|
||||
g_pending_report + 16);
|
||||
if (have_motion) g_pending_report[15] = 30;
|
||||
if (have_motion && pointer.tracking && pointer.owner == g_wii.slot &&
|
||||
pointer.connection_generation == g_wii_generation) {
|
||||
const uint16_t dx = static_cast<uint16_t>(pointer.dx);
|
||||
// Native Joy-Con Y is opposite to the shared desktop-pointer convention.
|
||||
// Keep the original pointer delta for commit/consumption below.
|
||||
const uint16_t dy = static_cast<uint16_t>(-pointer.dy);
|
||||
g_pending_report[9] = static_cast<uint8_t>(dx);
|
||||
g_pending_report[10] = static_cast<uint8_t>(dx >> 8);
|
||||
g_pending_report[11] = static_cast<uint8_t>(dy);
|
||||
g_pending_report[12] = static_cast<uint8_t>(dy >> 8);
|
||||
g_pending_report[13] = 20; // Observed contact-range value for virtual IR tracking.
|
||||
}
|
||||
// IR buttons are deliberately NOT mapped to desktop/native click buttons.
|
||||
g_pending_pointer = pointer;
|
||||
g_pending_motion_ready = have_motion;
|
||||
g_pending_generation = g_wii_generation;
|
||||
g_pending_us = now_us;
|
||||
g_pending_serial = ++g_report_serial;
|
||||
memcpy(report, g_pending_report, sizeof(g_pending_report));
|
||||
return g_pending_serial;
|
||||
}
|
||||
#endif
|
||||
} // namespace
|
||||
|
||||
extern "C" void probe_controller_input_clock_init(void) {
|
||||
|
|
@ -36,11 +359,19 @@ extern "C" void probe_controller_input_clock_init(void) {
|
|||
|
||||
extern "C" void probe_controller_input_init(void) {
|
||||
if (g_initialized) return;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
bluepad32_input_backend_init();
|
||||
bluepad32_input_backend_select_wii_source(kSourceAddress);
|
||||
g_screen_configured = wii_ir_pointer_configure_screen(
|
||||
kIrScreenConfig[0], kIrScreenConfig[1], kIrScreenConfig[2],
|
||||
kIrScreenConfig[3], kIrScreenConfig[4], kIrScreenConfig[5]);
|
||||
if (!g_screen_configured) probe_debug_printf("[PROBE] Invalid native IR viewport configuration\n");
|
||||
wii_ir_mouse_set_output_enabled(false);
|
||||
#else
|
||||
switch2_mouse_capture_init();
|
||||
switch2_mouse_capture_select_input(kSourceAddress);
|
||||
// Prepare the existing storage services without initializing legacy USB.
|
||||
// Core 1 loads their persisted state during the normal backend startup.
|
||||
bluepad32_input_backend_init();
|
||||
#endif
|
||||
controller_profile_runtime_reset();
|
||||
g_initialized = true;
|
||||
}
|
||||
|
|
@ -48,6 +379,9 @@ extern "C" void probe_controller_input_init(void) {
|
|||
extern "C" bool probe_controller_input_start(void) {
|
||||
if (!g_initialized) probe_controller_input_init();
|
||||
if (g_start_attempted) return g_flash_ready;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
if (!g_screen_configured) return false;
|
||||
#endif
|
||||
g_start_attempted = true;
|
||||
bluepad32_input_backend_start();
|
||||
const absolute_time_t deadline = make_timeout_time_ms(kFlashCoordinationTimeoutMs);
|
||||
|
|
@ -75,19 +409,61 @@ extern "C" bool probe_controller_input_pairing_task(void) {
|
|||
return true;
|
||||
}
|
||||
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
extern "C" void probe_controller_input_set_stick_calibration(const uint8_t calibration[9]) {
|
||||
if (!calibration) return;
|
||||
unpack_stick_pair(calibration, g_stick_center);
|
||||
unpack_stick_pair(calibration + 3, g_stick_positive);
|
||||
unpack_stick_pair(calibration + 6, g_stick_negative);
|
||||
g_pending_serial = 0;
|
||||
}
|
||||
|
||||
extern "C" void probe_controller_input_set_native_features(uint8_t features) {
|
||||
if (g_native_features == features) return;
|
||||
g_native_features = features;
|
||||
g_pending_serial = 0;
|
||||
update_wii_ir_gate(time_us_32());
|
||||
}
|
||||
#endif
|
||||
|
||||
extern "C" void probe_controller_input_set_native_stream(bool enabled) {
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
enabled = enabled && g_flash_ready;
|
||||
if (g_native_stream != enabled || !enabled) discard_wii_output();
|
||||
g_native_stream = enabled;
|
||||
update_wii_ir_gate(time_us_32());
|
||||
#else
|
||||
switch2_mouse_capture_set_native_stream(g_flash_ready && enabled);
|
||||
#endif
|
||||
}
|
||||
|
||||
extern "C" uint32_t probe_controller_input_peek_native_report(
|
||||
uint32_t now_ms, uint8_t report[63]) {
|
||||
if (!g_flash_ready) return 0;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
(void)now_ms;
|
||||
return prepare_wii_report(report);
|
||||
#else
|
||||
return switch2_mouse_capture_peek_native_report(now_ms, report);
|
||||
#endif
|
||||
}
|
||||
|
||||
extern "C" bool probe_controller_input_commit_native_report(uint32_t serial) {
|
||||
if (!g_flash_ready) return false;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
if (!g_native_stream || !serial || serial != g_pending_serial ||
|
||||
g_pending_generation != g_wii_generation || !g_wii_active) return false;
|
||||
wii_ir_mouse_commit(g_pending_pointer);
|
||||
g_pending_serial = 0;
|
||||
g_committed_ticks = g_pending_ticks;
|
||||
g_have_committed_ticks = true;
|
||||
g_have_submission = true;
|
||||
g_submitted_us = time_us_32();
|
||||
++g_report_counter;
|
||||
return true;
|
||||
#else
|
||||
return switch2_mouse_capture_commit_native_report(serial);
|
||||
#endif
|
||||
}
|
||||
|
||||
extern "C" bool probe_controller_input_play_sample(uint8_t sample_id, uint64_t* token) {
|
||||
|
|
@ -95,17 +471,30 @@ extern "C" bool probe_controller_input_play_sample(uint8_t sample_id, uint64_t*
|
|||
if (token != nullptr) *token = 0;
|
||||
return false;
|
||||
}
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
return bluepad32_input_backend_wii_sample_request(sample_id, token);
|
||||
#else
|
||||
return switch2_mouse_capture_request_sample(
|
||||
sample_id, to_ms_since_boot(get_absolute_time()), token);
|
||||
#endif
|
||||
}
|
||||
|
||||
extern "C" int probe_controller_input_sample_result(uint64_t token, uint32_t now_ms) {
|
||||
if (!g_flash_ready) return -1;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
(void)now_ms;
|
||||
return bluepad32_input_backend_wii_sample_result(token);
|
||||
#else
|
||||
return switch2_mouse_capture_sample_result(token, now_ms);
|
||||
#endif
|
||||
}
|
||||
|
||||
extern "C" void probe_controller_input_cancel_sample(void) {
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
bluepad32_input_backend_wii_sample_cancel();
|
||||
#else
|
||||
switch2_mouse_capture_cancel_sample();
|
||||
#endif
|
||||
}
|
||||
|
||||
extern "C" void probe_controller_input_poll(uint32_t now_ms,
|
||||
|
|
@ -115,13 +504,14 @@ extern "C" void probe_controller_input_poll(uint32_t now_ms,
|
|||
*out = {};
|
||||
return;
|
||||
}
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
poll_wii_source(now_ms);
|
||||
#else
|
||||
Switch2MouseCaptureInput sample;
|
||||
if (switch2_mouse_capture_latest_input(g_input.serial, &sample)) {
|
||||
g_input.serial = sample.serial;
|
||||
g_input.active = sample.active;
|
||||
g_received_ms = sample.received_ms;
|
||||
// Preserve physical byte meaning: never route through the generic
|
||||
// solo Joy-Con rotation/mapping. Teardown fields are already zeroed.
|
||||
memcpy(g_input.buttons, sample.buttons, sizeof(g_input.buttons));
|
||||
memcpy(g_input.stick, sample.stick, sizeof(g_input.stick));
|
||||
g_input.native_status = sample.native_status;
|
||||
|
|
@ -130,11 +520,8 @@ extern "C" void probe_controller_input_poll(uint32_t now_ms,
|
|||
g_input.mouse_total_y = sample.mouse_total_y;
|
||||
g_input.mouse_surface = sample.mouse_surface;
|
||||
}
|
||||
// The producer can be a millisecond ahead of the caller's pre-poll clock.
|
||||
// Signed elapsed time tolerates that race and ordinary uint32_t rollover.
|
||||
// Expiration latches inactive until a newer capture serial arrives.
|
||||
if (g_input.active &&
|
||||
static_cast<int32_t>(now_ms - g_received_ms) >=
|
||||
// A producer timestamp can be slightly ahead of this pre-poll clock.
|
||||
if (g_input.active && static_cast<int32_t>(now_ms - g_received_ms) >=
|
||||
static_cast<int32_t>(kInputDeadlineMs)) {
|
||||
g_input.active = false;
|
||||
memset(g_input.buttons, 0, sizeof(g_input.buttons));
|
||||
|
|
@ -145,5 +532,6 @@ extern "C" void probe_controller_input_poll(uint32_t now_ms,
|
|||
g_input.mouse_total_y = 0;
|
||||
g_input.mouse_surface = 0;
|
||||
}
|
||||
#endif
|
||||
*out = g_input;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,30 +36,34 @@ bool probe_controller_input_start(void);
|
|||
// True means a Bluetooth pairing-window request was queued. Long holds NEVER
|
||||
// clear pairings in this bridge, and this does not inject USB controller input.
|
||||
bool probe_controller_input_pairing_task(void);
|
||||
// Core 0 native 08 relay: disabled until explicitly enabled after flash-ready
|
||||
// startup. Disable clears queued data, repeated enable preserves it. Resets
|
||||
// must disable the stream; this never changes Bluetooth bonds or pairing.
|
||||
// Only subsequent selected-source packets enter the separate 32-entry FIFO.
|
||||
// Overflow drops queued history and retains only the arriving packet.
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
// Main supplies the same validated calibration record advertised to the host.
|
||||
void probe_controller_input_set_stick_calibration(const uint8_t calibration[9]);
|
||||
// Native feature changes are output barriers, not Bluetooth/IMU resets.
|
||||
void probe_controller_input_set_native_features(uint8_t features);
|
||||
#endif
|
||||
// Core0 native08 output. Disable discards queued/prepared data; repeated enable
|
||||
// preserves it. Joy-Con mode relays its bounded FIFO; Wii mode synthesizes from
|
||||
// fresh calibrated sensors and the selected IR pointer. No pairing changes.
|
||||
void probe_controller_input_set_native_stream(bool enabled);
|
||||
// Copy a full opaque 63-byte payload (without report ID), oldest first. Returns
|
||||
// its never-reused boot-lifetime serial; 0 leaves report untouched. Latest source
|
||||
// or head >=500 ms old discards the FIFO; now_ms is the Pico boot-ms clock.
|
||||
// Nondestructive until successful HID submission followed by commit.
|
||||
// Copy one63-byte payload without report ID. Returns a boot-unique token, or0
|
||||
// without changing output. Nondestructive until successful HID submission and
|
||||
// commit. now_ms uses the Pico boot-ms clock; unavailable/stale input is rejected.
|
||||
uint32_t probe_controller_input_peek_native_report(uint32_t now_ms, uint8_t report[63]);
|
||||
// Remove only the exact current head once. A stale/replaced token cannot pop a
|
||||
// new stream's packet. Before flash-ready startup peek/commit return 0/false.
|
||||
bool probe_controller_input_commit_native_report(uint32_t serial);
|
||||
// Built-in vibration samples only; raw HD-rumble output is not forwarded.
|
||||
// A nonzero token means queued, not acknowledged. Result: 0 pending, 1 real
|
||||
// source ACK, -1 failed/stale. Reset cancels the request, never stored pairing.
|
||||
// A nonzero token means queued, not completed. Result:0 pending,1 completion,
|
||||
// -1 failed/stale. Joy-Con completion is its application ACK; Wii completion is
|
||||
// actual bounded rumble-driver dispatch (not an HD-waveform fidelity claim).
|
||||
// Reset cancels the request, never stored pairing.
|
||||
bool probe_controller_input_play_sample(uint8_t sample_id, uint64_t* token);
|
||||
int probe_controller_input_sample_result(uint64_t token, uint32_t now_ms);
|
||||
void probe_controller_input_cancel_sample(void);
|
||||
// Core 0 at 250 Hz; now_ms uses the Pico boot-millisecond clock. Only fresh
|
||||
// native 08 buttons/stick and cumulative mouse totals are exposed. Inactive
|
||||
// fields are zero except serial; the USB protocol must supply its calibrated
|
||||
// stick center rather than forwarding inactive stick bytes.
|
||||
// Core0 at250Hz; now_ms uses Pico boot milliseconds. Supplies current mapped
|
||||
// controls for diagnostic reports; the native sender owns motion consumption.
|
||||
// Inactive controls are zero except serial; USB supplies its calibrated center.
|
||||
void probe_controller_input_poll(uint32_t now_ms, probe_controller_input* out);
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
#include <string.h>
|
||||
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
#include "bootsel.h"
|
||||
#include "controller_input.h"
|
||||
#else
|
||||
#include "platform/pico/bootsel_button_sample.h"
|
||||
|
|
@ -63,6 +64,9 @@ static uint32_t last_hid_complete_ms;
|
|||
static bool hid_completion_seen;
|
||||
static uint32_t mouse_delivered_reports, mouse_logged_reports;
|
||||
static int64_t mouse_delivered_x, mouse_delivered_y;
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
static uint32_t last_native_trace_ms;
|
||||
#endif
|
||||
#endif
|
||||
#ifndef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
static probe_button_state button_test;
|
||||
|
|
@ -214,8 +218,13 @@ static void reset_protocol(void) {
|
|||
protocol.play_sample = probe_controller_input_play_sample;
|
||||
#endif
|
||||
#ifdef SWITCH2_PROBE_MEMORY
|
||||
if (!probe_memory_right_stick_center(protocol.right_stick_center))
|
||||
uint8_t stick_calibration[9];
|
||||
if (!probe_memory_right_stick_calibration(stick_calibration))
|
||||
panic("Invalid captured Joy-Con stick calibration");
|
||||
memcpy(protocol.right_stick_center, stick_calibration, sizeof(protocol.right_stick_center));
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
probe_controller_input_set_stick_calibration(stick_calibration);
|
||||
#endif
|
||||
protocol.read_memory = probe_memory_read;
|
||||
#endif
|
||||
uint8_t pairing[PROBE_PAIRING_BLOB_SIZE];
|
||||
|
|
@ -230,12 +239,18 @@ static void reset_protocol(void) {
|
|||
command_expected = 8;
|
||||
last_input_ms = 0;
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
probe_controller_input_set_native_features(0);
|
||||
#endif
|
||||
last_controller_poll_ms = 0;
|
||||
last_delivered_buttons = 0;
|
||||
probe_controller_input_set_native_stream(false);
|
||||
native_stream_ready = false;
|
||||
last_hid_complete_ms = 0;
|
||||
hid_completion_seen = false;
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
last_native_trace_ms = 0;
|
||||
#endif
|
||||
#endif
|
||||
#ifndef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
(void)probe_button_update(&button_test, -1, false);
|
||||
|
|
@ -267,12 +282,15 @@ static void complete_command(void) {
|
|||
previous_features != protocol.enabled_features) {
|
||||
probe_controller_input_set_native_stream(false);
|
||||
native_stream_ready = false;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
probe_controller_input_set_native_features(protocol.enabled_features);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
reply->length = (uint8_t)length;
|
||||
++reply_count;
|
||||
if (reply->deferred_token) {
|
||||
probe_debug_printf("[PROBE] Sample %u awaiting source ACK token=%" PRIu64 "\n",
|
||||
probe_debug_printf("[PROBE] Sample %u awaiting source completion token=%" PRIu64 "\n",
|
||||
command_frame[8], reply->deferred_token);
|
||||
} else {
|
||||
log_packet("BULK_REPLY_QUEUED", 0, 0, reply->data, reply->length);
|
||||
|
|
@ -364,11 +382,23 @@ static void controller_input_task(uint32_t now) {
|
|||
}
|
||||
|
||||
static void gate_native_report(uint8_t input[PROBE_INPUT_SIZE]) {
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
// Generated status follows virtual feature state, not a donor snapshot.
|
||||
input[8] = (uint8_t)(0x30 | ((protocol.enabled_features & 0x20) ? 8 : 0));
|
||||
#endif
|
||||
if (!(protocol.enabled_features & 1)) memset(input + 2, 0, 2);
|
||||
if (!(protocol.enabled_features & 2))
|
||||
memcpy(input + 5, protocol.right_stick_center, sizeof(protocol.right_stick_center));
|
||||
if (!(protocol.enabled_features & 0x10)) memset(input + 9, 0, 5);
|
||||
#ifdef SWITCH2_PROBE_OMIT_NATIVE_IMU
|
||||
// Deliberate A/B fault injection: leave every other field and feature bit intact.
|
||||
memset(input + 15, 0, 41);
|
||||
#elif defined(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
|
||||
if (!(protocol.enabled_features & 4)) input[15] = 0;
|
||||
memset(input + 16, 0, 40); // Otherwise preserve the genuine length byte.
|
||||
#else
|
||||
if (!(protocol.enabled_features & 4)) memset(input + 15, 0, 41);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -381,8 +411,13 @@ static void protocol_task(uint32_t now) {
|
|||
if (!ready) {
|
||||
const int result = probe_controller_input_sample_result(reply->deferred_token, now);
|
||||
if (result > 0) {
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
probe_debug_printf("[PROBE] Wii cue dispatched token=%" PRIu64 "\n",
|
||||
reply->deferred_token);
|
||||
#else
|
||||
probe_debug_printf("[PROBE] Source sample ACK token=%" PRIu64 "\n",
|
||||
reply->deferred_token);
|
||||
#endif
|
||||
reply->deferred_token = 0;
|
||||
ready = true;
|
||||
log_packet("BULK_REPLY_QUEUED", 0, 0, reply->data, reply->length);
|
||||
|
|
@ -468,6 +503,13 @@ void tud_hid_report_complete_cb(uint8_t instance, const uint8_t* report, uint16_
|
|||
if (report[0] == 0x08) {
|
||||
const int32_t dx = signed_mouse_delta(report + 10);
|
||||
const int32_t dy = signed_mouse_delta(report + 12);
|
||||
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
if ((uint32_t)(last_hid_complete_ms - last_native_trace_ms) >= 1000) {
|
||||
last_native_trace_ms = last_hid_complete_ms;
|
||||
// Observe the completed transfer, not a proposed or ungated report.
|
||||
log_packet("NATIVE_INPUT_DELIVERED", instance, report[0], report, length);
|
||||
}
|
||||
#endif
|
||||
if (dx || dy) {
|
||||
++mouse_delivered_reports;
|
||||
mouse_delivered_x += dx;
|
||||
|
|
@ -529,6 +571,10 @@ bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage,
|
|||
const tusb_control_request_t* request) {
|
||||
if (stage == CONTROL_STAGE_SETUP)
|
||||
log_packet("VENDOR_CONTROL", rhport, 0, (const uint8_t*)request, sizeof(*request));
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
if (probe_bootsel_vendor_control(rhport, stage, request))
|
||||
return true;
|
||||
#endif
|
||||
#ifdef SWITCH2_PROBE_IDENTITY_REPLY
|
||||
if (request->bmRequestType == 0xc0 && request->bRequest == 0x03 &&
|
||||
request->wValue == 0 && request->wIndex == 0) {
|
||||
|
|
@ -611,9 +657,18 @@ int main(void) {
|
|||
#else
|
||||
probe_debug_printf("\n[PROBE] Joy-Con 2 (R) USB enumeration recorder\n");
|
||||
#endif
|
||||
#ifdef SWITCH2_PROBE_OMIT_NATIVE_IMU
|
||||
probe_debug_printf("[PROBE] ACTIVATION_TEST=no-imu: native08 bytes 15..55 omitted; features, power, status, counters and cadence unchanged\n");
|
||||
#elif defined(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
|
||||
probe_debug_printf("[PROBE] ACTIVATION_TEST=zero-imu-payload: native08 bytes 16..55 zeroed; length, features and all other fields unchanged\n");
|
||||
#endif
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
probe_controller_input_init();
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
probe_debug_printf("[PROBE] UART0 GP0=TX, 115200 8N1; selected Wii IR/MotionPlus source enabled\n");
|
||||
#else
|
||||
probe_debug_printf("[PROBE] UART0 GP0=TX, 115200 8N1; selected right Joy-Con Bluetooth source enabled\n");
|
||||
#endif
|
||||
#else
|
||||
probe_debug_printf("[PROBE] UART0 GP0=TX, 115200 8N1; Bluetooth disabled\n");
|
||||
#endif
|
||||
|
|
@ -633,7 +688,12 @@ int main(void) {
|
|||
probe_debug_printf("[PROBE] Own virtual pairing storage offset=%08" PRIx32 "\n",
|
||||
probe_storage_offset());
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
probe_debug_printf("[PROBE] Wii IR drives native mouse movement; buttons retain profile mapping; keep Wii still for MotionPlus calibration\n");
|
||||
probe_debug_printf("[PROBE] Hold BOOTSEL2s for pairing; Wii cue feedback uses bounded ERM patterns, not HD audio waveforms\n");
|
||||
#else
|
||||
probe_debug_printf("[PROBE] Live right Joy-Con buttons/stick/native mouse; hold BOOTSEL 2s for Bluetooth pairing (never clears pairings)\n");
|
||||
#endif
|
||||
#else
|
||||
probe_debug_printf("[PROBE] Manual input test: hold BOOTSEL for SL+SR, release for neutral; no controller forwarding\n");
|
||||
#endif
|
||||
|
|
@ -650,6 +710,9 @@ int main(void) {
|
|||
tud_task();
|
||||
drain_log();
|
||||
const uint32_t now = to_ms_since_boot(get_absolute_time());
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
probe_bootsel_task(now);
|
||||
#endif
|
||||
#ifdef SWITCH2_PROBE_USB_INIT
|
||||
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
|
||||
if (probe_controller_input_pairing_task())
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ static bool valid_calibration(const uint8_t* data) {
|
|||
return true;
|
||||
}
|
||||
|
||||
bool probe_memory_right_stick_center(uint8_t output[3]) {
|
||||
bool probe_memory_right_stick_calibration(uint8_t output[9]) {
|
||||
if (!output) return false;
|
||||
// A solo Joy-Con uses the primary calibration record, even for the right
|
||||
// controller. User magic precedes its 9-byte record; factory has no magic.
|
||||
|
|
@ -52,6 +52,6 @@ bool probe_memory_right_stick_center(uint8_t output[3]) {
|
|||
if (user[0] == 0xb2 && user[1] == 0xa1 && valid_calibration(user + 2))
|
||||
selected = user + 2;
|
||||
if (!valid_calibration(selected)) return false;
|
||||
memcpy(output, selected, 3);
|
||||
memcpy(output, selected, 9);
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,4 +4,5 @@
|
|||
#include <stdint.h>
|
||||
|
||||
bool probe_memory_read(uint32_t address, uint8_t* output, size_t length);
|
||||
bool probe_memory_right_stick_center(uint8_t output[3]);
|
||||
// Packed center, positive travel, negative travel (two12-bit axes each).
|
||||
bool probe_memory_right_stick_calibration(uint8_t output[9]);
|
||||
|
|
|
|||
440
tools/switch2_usb_probe/native_imu.cpp
Normal file
440
tools/switch2_usb_probe/native_imu.cpp
Normal file
|
|
@ -0,0 +1,440 @@
|
|||
#include "native_imu.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <limits.h>
|
||||
|
||||
namespace {
|
||||
constexpr uint32_t kFreshUs = 150000;
|
||||
constexpr uint32_t kMaximumUpdateGapUs = 50000;
|
||||
constexpr uint32_t kCalibrationUs = 1500000;
|
||||
constexpr uint32_t kCalibrationSamples = 64;
|
||||
constexpr uint32_t kVariationSamples = 16;
|
||||
// The stationary Wii trace has 0.383 dps / 0.010 g vector RMS variation;
|
||||
// quantized sample-to-sample steps reach 0.806 dps / 0.030 g. Judge variance
|
||||
// over distinct samples, with roughly twice that measured noise allowance.
|
||||
constexpr float kGyroRmsDps = 0.75f;
|
||||
constexpr float kAccelRmsG = 0.025f;
|
||||
// One degree between averaged gravity directions, independent of g scale.
|
||||
constexpr float kGravityDirectionCosSquared = 0.9996954135f;
|
||||
constexpr float kGravityTimeConstantUs = 1000000.0f;
|
||||
constexpr float kRadiansPerDegree = 0.017453292519943295f;
|
||||
constexpr float kTwoPi = 6.2831853071795864769f;
|
||||
// A two-second complementary heading correction, driven by distinct optical
|
||||
// sample times rather than polling frequency. Reuse the 150ms sensor lifetime.
|
||||
constexpr float kOpticalTimeConstantUs = 2000000.0f;
|
||||
// Reject camera-forward directions within about 14.5 degrees of world vertical.
|
||||
constexpr float kMinimumOpticalHorizontalSquared = 0.25f * 0.25f;
|
||||
|
||||
bool finite_vector(const float* values, unsigned count) {
|
||||
for (unsigned i = 0; i < count; ++i) {
|
||||
if (!std::isfinite(values[i])) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
float squared_norm(const float values[3]) {
|
||||
return values[0] * values[0] + values[1] * values[1] + values[2] * values[2];
|
||||
}
|
||||
|
||||
bool accumulate_stationary(const float sample[3], uint32_t count, float mean[3],
|
||||
float& variation, float rms_limit) {
|
||||
if (count == 1) {
|
||||
for (unsigned i = 0; i < 3; ++i) mean[i] = sample[i];
|
||||
variation = 0.0f;
|
||||
return true;
|
||||
}
|
||||
// Welford's sum of squared vector deviations avoids subtracting the large
|
||||
// uncalibrated bias from a sum of squares. No samples or heap state retained.
|
||||
const float weight = 1.0f / static_cast<float>(count);
|
||||
float deviation_squared = 0.0f;
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
const float delta = sample[i] - mean[i];
|
||||
deviation_squared += delta * delta;
|
||||
mean[i] += delta * weight;
|
||||
variation += delta * (sample[i] - mean[i]);
|
||||
}
|
||||
const float variance_limit = rms_limit * rms_limit;
|
||||
// A four-RMS excursion discards impulses immediately instead of letting a
|
||||
// long quiet prefix dilute them. Ordinary quantization is judged by RMS.
|
||||
return std::isfinite(deviation_squared) && std::isfinite(variation) &&
|
||||
deviation_squared < 16.0f * variance_limit &&
|
||||
(count < kVariationSamples || variation < static_cast<float>(count - 1) * variance_limit);
|
||||
}
|
||||
|
||||
// Match the reference codec's nearest-even rounding without double arithmetic
|
||||
// or a dependency on the process floating-point rounding mode. Callers ensure
|
||||
// the input and its rounded result fit int32_t.
|
||||
int32_t round_even(float value) {
|
||||
int32_t integral = static_cast<int32_t>(value);
|
||||
const float remainder = value - static_cast<float>(integral);
|
||||
if (remainder > 0.5f || (remainder == 0.5f && (integral & 1))) ++integral;
|
||||
if (remainder < -0.5f || (remainder == -0.5f && (integral & 1))) --integral;
|
||||
return integral;
|
||||
}
|
||||
|
||||
uint32_t ratio_code(float ratio) {
|
||||
// Add the midpoint as an integer: (ratio + 1.0f) would first discard low
|
||||
// ratio bits. The +1 endpoint is 2^31, outside the unsigned 31-bit field.
|
||||
const int32_t offset = round_even(ratio * 1073741824.0f);
|
||||
const uint32_t code = static_cast<uint32_t>(offset) + UINT32_C(0x40000000);
|
||||
return code > UINT32_C(0x7fffffff) ? UINT32_C(0x7fffffff) : code;
|
||||
}
|
||||
|
||||
int32_t acceleration_code(float acceleration) {
|
||||
if (acceleration >= 8.0f) return INT32_MAX;
|
||||
if (acceleration <= -8.0f) return INT32_MIN;
|
||||
return round_even(acceleration * 268435456.0f);
|
||||
}
|
||||
|
||||
void put_u32(uint8_t* output, uint32_t value) {
|
||||
output[0] = static_cast<uint8_t>(value);
|
||||
output[1] = static_cast<uint8_t>(value >> 8);
|
||||
output[2] = static_cast<uint8_t>(value >> 16);
|
||||
output[3] = static_cast<uint8_t>(value >> 24);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
extern "C" bool probe_native_imu_pack(const float quaternion_wxyz[4], const float accel_g[3],
|
||||
uint16_t counter_ticks, uint16_t elapsed_ticks,
|
||||
int16_t temperature_raw, uint8_t output[30]) {
|
||||
if (!quaternion_wxyz || !accel_g || !output || counter_ticks > 0x0fff ||
|
||||
elapsed_ticks > 0x0fff || !finite_vector(quaternion_wxyz, 4) || !finite_vector(accel_g, 3)) {
|
||||
return false;
|
||||
}
|
||||
unsigned largest = 0;
|
||||
for (unsigned i = 1; i < 4; ++i) {
|
||||
if (std::fabs(quaternion_wxyz[i]) > std::fabs(quaternion_wxyz[largest])) largest = i;
|
||||
}
|
||||
if (quaternion_wxyz[largest] == 0.0f) return false;
|
||||
|
||||
uint32_t ratios[3];
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
ratios[i] = ratio_code(quaternion_wxyz[(largest + i + 1) & 3] / quaternion_wxyz[largest]);
|
||||
}
|
||||
output[0] = static_cast<uint8_t>(counter_ticks);
|
||||
output[1] = static_cast<uint8_t>((counter_ticks >> 8) | (elapsed_ticks << 4));
|
||||
output[2] = static_cast<uint8_t>(elapsed_ticks >> 4);
|
||||
output[3] = 0x0c;
|
||||
put_u32(output + 4, largest | (ratios[0] << 3));
|
||||
put_u32(output + 8, (ratios[0] >> 29) | (ratios[1] << 2));
|
||||
put_u32(output + 12, (ratios[1] >> 30) | (ratios[2] << 1));
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
put_u32(output + 16 + i * 4, static_cast<uint32_t>(acceleration_code(accel_g[i])));
|
||||
}
|
||||
const uint16_t temperature = static_cast<uint16_t>(temperature_raw);
|
||||
output[28] = static_cast<uint8_t>(temperature);
|
||||
output[29] = static_cast<uint8_t>(temperature >> 8);
|
||||
return true;
|
||||
}
|
||||
|
||||
void ProbeNativeMotion::reset() {
|
||||
*this = ProbeNativeMotion{};
|
||||
}
|
||||
|
||||
void ProbeNativeMotion::clear_candidate() {
|
||||
candidate_ = false;
|
||||
gyro_count_ = 0;
|
||||
accel_count_ = 0;
|
||||
}
|
||||
|
||||
void ProbeNativeMotion::invalidate() {
|
||||
ready_ = false;
|
||||
have_accel_ = false;
|
||||
have_gyro_ = false;
|
||||
have_optical_reference_ = false;
|
||||
have_optical_sample_ = false;
|
||||
clear_candidate();
|
||||
quaternion_[0] = 1.0f;
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
quaternion_[i + 1] = 0.0f;
|
||||
acceleration_[i] = 0.0f;
|
||||
gyro_dps_[i] = 0.0f;
|
||||
bias_[i] = 0.0f;
|
||||
}
|
||||
// Keep the last observed identities until a connection change or explicit
|
||||
// reset. Restoring availability cannot turn the same packet into new data.
|
||||
}
|
||||
|
||||
bool ProbeNativeMotion::initialize_orientation() {
|
||||
const float norm = std::sqrt(squared_norm(mean_accel_));
|
||||
if (!std::isfinite(norm) || norm <= 0.0f) return false;
|
||||
const float x = mean_accel_[0] / norm;
|
||||
const float y = mean_accel_[1] / norm;
|
||||
const float z = mean_accel_[2] / norm;
|
||||
const float horizontal = std::sqrt(x * x + y * y);
|
||||
quaternion_[3] = 0.0f;
|
||||
if (horizontal > 0.0f) {
|
||||
// atan2 remains well conditioned near -Z, unlike normalizing [1+z,y,-x,0].
|
||||
const float half_angle = 0.5f * std::atan2(horizontal, z);
|
||||
const float sine = std::sin(half_angle);
|
||||
quaternion_[0] = std::cos(half_angle);
|
||||
quaternion_[1] = (y / horizontal) * sine;
|
||||
quaternion_[2] = (-x / horizontal) * sine;
|
||||
} else {
|
||||
quaternion_[0] = z >= 0.0f ? 1.0f : 0.0f;
|
||||
quaternion_[1] = z >= 0.0f ? 0.0f : 1.0f;
|
||||
quaternion_[2] = 0.0f;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ProbeNativeMotion::integrate(const float gyro_dps[3], uint32_t elapsed_us) {
|
||||
if (elapsed_us == 0) return true;
|
||||
const float x = gyro_dps[0] - bias_[0];
|
||||
const float y = gyro_dps[1] - bias_[1];
|
||||
const float z = gyro_dps[2] - bias_[2];
|
||||
const float norm_squared = x * x + y * y + z * z;
|
||||
if (!std::isfinite(norm_squared)) return false;
|
||||
if (norm_squared == 0.0f) return true;
|
||||
const float norm = std::sqrt(norm_squared);
|
||||
const float half_angle = norm * (static_cast<float>(elapsed_us) * (0.5e-6f * kRadiansPerDegree));
|
||||
const float sine_scale = std::sin(half_angle) / norm;
|
||||
const float dw = std::cos(half_angle);
|
||||
const float dx = x * sine_scale;
|
||||
const float dy = y * sine_scale;
|
||||
const float dz = z * sine_scale;
|
||||
const float w = quaternion_[0];
|
||||
const float qx = quaternion_[1];
|
||||
const float qy = quaternion_[2];
|
||||
const float qz = quaternion_[3];
|
||||
// Body-local rates multiply on the right of the body-to-reference rotation.
|
||||
quaternion_[0] = w * dw - qx * dx - qy * dy - qz * dz;
|
||||
quaternion_[1] = w * dx + qx * dw + qy * dz - qz * dy;
|
||||
quaternion_[2] = w * dy - qx * dz + qy * dw + qz * dx;
|
||||
quaternion_[3] = w * dz + qx * dy - qy * dx + qz * dw;
|
||||
return normalize_orientation();
|
||||
}
|
||||
|
||||
bool ProbeNativeMotion::normalize_orientation() {
|
||||
const float length_squared = quaternion_[0] * quaternion_[0] + quaternion_[1] * quaternion_[1] +
|
||||
quaternion_[2] * quaternion_[2] + quaternion_[3] * quaternion_[3];
|
||||
if (!std::isfinite(length_squared) || length_squared <= 0.0f) return false;
|
||||
const float reciprocal_length = 1.0f / std::sqrt(length_squared);
|
||||
for (float& component : quaternion_) component *= reciprocal_length;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ProbeNativeMotion::correct_gravity(uint32_t elapsed_us) {
|
||||
if (elapsed_us == 0) return true;
|
||||
const float norm_squared = squared_norm(acceleration_);
|
||||
// Do not treat obvious dynamic acceleration as a gravity observation.
|
||||
// The real acceleration remains unchanged in the outgoing report.
|
||||
if (norm_squared < 0.9f * 0.9f || norm_squared > 1.1f * 1.1f) return true;
|
||||
const float reciprocal_norm = 1.0f / std::sqrt(norm_squared);
|
||||
const float ax = acceleration_[0] * reciprocal_norm;
|
||||
const float ay = acceleration_[1] * reciprocal_norm;
|
||||
const float az = acceleration_[2] * reciprocal_norm;
|
||||
const float w = quaternion_[0], x = quaternion_[1], y = quaternion_[2], z = quaternion_[3];
|
||||
const float tx = 2.0f * (y * az - z * ay);
|
||||
const float ty = 2.0f * (z * ax - x * az);
|
||||
const float tz = 2.0f * (x * ay - y * ax);
|
||||
const float gx = ax + w * tx + y * tz - z * ty;
|
||||
const float gy = ay + w * ty + z * tx - x * tz;
|
||||
const float gz = az + w * tz + x * ty - y * tx;
|
||||
const float horizontal = std::sqrt(gx * gx + gy * gy);
|
||||
if (horizontal == 0.0f && gz >= 0.0f) return true;
|
||||
// Complementary tilt correction in reference space. Its rotation axis has
|
||||
// no reference-Z component: do not invent a yaw observation from gravity.
|
||||
// Sensor elapsed time, not Core 0 polling, controls the filter strength.
|
||||
const float alpha = static_cast<float>(elapsed_us) /
|
||||
(kGravityTimeConstantUs + static_cast<float>(elapsed_us));
|
||||
const float half_angle = 0.5f * alpha * std::atan2(horizontal, gz);
|
||||
const float sine = std::sin(half_angle);
|
||||
const float cw = std::cos(half_angle);
|
||||
const float cx = horizontal > 0.0f ? (gy / horizontal) * sine : sine;
|
||||
const float cy = horizontal > 0.0f ? (-gx / horizontal) * sine : 0.0f;
|
||||
quaternion_[0] = cw * w - cx * x - cy * y;
|
||||
quaternion_[1] = cw * x + cx * w + cy * z;
|
||||
quaternion_[2] = cw * y - cx * z + cy * w;
|
||||
quaternion_[3] = cw * z + cx * y - cy * x;
|
||||
return normalize_orientation();
|
||||
}
|
||||
|
||||
void ProbeNativeMotion::observe_optical_heading(uint32_t now_us, uint32_t reference_generation,
|
||||
uint32_t sequence, uint32_t sample_us,
|
||||
float yaw_radians, bool valid) {
|
||||
if (!ready_) return;
|
||||
if (!have_optical_generation_ || reference_generation != optical_generation_) {
|
||||
have_optical_generation_ = true;
|
||||
optical_generation_ = reference_generation;
|
||||
seen_optical_sequence_ = false;
|
||||
have_optical_reference_ = false;
|
||||
have_optical_sample_ = false;
|
||||
}
|
||||
if (!valid || (have_optical_sample_ && now_us - optical_sample_us_ >= kFreshUs)) {
|
||||
// Retain the relative anchor while hidden, but never accumulate filter
|
||||
// weight for the interval without observations.
|
||||
have_optical_sample_ = false;
|
||||
}
|
||||
const uint32_t sequence_delta = sequence - optical_sequence_;
|
||||
if (seen_optical_sequence_ && (sequence_delta == 0 || sequence_delta >= 0x80000000u)) return;
|
||||
seen_optical_sequence_ = true;
|
||||
optical_sequence_ = sequence;
|
||||
if (!valid || !std::isfinite(yaw_radians) || now_us - sample_us >= kFreshUs) {
|
||||
have_optical_sample_ = false;
|
||||
return;
|
||||
}
|
||||
// Reject backwards timestamps while both observations could still be
|
||||
// fresh. Unsigned differences admit normal microsecond-clock rollover.
|
||||
if (have_optical_reference_ && now_us - optical_sample_us_ < kFreshUs &&
|
||||
sample_us - optical_sample_us_ >= 0x80000000u) return;
|
||||
const float w = quaternion_[0], x = quaternion_[1], y = quaternion_[2], z = quaternion_[3];
|
||||
// Native body Y is camera forward. Its world XY projection supplies heading;
|
||||
// near vertical, yaw is ill-conditioned and must not become an observation.
|
||||
const float forward_x = 2.0f * (x * y - w * z);
|
||||
const float forward_y = 1.0f - 2.0f * (x * x + z * z);
|
||||
if (forward_x * forward_x + forward_y * forward_y < kMinimumOpticalHorizontalSquared) {
|
||||
have_optical_sample_ = false;
|
||||
return;
|
||||
}
|
||||
const uint32_t elapsed_us = have_optical_sample_ ? sample_us - optical_sample_us_ : 0;
|
||||
optical_sample_us_ = sample_us;
|
||||
have_optical_sample_ = true;
|
||||
const float heading = std::atan2(forward_y, forward_x);
|
||||
const float bearing = std::remainder(yaw_radians, kTwoPi);
|
||||
if (!have_optical_reference_) {
|
||||
optical_reference_radians_ = std::remainder(heading + bearing, kTwoPi);
|
||||
have_optical_reference_ = true;
|
||||
return;
|
||||
}
|
||||
if (elapsed_us == 0 || elapsed_us >= kFreshUs) return;
|
||||
// Optical direction complements gyro heading; concept reference:
|
||||
// https://github.com/dolphin-emu/dolphin/blob/master/Source/Core/InputCommon/ControllerInterface/Wiimote/WiimoteController.cpp
|
||||
// Independently use a relative yaw anchor and elapsed-time gain, not its
|
||||
// per-frame full-vector correction. Translation also changes bar bearing;
|
||||
// bar placement/pitch can bias it, so this is not absolute world heading.
|
||||
// Positive optical aim-right is a negative world-Z heading change.
|
||||
const float error = std::remainder(optical_reference_radians_ - bearing - heading, kTwoPi);
|
||||
const float alpha = static_cast<float>(elapsed_us) /
|
||||
(kOpticalTimeConstantUs + static_cast<float>(elapsed_us));
|
||||
const float half_angle = 0.5f * alpha * error;
|
||||
const float cw = std::cos(half_angle), cz = std::sin(half_angle);
|
||||
// Left multiplication about reference-world Z preserves gravity-aligned
|
||||
// tilt, and neither the measured acceleration nor learned gyro bias changes.
|
||||
quaternion_[0] = cw * w - cz * z;
|
||||
quaternion_[1] = cw * x - cz * y;
|
||||
quaternion_[2] = cw * y + cz * x;
|
||||
quaternion_[3] = cw * z + cz * w;
|
||||
if (!normalize_orientation()) invalidate();
|
||||
}
|
||||
|
||||
void ProbeNativeMotion::update(uint32_t now_us, uint32_t connection_generation,
|
||||
const ProbeNativeMotionSample& sample) {
|
||||
if (!have_generation_ || connection_generation != connection_generation_) {
|
||||
reset();
|
||||
have_generation_ = true;
|
||||
connection_generation_ = connection_generation;
|
||||
}
|
||||
const uint32_t elapsed_us = have_update_ ? now_us - update_us_ : 0;
|
||||
update_us_ = now_us;
|
||||
have_update_ = true;
|
||||
if (elapsed_us > kMaximumUpdateGapUs) invalidate();
|
||||
if (!sample.accel_valid || !sample.gyro_valid ||
|
||||
!finite_vector(sample.accel_g, 3) || !finite_vector(sample.gyro_dps, 3)) {
|
||||
invalidate();
|
||||
return;
|
||||
}
|
||||
|
||||
const bool new_accel = !seen_accel_sequence_ || sample.accel_sequence != accel_sequence_;
|
||||
const bool new_gyro = !seen_gyro_sequence_ || sample.gyro_sequence != gyro_sequence_;
|
||||
// A fresh replacement cannot conceal a stale interval between observations.
|
||||
// Unsigned differences also reject timestamps that move backwards.
|
||||
if ((have_accel_ && new_accel && sample.accel_us - accel_us_ > kFreshUs) ||
|
||||
(have_gyro_ && new_gyro && sample.gyro_us - gyro_us_ > kFreshUs)) {
|
||||
invalidate();
|
||||
}
|
||||
const bool was_ready = ready_;
|
||||
const uint32_t accel_elapsed_us = was_ready && new_accel ? sample.accel_us - accel_us_ : 0;
|
||||
float previous_gyro[3];
|
||||
if (was_ready && new_gyro) {
|
||||
for (unsigned i = 0; i < 3; ++i) previous_gyro[i] = gyro_dps_[i];
|
||||
}
|
||||
if (new_accel) {
|
||||
seen_accel_sequence_ = true;
|
||||
accel_sequence_ = sample.accel_sequence;
|
||||
accel_us_ = sample.accel_us;
|
||||
have_accel_ = true;
|
||||
for (unsigned i = 0; i < 3; ++i) acceleration_[i] = sample.accel_g[i];
|
||||
}
|
||||
if (new_gyro) {
|
||||
seen_gyro_sequence_ = true;
|
||||
gyro_sequence_ = sample.gyro_sequence;
|
||||
gyro_us_ = sample.gyro_us;
|
||||
have_gyro_ = true;
|
||||
for (unsigned i = 0; i < 3; ++i) gyro_dps_[i] = sample.gyro_dps[i];
|
||||
}
|
||||
if ((have_accel_ && now_us - accel_us_ >= kFreshUs) || (have_gyro_ && now_us - gyro_us_ >= kFreshUs)) {
|
||||
invalidate();
|
||||
return;
|
||||
}
|
||||
// Wii acceleration and gyro arrive on independent packets. Keep a fresh
|
||||
// first half while waiting for the other sensor after invalidation.
|
||||
if (!have_accel_ || !have_gyro_) return;
|
||||
if (was_ready) {
|
||||
// Zero-order hold uses actual receipt times, independent of update/USB
|
||||
// cadence. Split only when a new rate arrived inside this update span.
|
||||
const uint32_t rate_age_us = now_us - gyro_us_;
|
||||
if (new_gyro && rate_age_us < elapsed_us) {
|
||||
if (!integrate(previous_gyro, elapsed_us - rate_age_us) || !integrate(gyro_dps_, rate_age_us)) {
|
||||
invalidate();
|
||||
}
|
||||
} else if (!integrate(gyro_dps_, elapsed_us)) {
|
||||
invalidate();
|
||||
}
|
||||
if (ready_ && new_accel && !correct_gravity(accel_elapsed_us)) invalidate();
|
||||
return;
|
||||
}
|
||||
|
||||
const float acceleration_norm_squared = squared_norm(acceleration_);
|
||||
if (acceleration_norm_squared < 0.85f * 0.85f ||
|
||||
acceleration_norm_squared > 1.15f * 1.15f) {
|
||||
clear_candidate();
|
||||
return;
|
||||
}
|
||||
if (!candidate_) {
|
||||
if (!new_gyro) return;
|
||||
candidate_ = true;
|
||||
candidate_us_ = gyro_us_;
|
||||
gyro_count_ = 0;
|
||||
accel_count_ = 0;
|
||||
}
|
||||
if (new_accel || accel_count_ == 0) {
|
||||
if (!accumulate_stationary(acceleration_, ++accel_count_, mean_accel_,
|
||||
accel_variation_, kAccelRmsG)) {
|
||||
clear_candidate();
|
||||
return;
|
||||
}
|
||||
if (accel_count_ == kVariationSamples) {
|
||||
for (unsigned i = 0; i < 3; ++i) gravity_reference_[i] = mean_accel_[i];
|
||||
} else if (accel_count_ > kVariationSamples) {
|
||||
// Anchor the averaged direction rather than the previous packet:
|
||||
// slow tilt must not be accepted as a series of small noisy steps.
|
||||
float dot = 0.0f;
|
||||
for (unsigned i = 0; i < 3; ++i) dot += gravity_reference_[i] * mean_accel_[i];
|
||||
if (dot <= 0.0f || dot * dot < squared_norm(gravity_reference_) *
|
||||
squared_norm(mean_accel_) * kGravityDirectionCosSquared) {
|
||||
clear_candidate();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!new_gyro) return;
|
||||
// An absolute angular-rate limit cannot distinguish motion from the bias
|
||||
// being estimated. Changing rates, acceleration and gravity direction can;
|
||||
// perfectly steady rotation about gravity still requires the user to rest.
|
||||
if (!accumulate_stationary(gyro_dps_, ++gyro_count_, mean_gyro_,
|
||||
gyro_variation_, kGyroRmsDps)) {
|
||||
clear_candidate();
|
||||
return;
|
||||
}
|
||||
if (gyro_count_ >= kCalibrationSamples && accel_count_ >= kVariationSamples &&
|
||||
gyro_us_ - candidate_us_ >= kCalibrationUs) {
|
||||
if (!initialize_orientation()) {
|
||||
clear_candidate();
|
||||
return;
|
||||
}
|
||||
for (unsigned i = 0; i < 3; ++i) bias_[i] = mean_gyro_[i];
|
||||
ready_ = true;
|
||||
clear_candidate();
|
||||
}
|
||||
}
|
||||
99
tools/switch2_usb_probe/native_imu.h
Normal file
99
tools/switch2_usb_probe/native_imu.h
Normal file
|
|
@ -0,0 +1,99 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// Encode one native mode-0 IMU block. Inputs use the native-right body frame;
|
||||
// quaternion wxyz rotates body vectors into the reference frame. Quaternion
|
||||
// scale/sign do not matter. Finite acceleration saturates at signed Q28 limits.
|
||||
// Invalid inputs leave output untouched; counter and elapsed must fit 12 bits.
|
||||
bool probe_native_imu_pack(const float quaternion_wxyz[4], const float accel_g[3],
|
||||
uint16_t counter_ticks, uint16_t elapsed_ticks,
|
||||
int16_t temperature_raw, uint8_t output[30]);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
struct ProbeNativeMotionSample {
|
||||
bool accel_valid = false;
|
||||
bool gyro_valid = false;
|
||||
uint32_t accel_sequence = 0;
|
||||
uint32_t gyro_sequence = 0;
|
||||
uint32_t accel_us = 0;
|
||||
uint32_t gyro_us = 0;
|
||||
float accel_g[3]{};
|
||||
float gyro_dps[3]{};
|
||||
};
|
||||
|
||||
// Core 0 only. Call update even when sensors are unavailable, and consult ready
|
||||
// before using the orientation. Sequence identities, not polling, admit samples;
|
||||
// repeated sequences cannot refresh timestamps or contribute to calibration.
|
||||
// Startup requires the user to rest the controller: constant rotation about
|
||||
// gravity is indistinguishable from an unknown gyro bias without another sensor.
|
||||
// Fresh near-1g acceleration corrects tilt drift after startup. Heading remains
|
||||
// gyro-derived unless a reliable optical heading observation is supplied.
|
||||
class ProbeNativeMotion {
|
||||
public:
|
||||
void reset();
|
||||
void update(uint32_t now_us, uint32_t connection_generation,
|
||||
const ProbeNativeMotionSample& sample);
|
||||
// Call after update, using a full observed sensor-bar pair (never inferred).
|
||||
// Positive optical yaw means aim-right, the negative reference-world turn.
|
||||
// The first sample in each optical generation anchors the current heading;
|
||||
// it supplies neither absolute world yaw nor an absolute console cursor.
|
||||
void observe_optical_heading(uint32_t now_us, uint32_t reference_generation,
|
||||
uint32_t sequence, uint32_t sample_us,
|
||||
float yaw_radians, bool valid);
|
||||
bool ready() const { return ready_; }
|
||||
const float* quaternion() const { return quaternion_; }
|
||||
const float* acceleration() const { return acceleration_; }
|
||||
const float* bias() const { return bias_; }
|
||||
|
||||
private:
|
||||
void invalidate();
|
||||
void clear_candidate();
|
||||
bool initialize_orientation();
|
||||
bool integrate(const float gyro_dps[3], uint32_t elapsed_us);
|
||||
bool correct_gravity(uint32_t elapsed_us);
|
||||
bool normalize_orientation();
|
||||
|
||||
bool have_generation_ = false;
|
||||
bool have_update_ = false;
|
||||
bool seen_accel_sequence_ = false;
|
||||
bool seen_gyro_sequence_ = false;
|
||||
bool have_accel_ = false;
|
||||
bool have_gyro_ = false;
|
||||
bool candidate_ = false;
|
||||
bool ready_ = false;
|
||||
bool have_optical_generation_ = false;
|
||||
bool seen_optical_sequence_ = false;
|
||||
bool have_optical_reference_ = false;
|
||||
bool have_optical_sample_ = false;
|
||||
uint32_t connection_generation_ = 0;
|
||||
uint32_t update_us_ = 0;
|
||||
uint32_t accel_sequence_ = 0;
|
||||
uint32_t gyro_sequence_ = 0;
|
||||
uint32_t accel_us_ = 0;
|
||||
uint32_t gyro_us_ = 0;
|
||||
uint32_t candidate_us_ = 0;
|
||||
uint32_t gyro_count_ = 0;
|
||||
uint32_t accel_count_ = 0;
|
||||
uint32_t optical_generation_ = 0;
|
||||
uint32_t optical_sequence_ = 0;
|
||||
uint32_t optical_sample_us_ = 0;
|
||||
float optical_reference_radians_ = 0.0f;
|
||||
float gyro_variation_ = 0.0f;
|
||||
float accel_variation_ = 0.0f;
|
||||
float quaternion_[4]{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
float acceleration_[3]{};
|
||||
float gyro_dps_[3]{};
|
||||
float bias_[3]{};
|
||||
float mean_gyro_[3]{};
|
||||
float mean_accel_[3]{};
|
||||
float gravity_reference_[3]{};
|
||||
};
|
||||
#endif
|
||||
|
|
@ -18,6 +18,68 @@ function(switch2_usb_probe_configure target)
|
|||
${PICO_SDK_PATH}/lib/btstack/platform/embedded)
|
||||
target_compile_definitions(${target} PRIVATE
|
||||
CFG_TUSB_CONFIG_FILE="${SWITCH2_USB_PROBE_DIR}/tusb_config.h")
|
||||
option(SWITCH2_PROBE_TRACE_NATIVE_INPUT
|
||||
"Trace one completed native USB input report per second without changing its contents" OFF)
|
||||
if(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
||||
if(NOT SWITCH_PICO_SWITCH2_USB_BRIDGE)
|
||||
message(FATAL_ERROR "Native input tracing requires the Bluetooth USB bridge")
|
||||
endif()
|
||||
target_compile_definitions(${target} PRIVATE SWITCH2_PROBE_TRACE_NATIVE_INPUT=1)
|
||||
endif()
|
||||
if(SWITCH2_BRIDGE_WII_INPUT)
|
||||
set(SWITCH2_WII_IR_SENSITIVITY 3 CACHE STRING "Standard Wii camera sensitivity preset: 2 or 3")
|
||||
set_property(CACHE SWITCH2_WII_IR_SENSITIVITY PROPERTY STRINGS 2 3)
|
||||
if(NOT "${SWITCH2_WII_IR_SENSITIVITY}" MATCHES "^[23]$")
|
||||
message(FATAL_ERROR "SWITCH2_WII_IR_SENSITIVITY must be standard preset 2 or 3")
|
||||
endif()
|
||||
target_compile_definitions(bluepad32 PRIVATE
|
||||
SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL=${SWITCH2_WII_IR_SENSITIVITY})
|
||||
set(SWITCH2_WII_IR_VIEW_WIDTH 660 CACHE STRING "Native Wii IR viewport width in camera pixels")
|
||||
set(SWITCH2_WII_IR_VIEW_HEIGHT 370 CACHE STRING "Native Wii IR viewport height in camera pixels")
|
||||
set(SWITCH2_WII_IR_OFFSET_X 0 CACHE STRING "Native Wii IR viewport horizontal camera offset")
|
||||
set(SWITCH2_WII_IR_OFFSET_Y -115 CACHE STRING "Native Wii IR viewport vertical offset: -115 below, +115 above")
|
||||
set(SWITCH2_WII_IR_SPAN_X 1920 CACHE STRING "Native mouse counts across the IR viewport width")
|
||||
set(SWITCH2_WII_IR_SPAN_Y 1080 CACHE STRING "Native mouse counts across the IR viewport height")
|
||||
foreach(field VIEW_WIDTH VIEW_HEIGHT OFFSET_X OFFSET_Y SPAN_X SPAN_Y)
|
||||
if(NOT "${SWITCH2_WII_IR_${field}}" MATCHES "^-?[0-9]+$")
|
||||
message(FATAL_ERROR "SWITCH2_WII_IR_${field} must be an integer")
|
||||
endif()
|
||||
endforeach()
|
||||
math(EXPR offset_x_twice "2 * ${SWITCH2_WII_IR_OFFSET_X}")
|
||||
math(EXPR offset_y_twice "2 * ${SWITCH2_WII_IR_OFFSET_Y}")
|
||||
math(EXPR room_x "1024 - ${SWITCH2_WII_IR_VIEW_WIDTH}")
|
||||
math(EXPR room_y "768 - ${SWITCH2_WII_IR_VIEW_HEIGHT}")
|
||||
if(SWITCH2_WII_IR_VIEW_WIDTH LESS 1 OR SWITCH2_WII_IR_VIEW_WIDTH GREATER 1024 OR
|
||||
SWITCH2_WII_IR_VIEW_HEIGHT LESS 1 OR SWITCH2_WII_IR_VIEW_HEIGHT GREATER 768 OR
|
||||
offset_x_twice LESS -${room_x} OR offset_x_twice GREATER ${room_x} OR
|
||||
offset_y_twice LESS -${room_y} OR offset_y_twice GREATER ${room_y} OR
|
||||
SWITCH2_WII_IR_SPAN_X LESS 1 OR SWITCH2_WII_IR_SPAN_X GREATER 32767 OR
|
||||
SWITCH2_WII_IR_SPAN_Y LESS 1 OR SWITCH2_WII_IR_SPAN_Y GREATER 32767)
|
||||
message(FATAL_ERROR "Native IR viewport must lie inside the camera; mouse spans must be 1..32767")
|
||||
endif()
|
||||
target_compile_definitions(${target} PRIVATE
|
||||
SWITCH2_WII_IR_SCREEN_CONFIG=${SWITCH2_WII_IR_VIEW_WIDTH},${SWITCH2_WII_IR_VIEW_HEIGHT},${SWITCH2_WII_IR_OFFSET_X},${SWITCH2_WII_IR_OFFSET_Y},${SWITCH2_WII_IR_SPAN_X},${SWITCH2_WII_IR_SPAN_Y})
|
||||
endif()
|
||||
option(SWITCH2_PROBE_OMIT_NATIVE_IMU
|
||||
"Activation experiment: omit only native IMU bytes while retaining advertised features" OFF)
|
||||
option(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD
|
||||
"Activation experiment: zero native IMU payload while preserving its length" OFF)
|
||||
if(SWITCH2_PROBE_OMIT_NATIVE_IMU AND SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
|
||||
message(FATAL_ERROR "Select only one IMU activation experiment")
|
||||
endif()
|
||||
if(SWITCH2_PROBE_OMIT_NATIVE_IMU OR SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
|
||||
if(NOT SWITCH_PICO_SWITCH2_USB_BRIDGE)
|
||||
message(FATAL_ERROR "Native IMU activation experiments require the Bluetooth USB bridge")
|
||||
endif()
|
||||
if(SWITCH2_BRIDGE_WII_INPUT)
|
||||
message(FATAL_ERROR "Disable IMU ablation experiments for the Wii source bridge")
|
||||
endif()
|
||||
endif()
|
||||
if(SWITCH2_PROBE_OMIT_NATIVE_IMU)
|
||||
target_compile_definitions(${target} PRIVATE SWITCH2_PROBE_OMIT_NATIVE_IMU=1)
|
||||
elseif(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
|
||||
target_compile_definitions(${target} PRIVATE SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD=1)
|
||||
endif()
|
||||
|
||||
set(SWITCH2_PROBE_IDENTITY_FILE "" CACHE FILEPATH "64-byte Joy-Con 2 (R) factory-format identity block")
|
||||
if(SWITCH2_PROBE_IDENTITY_FILE)
|
||||
|
|
@ -130,10 +192,30 @@ function(switch2_usb_probe_configure target)
|
|||
hardware_flash pico_flash pico_mbedtls_crypto pico_mbedtls_headers tinyusb_device)
|
||||
pico_enable_stdio_usb(${target} 0)
|
||||
pico_enable_stdio_uart(${target} 1)
|
||||
if(SWITCH_PICO_SWITCH2_USB_BRIDGE)
|
||||
if(SWITCH2_BRIDGE_WII_INPUT)
|
||||
pico_set_program_name(${target} "Switch 2 Wii IR and native motion bridge")
|
||||
elseif(SWITCH_PICO_SWITCH2_USB_BRIDGE)
|
||||
pico_set_program_name(${target} "Switch 2 right Joy-Con Bluetooth bridge")
|
||||
else()
|
||||
pico_set_program_name(${target} "Switch 2 USB initialization capture")
|
||||
endif()
|
||||
pico_set_program_version(${target} "0.24")
|
||||
if(SWITCH2_PROBE_OMIT_NATIVE_IMU)
|
||||
pico_set_program_version(${target} "0.24-no-imu")
|
||||
elseif(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
|
||||
pico_set_program_version(${target} "0.24-zero-imu-payload")
|
||||
elseif(SWITCH2_BRIDGE_WII_INPUT)
|
||||
if(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
||||
pico_set_program_version(${target} "0.33-wii-trace")
|
||||
else()
|
||||
pico_set_program_version(${target} "0.33-wii")
|
||||
endif()
|
||||
elseif(SWITCH_PICO_SWITCH2_USB_BRIDGE)
|
||||
if(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
||||
pico_set_program_version(${target} "0.25-trace")
|
||||
else()
|
||||
pico_set_program_version(${target} "0.25")
|
||||
endif()
|
||||
else()
|
||||
pico_set_program_version(${target} "0.24")
|
||||
endif()
|
||||
endfunction()
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue