From 2bb50861d402d202903da3060b1f667b477c5f83 Mon Sep 17 00:00:00 2001 From: xdavidwu Date: Mon, 4 Jul 2022 14:22:15 +0800 Subject: [PATCH] implement sensors reading for joycon --- Makefile | 9 +- sensors/joycon/joycon.ha | 146 ++++++++++++++++++++++++++++++ sensors/joycon/types.ha | 25 +++++ tools/sensors-dump/main+joycon.ha | 68 ++++++++++++++ 4 files changed, 246 insertions(+), 2 deletions(-) create mode 100644 sensors/joycon/joycon.ha create mode 100644 sensors/joycon/types.ha create mode 100644 tools/sensors-dump/main+joycon.ha diff --git a/Makefile b/Makefile index 7ca3324..f17aa93 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,12 @@ SENSORS ?= gy801 +SENSORS_LIBS = HEADERS_PREFIX ?= /usr/include BINARIES = evdev-dump-events uinput-pointer sensors-dump pointerd pointerc motion-control buttonc +ifeq ($(SENSORS), joycon) + SENSORS_LIBS = -levdev +endif + event-codes: hare build -o $@ aux/$@/ evdev/codes.ha: event-codes @@ -10,13 +15,13 @@ evdev/codes.ha: event-codes evdev-dump-events uinput-pointer: evdev/codes.ha hare build -levdev -o $@ tools/$@/ sensors-dump pointerc: - hare build -T +$(SENSORS) -o $@ tools/$@/ + hare build $(SENSORS_LIBS) -T +$(SENSORS) -o $@ tools/$@/ buttonc: hare build -o $@ tools/$@/ pointerd: evdev/codes.ha hare build -levdev -o $@ cmd/$@/ motion-control: - hare build -T +$(SENSORS) -o $@ cmd/$@/ + hare build $(SENSORS_LIBS) -T +$(SENSORS) -o $@ cmd/$@/ .PHONY: $(BINARIES) all: $(BINARIES) clean: diff --git a/sensors/joycon/joycon.ha b/sensors/joycon/joycon.ha new file mode 100644 index 0000000..b8e01ce --- /dev/null +++ b/sensors/joycon/joycon.ha @@ -0,0 +1,146 @@ +use errors; +use evdev; +use fmt; +use fs; +use io; +use os; +use strings; +use types; + +export fn new(dev: (io::file | void)) (joycon | errors::error) = { + match (dev) { + case let f: io::file => + return alloc(joycon_impl { dev = evdev::new_from_fd(f)?, ... }); + case void => + const preferences: []const str = [ + "Nintendo Switch Pro Controller IMU", + "Nintendo Switch Right Joy-Con IMU (Grip)", + "Nintendo Switch Left Joy-Con IMU (Grip)", + "Nintendo Switch Right Joy-Con IMU", + "Nintendo Switch Left Joy-Con IMU", + ]; + const fs = os::diropen("/dev/input")!; + const iter = fs::iter(fs, ".")!; + let candidate = struct { + fd: io::file = 0, + dev: evdev::libevdev = 0: uintptr, + preference: size = len(preferences), + }; + let has_candidate = false; + + for (true) { + const ent = match (fs::next(iter)) { + case let e: fs::dirent => + if (!fs::ischdev(e.ftype)) { + continue; + }; + yield e.name; + case void => + break; + }; + const f = match (fs::open_file(fs, ent)) { + case let f: io::file => + yield f; + case let e: fs::error => + fmt::errorfln("Cannot probe {}: {}", ent, fs::strerror(e))!; + continue; + }; + const e = evdev::new_from_fd(f)!; + const name = evdev::get_name(e); + + let eligible = false; + for (let i = 0z; i < candidate.preference; i += 1) { + if (strings::compare(name, preferences[i]) == 0) { + has_candidate = true; + eligible = true; + candidate.fd = f; + candidate.dev = e; + candidate.preference = i; + break; + }; + }; + + if (!eligible) { + evdev::destroy(e); + io::close(f)!; + }; + }; + fs::finish(iter); + + if (!has_candidate) { + return errors::noentry; + }; + + fmt::printfln("Picking {}", preferences[candidate.preference])!; + return alloc(joycon_impl { + fd = candidate.fd, + dev = candidate.dev, + ... + }); + }; +}; + +export fn destroy(joycon: joycon) void = { + evdev::destroy(joycon.dev); + if (joycon.fd != 0) { + io::close(joycon.fd)!; + }; + free(joycon); +}; + +// +-8g, 16-bit signed +def ACCEL_SCALE: f64 = 8.0 / types::I16_MAX: f64; + +// +-2000dps, 16-bit signed, hid-nintendo * 1000 for calibration precision +def GYRO_SCALE: f64 = 2.0 / types::I16_MAX: f64; + +export fn next_sample(joycon: joycon) (sample | errors::error) = { + let ev = evdev::input_event {...}; + for (true) { + switch (evdev::next_event(joycon.dev, evdev::read_flag::NORMAL, &ev)?) { + case evdev::read_status::SUCCESS => + yield; + case evdev::read_status::SYNC => + continue; + }; + switch (ev._type) { + case evdev::EV_SYN => + if (joycon.read == READ_FULL) { + return joycon.state; + } else { + continue; + }; + case evdev::EV_ABS => + switch (ev.code) { + case evdev::ABS_X => + joycon.read |= READ_X; + joycon.state.accelerometer.0 = + ev.value: f64 * ACCEL_SCALE; + case evdev::ABS_Y => + joycon.read |= READ_Y; + joycon.state.accelerometer.1 = + ev.value: f64 * ACCEL_SCALE; + case evdev::ABS_Z => + joycon.read |= READ_Z; + joycon.state.accelerometer.2 = + ev.value: f64 * ACCEL_SCALE; + case evdev::ABS_RX => + joycon.read |= READ_RX; + joycon.state.gyroscope.0 = + ev.value: f64 * GYRO_SCALE; + case evdev::ABS_RY => + joycon.read |= READ_RY; + joycon.state.gyroscope.1 = + ev.value: f64 * GYRO_SCALE; + case evdev::ABS_RZ => + joycon.read |= READ_RZ; + joycon.state.gyroscope.2 = + ev.value: f64 * GYRO_SCALE; + }; + case evdev::EV_MSC => + assert(ev.code == evdev::MSC_TIMESTAMP); + joycon.state.timestamp = ev.value: u32; + }; + }; + abort("Unreachable"); +}; diff --git a/sensors/joycon/types.ha b/sensors/joycon/types.ha new file mode 100644 index 0000000..0bf35c2 --- /dev/null +++ b/sensors/joycon/types.ha @@ -0,0 +1,25 @@ +use evdev; +use io; + +export type sample = struct { + timestamp: u32, + gyroscope: (f64, f64, f64), + accelerometer: (f64, f64, f64), +}; + +export type joycon_impl = struct { + fd: io::file, + dev: evdev::libevdev, + state: sample, + read: u8, +}; + +def READ_X: u8 = 1; +def READ_Y: u8 = 1 << 1; +def READ_Z: u8 = 1 << 2; +def READ_RX: u8 = 1 << 3; +def READ_RY: u8 = 1 << 4; +def READ_RZ: u8 = 1 << 5; +def READ_FULL: u8 = (1 << 6) - 1; + +export type joycon = *joycon_impl; diff --git a/tools/sensors-dump/main+joycon.ha b/tools/sensors-dump/main+joycon.ha new file mode 100644 index 0000000..3498996 --- /dev/null +++ b/tools/sensors-dump/main+joycon.ha @@ -0,0 +1,68 @@ +use ahrs::complementary; +use ahrs::tilt; +use ahrs::integration; +use errors; +use fmt; +use fmt; +use math; +use sensors::joycon; +use unix::signal; + +let exit: bool = false; + +fn sigint(sig: int, info: *signal::siginfo, ucontext: *void) void = { + exit = true; +}; + +export fn main() void = { + const joycon = joycon::new(void)!; + defer joycon::destroy(joycon); + + signal::handle(signal::SIGINT, &sigint, signal::flag::RESTART); + + const sample = joycon::next_sample(joycon)!; + let prev_us = sample.timestamp; + + const acc_readings = sample.accelerometer; + fmt::printfln("Neutral acc.: {} {} {} g", acc_readings.0, acc_readings.1, acc_readings.2)!; + const integrator = integration::new(); + const tilt = tilt::new(acc_readings); + const complementary = complementary::new(acc_readings, 0.02); + + for (let i = 0; !exit; i += 1) { + const sample = joycon::next_sample(joycon)!; + const gyro_readings = sample.gyroscope; + const acc_readings = sample.accelerometer; + let rps = (gyro_readings.0 / 180.0 * math::PI, + gyro_readings.1 / 180.0 * math::PI, + gyro_readings.2 / 180.0 * math::PI); + const dt = (sample.timestamp - prev_us): f64 / 1000000.0; + integration::update(integrator, rps, dt); + complementary::update(complementary, rps, dt); + complementary::update_accelerometer(complementary, acc_readings); + prev_us = sample.timestamp; + + if (i % 50 == 0) { + let attitude = integration::read_euler(integrator); + let tilt_attitude = tilt::estimate_euler(tilt, acc_readings); + let complementary_attitude = complementary::read_euler(complementary); + + fmt::printfln("Accelerometer: {} {} {} g", + acc_readings.0, acc_readings.1, acc_readings.2)!; + fmt::printfln("Gyroscope: {} {} {} dps", + gyro_readings.0, gyro_readings.1, gyro_readings.2)!; + fmt::printfln("Attitude: {} {} {} deg", + attitude.0 / math::PI * 180.0, + attitude.1 / math::PI * 180.0, + attitude.2 / math::PI * 180.0)!; + fmt::printfln("Attitude (tilt): {} {} {} deg", + tilt_attitude.0 / math::PI * 180.0, + tilt_attitude.1 / math::PI * 180.0, + tilt_attitude.2 / math::PI * 180.0)!; + fmt::printfln("Attitude (complementary): {} {} {} deg", + complementary_attitude.0 / math::PI * 180.0, + complementary_attitude.1 / math::PI * 180.0, + complementary_attitude.2 / math::PI * 180.0)!; + }; + }; +}; -- 2.45.2