added a timeout and improvements to GPS and INS

closes #6

src/bin/GPS.hx

@@ -51,11 +51,12 @@ class GPS extends CLIAppBase {
 		});
 
 		registerAsyncSubcommand("locate", (args) -> {
-			return kernel.gps.GPS.locate().map((pos) -> {
-				if (pos != null) {
-					handle.writeLine('Position x:${pos.x} y:${pos.y} z:${pos.z}');
-				} else {
-					handle.writeLine("Position not available");
+			return kernel.gps.GPS.locate().map((result) -> {
+				switch result {
+					case Success(pos):
+						handle.writeLine('Position x:${pos.x} y:${pos.y} z:${pos.z}');
+					case Failure(err):
+						handle.writeLine("Position not available: " + err);
 				}
 				return true;
 			});

src/kernel/gps/GPS.hx

@@ -1,5 +1,6 @@
 package kernel.gps;
 
+import lib.SingleTimeoutPromise;
 import kernel.log.Log;
 import lib.KVStore;
 import kernel.net.Net;
@@ -15,13 +16,15 @@ using tink.CoreApi;
 	You need at least 3 computers that know their position to determine the position of the computer.
 **/
 class GPS {
+	private static inline final TIMEOUT:Int = 1;
+
 	private static var shouldRespond = true;
 	private static var shouldDoWholeNumberCheck = true;
 	private static var posAccuracy = 0; // 0 = unkown, 1 = (ins,best guess), 2 = (stored/manual,should be right), 3 = (gps,confirmed)
 	private static var cachedPosition:WorldPos;
 	private static var lastPositionResponse:Array<{pos:WorldPos, dist:Float}> = [];
 
-	private static var futureResolve:(pos:Null<WorldPos>) -> Void = null;
+	private static final locatePromise:SingleTimeoutPromise<WorldPos> = new SingleTimeoutPromise(TIMEOUT, brodcastPositionRequest);
 
 	@:allow(kernel.Init)
 	private static function init() {
@@ -58,18 +61,8 @@ class GPS {
 		posAccuracy = 0;
 	}
 
-	public static function locate():Future<Null<WorldPos>> {
-		// TODO: implenet a timeout
-		// TODO: dont send a request twice if the last one is still pending or we moved
-		return new Future<Null<WorldPos>>((resolve) -> {
-			futureResolve = resolve;
-			sendPositionRequest();
-			return null;
-		});
-	}
-
-	private static function resolveFuture(pos:Null<WorldPos>) {
-		futureResolve(pos);
+	public static function locate():Promise<WorldPos> {
+		return locatePromise.request();
 	}
 
 	private static function persistCachedPositon() {
@@ -117,7 +110,7 @@ class GPS {
 		}
 	}
 
-	private static function sendPositionRequest() {
+	private static function brodcastPositionRequest() {
 		Net.brodcastGPSRequest();
 	}
 
@@ -132,13 +125,13 @@ class GPS {
 				if (cachedPosition == null)
 					return;
 
-				var response = new Package(Net.networkID, pack.fromID, pack.msgID, GPSResponse(cachedPosition), null, 0);
+				var response = new Package(Net.networkID, pack.fromID, pack.msgID, GPSResponse(cachedPosition.x, cachedPosition.y, cachedPosition.z,), null, 0);
 				iface.send(pack.fromID, Net.networkID, response);
-			case GPSResponse(pos):
-				if (lastPositionResponse.contains({pos: pos, dist: dist}))
+			case GPSResponse(x, y, z):
+				if (lastPositionResponse.contains({pos: {x: x, y: y, z: z}, dist: dist}))
 					return; // Ignore duplicate responses
 
-				lastPositionResponse.push({pos: pos, dist: dist});
+				lastPositionResponse.push({pos: {x: x, y: y, z: z}, dist: dist});
 
 				// TODO: wait for a few seconds before calculating the position, so we can get more responses
 
@@ -158,7 +151,7 @@ class GPS {
 				cachedPosition = calculatedPosition;
 				posAccuracy = 3;
 
-				resolveFuture(calculatedPosition);
+				locatePromise.resolve(calculatedPosition);
 			default:
 		}
 	}
@@ -228,7 +221,7 @@ class GPS {
 		var i = ex.dot(a2c);
 		var ey = (a2c - ex * i).normalize();
 		var j = ey.dot(a2c);
-		var ez = ex.cross(ey);
+		var ez:WorldPos = ex.cross(ey);
 
 		var x = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
 		var y = (r1 * r1 - r3 * r3 - x * x + (x - i) * (x - i) + j * j) / (2 * j);

src/kernel/gps/INS.hx

@@ -1,5 +1,6 @@
 package kernel.gps;
 
+import lib.SinglePromise;
 import kernel.log.Log;
 import kernel.turtle.Turtle;
 import lib.WorldPos;
@@ -9,6 +10,7 @@ using tink.CoreApi;
 class INS {
 	private static var heading:Null<WorldPos> = null;
 	private static var alingment:Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned
+	private static final alignPromise:SinglePromise<Noise> = new SinglePromise(startAlign);
 
 	@:allow(kernel.turtle.Turtle)
 	private static function moveForward() {
@@ -81,7 +83,10 @@ class INS {
 	}
 
 	public static function align():Promise<Noise> {
-		Log.info("Aligning INS");
+		return alignPromise.request();
+	}
+
+	public static function startAlign():Promise<Noise> {
 		return new Promise<Noise>((resolve, reject) -> {
 			if (Turtle.getFuelLevel() < 2) {
 				Log.warn("Not enough fuel to align");
@@ -89,43 +94,43 @@ class INS {
 				return null;
 			}
 
-			GPS.locate().handle((pos1) -> {
-				Log.debug('pos1: $pos1');
-				if (pos1 == null) {
-					Log.warn("GPS not available for 1st position");
-					reject(new Error("GPS not available"));
-					return;
-				}
-
-				var moved = tryMoving();
-
-				if (moved == -1) {
-					Log.warn("Can't move");
-					reject(new Error("Can't move"));
-					return;
-				}
-
-				GPS.locate().handle((pos2) -> {
-					Log.debug('pos2: $pos2');
-					if (pos2 == null) {
-						Log.warn("GPS not available for 2nd position");
-						reject(new Error("GPS not available for 2nd position"));
+			GPS.locate().handle((result) -> {
+				switch result {
+					case Failure(err):
+						Log.warn("GPS not available for 1st position");
+						reject(new Error("GPS not available"));
 						return;
-					}
+					case Success(pos1):
+						var moved = tryMoving();
 
-					var cHeading = calcHeading(pos1, pos2, moved);
-					if (cHeading == null) {
-						Log.error("Can't calculate heading");
-						reject(new Error("Can't calculate heading"));
-						return;
-					}
+						if (moved == -1) {
+							Log.warn("Can't move");
+							reject(new Error("Can't move"));
+							return;
+						}
 
-					heading = cHeading;
-					moveBack(moved);
-					GPS.setINSPosition(pos1);
+						GPS.locate().handle((result) -> {
+							switch result {
+								case Failure(err):
+									Log.warn("GPS not available for 2nd position");
+									reject(new Error("GPS not available for 2nd position"));
+									return;
+								case Success(pos2):
+									var cHeading = calcHeading(pos1, pos2, moved);
+									if (cHeading == null) {
+										Log.error("Can't calculate heading");
+										reject(new Error("Can't calculate heading"));
+										return;
+									}
 
-					resolve(Noise);
-				});
+									heading = cHeading;
+									moveBack(moved);
+									GPS.setINSPosition(pos1);
+
+									resolve(Noise);
+							}
+						});
+				}
 			});
 			return null;
 		});

src/kernel/net/Package.hx

@@ -1,7 +1,5 @@
 package kernel.net;
 
-import lib.WorldPos;
-
 typedef NetworkID = Int;
 typedef GenericPackage = Package<Dynamic>;
 
@@ -12,7 +10,7 @@ enum PackageTypes {
 	RouteDiscover(reachableIDs:Array<{id:NetworkID, cost:Int}>);
 	RouteDiscoverResponse(reachableIDs:Array<{id:NetworkID, cost:Int}>);
 	RouteDiscoverUpdate(reachableIDs:Array<{id:NetworkID, cost:Int}>);
-	GPSResponse(pos:WorldPos);
+	GPSResponse(x:Float, y:Float, z:Float);
 	GPSRequest();
 }