From 655439461aef49940300dc55f56264fe162ce520 Mon Sep 17 00:00:00 2001
From: Djeeberjr <djeeberjr@gmail.com>
Date: Thu, 13 Apr 2023 22:26:37 +0200
Subject: [PATCH] updated GPS bin with INS

---
 src/bin/GPS.hx | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/src/bin/GPS.hx b/src/bin/GPS.hx
index edd90a3..01bbb19 100644
--- a/src/bin/GPS.hx
+++ b/src/bin/GPS.hx
@@ -1,5 +1,6 @@
 package bin;
 
+import kernel.gps.INS;
 import lib.Pos3;
 import lib.Vec.Vec3;
 import lib.cli.TermHandle;
@@ -26,8 +27,11 @@ class GPS extends CLIApp {
 			case "locate":
 				kernel.gps.GPS.instance.locate();
 				return Future.sync(true);
+			case "ins":
+				return INS.instance.align().isSuccess();
 			default:
 				handle.writeLn("Unknown subcommand: " + subcommand);
+				printHelp();
 				return Future.sync(false);
 
 		}
@@ -35,6 +39,14 @@ class GPS extends CLIApp {
 		return Future.sync(true);
 	}
 
+	private function printHelp(){
+		handle.writeLn("GPS commands:");
+		handle.writeLn("set <x> <y> <z> - set manual position");
+		handle.writeLn("status - get current position and accuracy");
+		handle.writeLn("locate - get current position");
+		handle.writeLn("ins - align INS");
+	}
+
 	private function setManuelPos(args: Array<String>): Bool {
 		var x: Float = Std.parseFloat(args[0]);
 		var y: Float = Std.parseFloat(args[1]);
@@ -66,6 +78,13 @@ class GPS extends CLIApp {
 			handle.writeLn("Accuracy: High");
 		}
 
+		var ins = INS.instance.getHeading();
+		if (ins != null) {
+			handle.writeLn('INS heading: ${ins.x} y:${ins.y} z:${ins.z}');
+		} else {
+			handle.writeLn("INS heading not available");
+		}
+
 		return true;
 	}
 }