M Makefile => Makefile +7 -2
@@ 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:
A sensors/joycon/joycon.ha => sensors/joycon/joycon.ha +146 -0
@@ 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");
+};
A sensors/joycon/types.ha => sensors/joycon/types.ha +25 -0
@@ 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;
A tools/sensors-dump/main+joycon.ha => tools/sensors-dump/main+joycon.ha +68 -0
@@ 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)!;
+ };
+ };
+};