From 1f9ec889053760090c7d0b5053f05d7cdfb54379 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Thu, 19 May 2022 16:04:23 +0800
Subject: [PATCH] add controlling client for gy-801

---
 .gitignore                 |   1 +
 Makefile                   |   4 +-
 cmd/motion-control/main.ha | 115 +++++++++++++++++++++++++++++++++++++
 3 files changed, 119 insertions(+), 1 deletion(-)
 create mode 100644 cmd/motion-control/main.ha

diff --git a/.gitignore b/.gitignore
index 6274f75..b8c8030 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@
 /gy-801-dump
 /pointerc
 /pointerd
+/motion-control
diff --git a/Makefile b/Makefile
index aa71dc1..0521aa9 100644
--- a/Makefile
+++ b/Makefile
@@ -4,4 +4,6 @@ gy-801-dump:
 	hare build -o $@ tools/$@/
 pointerd:
 	hare build -levdev -o $@ cmd/$@/
-.PHONY: evdev-dump-events uinput-pointer gy-801-dump pointerd pointerc
+motion-control:
+	hare build -o $@ cmd/$@/
+.PHONY: evdev-dump-events uinput-pointer gy-801-dump pointerd pointerc motion-control
diff --git a/cmd/motion-control/main.ha b/cmd/motion-control/main.ha
new file mode 100644
index 0000000..fb74c43
--- /dev/null
+++ b/cmd/motion-control/main.ha
@@ -0,0 +1,115 @@
+use ahrs::complementary;
+use fmt;
+use fs;
+use io;
+use linux::timerfd;
+use math;
+use net;
+use net::ip;
+use net::udp;
+use os;
+use proto::control;
+use rt;
+use sensors::ADXL345;
+use sensors::L3G4200D;
+use strconv;
+use time;
+use unix::signal;
+
+let exit: bool	= false;
+
+fn sigint(sig: int, info: *signal::siginfo, ucontext: *void) void = {
+	exit = true;
+};
+
+export fn main() int = {
+	if (len(os::args) != 4) {
+		fmt::fatalf("Usage: {} bus addr port", os::args[0]);
+	};
+	const bus = match (strconv::stoi(os::args[1])) {
+	case let bus: int =>
+		yield bus;
+	case =>
+		fmt::fatal("bus should be a integer");
+	};
+	const addr = match (ip::parse(os::args[2])) {
+	case let addr: ip::addr =>
+		yield addr;
+	case ip::invalid =>
+		fmt::fatalf("Invalid address {}", os::args[1]);
+	};
+	const port = match (strconv::stou16(os::args[3])) {
+	case let port: u16 =>
+		yield port;
+	case =>
+		fmt::fatalf("Invalid port {}", os::args[2]);
+	};
+
+	const buf = control::request {
+		code = control::code::RELATIVE_MOVEMENT,
+		...,
+	};
+
+	const sock = match (udp::connect(addr, port)) {
+	case let s: io::file =>
+		yield s;
+	case let err: net::error =>
+		fmt::fatalf("Cannot connect() {}:{}: {}", ip::string(addr), port, net::strerror(err));
+	};
+	defer io::close(sock)!;
+
+	const ADXL345 = match (ADXL345::new(bus)) {
+	case let ADXL345: ADXL345::ADXL345 =>
+		yield ADXL345;
+	case let err: fs::error =>
+		fmt::fatalf("Cannot open i2c device: {}", fs::strerror(err));
+	case let err: rt::errno =>
+		fmt::fatalf("Cannot ioctl i2c device: {}", rt::strerror(err));
+	};
+	defer ADXL345::destroy(ADXL345);
+
+	const L3G4200D = match (L3G4200D::new(bus)) {
+	case let L3G4200D: L3G4200D::L3G4200D =>
+		yield L3G4200D;
+	case let err: fs::error =>
+		fmt::fatalf("Cannot open i2c device: {}", fs::strerror(err));
+	case let err: rt::errno =>
+		fmt::fatalf("Cannot ioctl i2c device: {}", rt::strerror(err));
+	};
+	defer L3G4200D::destroy(L3G4200D);
+
+	ADXL345::init(ADXL345)!;
+	L3G4200D::init(L3G4200D)!;
+
+	const fd = timerfd::new(time::clock::MONOTONIC, 0)!;
+	defer io::close(fd)!;
+	timerfd::set(fd, (10 * time::MILLISECOND): timerfd::interval, 0)!;
+	signal::handle(signal::SIGINT, &sigint);
+
+	timerfd::read(fd)!;
+	const acc_readings = ADXL345::read(ADXL345)!;
+	const complementary = complementary::new(acc_readings, 0.02);
+
+	for (let i = 0; !exit; i += 1) {
+		timerfd::read(fd)!;
+		const gyro_readings = L3G4200D::read(L3G4200D)!;
+		const acc_readings = ADXL345::read(ADXL345)!;
+		let rps = (gyro_readings.0 / 180.0 * math::PI,
+			gyro_readings.1 / 180.0 * math::PI,
+			gyro_readings.2 / 180.0 * math::PI);
+		complementary::update(complementary, rps, 0.01);
+		complementary::update_accelerometer(complementary, acc_readings);
+
+		let attitude = complementary::read_euler(complementary);
+		buf.x = ((attitude.2 / math::PI * 180.0): int / 8): i8;
+		buf.y = ((attitude.1 / math::PI * 180.0): int / 8): i8;
+		match (udp::send(sock, buf.bytes)) {
+		case let err: net::error =>
+			fmt::fatalf("Cannot send(): {}", net::strerror(err));
+		case size =>
+			yield;
+		};
+	};
+
+	return 0;
+};
-- 
2.45.2