From aac527ae89b2fb0c6b8a769635eff5af7c261e3a Mon Sep 17 00:00:00 2001 From: Niklas Kapelle Date: Wed, 16 Oct 2024 23:45:20 +0200 Subject: [PATCH] added a timeout and improvements to GPS and INS closes #6 --- src/bin/GPS.hx | 11 +++--- src/kernel/gps/GPS.hx | 33 +++++++----------- src/kernel/gps/INS.hx | 73 +++++++++++++++++++++------------------ src/kernel/net/Package.hx | 4 +-- 4 files changed, 59 insertions(+), 62 deletions(-) diff --git a/src/bin/GPS.hx b/src/bin/GPS.hx index 9d9663c..0f25cc5 100644 --- a/src/bin/GPS.hx +++ b/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; }); diff --git a/src/kernel/gps/GPS.hx b/src/kernel/gps/GPS.hx index f2c2a1f..04d8e16 100644 --- a/src/kernel/gps/GPS.hx +++ b/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) -> Void = null; + private static final locatePromise:SingleTimeoutPromise = new SingleTimeoutPromise(TIMEOUT, brodcastPositionRequest); @:allow(kernel.Init) private static function init() { @@ -58,18 +61,8 @@ class GPS { posAccuracy = 0; } - public static function locate():Future> { - // TODO: implenet a timeout - // TODO: dont send a request twice if the last one is still pending or we moved - return new Future>((resolve) -> { - futureResolve = resolve; - sendPositionRequest(); - return null; - }); - } - - private static function resolveFuture(pos:Null) { - futureResolve(pos); + public static function locate():Promise { + 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); diff --git a/src/kernel/gps/INS.hx b/src/kernel/gps/INS.hx index d50667a..c492a11 100644 --- a/src/kernel/gps/INS.hx +++ b/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 = null; private static var alingment:Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned + private static final alignPromise:SinglePromise = new SinglePromise(startAlign); @:allow(kernel.turtle.Turtle) private static function moveForward() { @@ -81,7 +83,10 @@ class INS { } public static function align():Promise { - Log.info("Aligning INS"); + return alignPromise.request(); + } + + public static function startAlign():Promise { return new Promise((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; }); diff --git a/src/kernel/net/Package.hx b/src/kernel/net/Package.hx index 9774e3c..6b40831 100644 --- a/src/kernel/net/Package.hx +++ b/src/kernel/net/Package.hx @@ -1,7 +1,5 @@ package kernel.net; -import lib.WorldPos; - typedef NetworkID = Int; typedef GenericPackage = Package; @@ -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(); }