From 4f8db600dcc9eddd61dd518edfff21b8babae887 Mon Sep 17 00:00:00 2001 From: Djeeberjr Date: Tue, 28 Mar 2023 00:56:49 +0200 Subject: [PATCH] added GPS --- src/kernel/Init.hx | 8 ++ src/kernel/gps/GPS.hx | 219 ++++++++++++++++++++++++++++++++++++++++++ src/kernel/net/Net.hx | 26 ++++- 3 files changed, 252 insertions(+), 1 deletion(-) create mode 100644 src/kernel/gps/GPS.hx diff --git a/src/kernel/Init.hx b/src/kernel/Init.hx index 14ffbc3..22ca02a 100644 --- a/src/kernel/Init.hx +++ b/src/kernel/Init.hx @@ -1,5 +1,7 @@ package kernel; +import kernel.fs.FS; +import kernel.gps.GPS; import kernel.log.Log; import kernel.turtle.Turtle; import haxe.MainLoop; @@ -27,6 +29,8 @@ class Init { Routing.instance = new Routing(); Net.instance = new Net(); + GPS.instance = new GPS(); + // Register default terminate handler KernelEvents.instance.onTerminate.handle(_->{ OS.reboot(); @@ -36,6 +40,10 @@ class Init { Routing.instance.init(); + if (!FS.exists("/var/ns")) { + FS.makeDir("/var/ns"); + } + MainLoop.add(()->{ KernelEvents.instance.startEventLoop(); }); diff --git a/src/kernel/gps/GPS.hx b/src/kernel/gps/GPS.hx new file mode 100644 index 0000000..14a8d08 --- /dev/null +++ b/src/kernel/gps/GPS.hx @@ -0,0 +1,219 @@ +package kernel.gps; + +import lib.KVStore; +import kernel.net.Net; +import kernel.net.INetworkInterface; +import kernel.net.Package; +import lib.Pos3; + +/** + Determines the position of the computer based on the distance to other computers. + When receiving a message from another computer via wireless, the distance is also received. + You need at least 3 computers that know their position to determine the position of the computer. +**/ +class GPS { + public static var instance:GPS; + + private var shouldRespond = true; + private var posAccuracy = 0; // 0 = unkown, 1 = (ins,best guess), 2 = (stored/manual,should be right), 3 = (gps,confirmed) + private var cachedPosition:Pos3; + private var lastPositionResponse: Array<{pos:Pos3,dist:Float}> = []; + + @:allow(kernel.Init) + private function new() { + this.loadCachedPosition(); + } + + public function setManualPosition(pos:Pos3) { + var kvstore = new KVStore("gps"); + kvstore.set("mpos",pos); + kvstore.save(); + + if (cachedPosition == null) { + cachedPosition = pos; + posAccuracy = 2; + } + } + + public function getPosition():Null { + return cachedPosition; + } + + public function getAccuracy():Int { + return posAccuracy; + } + + public function invalidatePosition() { + cachedPosition = null; + posAccuracy = 0; + } + + public function locate() { + sendPositionRequest(); + } + + private function persistCachedPositon() { + if (cachedPosition == null) return; + var kvstore = new KVStore("gps"); + + kvstore.set("cpos",cachedPosition); + kvstore.save(); + } + + private function loadCachedPosition() { + var kvstore = new KVStore("gps"); + kvstore.load(); + + var mPos:Null = kvstore.get("mpos"); // Manual set position + var cPos:Null = kvstore.get("cpos"); // Cached position + + if (mPos != null && cPos != null && mPos == cPos) { + // Both are the same, so we can use the cached position + cachedPosition = mPos; + posAccuracy = 3; + return; + } + + if (mPos != null && cPos != null && mPos != cPos){ + // Both are different, so we can use the manual position + cachedPosition = mPos; + posAccuracy = 1; + return; + } + + if (mPos == null && cPos != null) { + // No manual position set, so we can use the cached position + cachedPosition = cPos; + posAccuracy = 2; + return; + } + + if (mPos != null && cPos == null) { + // No cached position, so we can use the manual position + cachedPosition = mPos; + posAccuracy = 2; + return; + } + + } + + private function sendPositionRequest() { + Net.instance.brodcastGPSRequest(); + } + + @:allow(kernel.net.Net) + private function handlePackage(pack:Package, dist: Float,iface: INetworkInterface) { + switch (pack.type) { + case GPSRequest: + if (!shouldRespond) return; + if (posAccuracy < 2) return; + if (cachedPosition == null) return; + + var response = new Package(Net.instance.networkID,pack.fromID, pack.msgID, GPSResponse(cachedPosition),null,0); + iface.send(pack.fromID,Net.instance.networkID,response); + case GPSResponse(pos): + if (lastPositionResponse.contains({pos:pos,dist:dist})) return; + lastPositionResponse.push({pos:pos,dist:dist}); + if (lastPositionResponse.length > 5) lastPositionResponse.shift(); + if (lastPositionResponse.length < 3) return; + + var calculatedPosition = calculatePosition(); + if (calculatedPosition == null) return; + cachedPosition = calculatedPosition; + posAccuracy = 3; + default: + } + } + + private function calculatePosition():Null { + if (lastPositionResponse.length < 3) return null; + + // do a simple trilateration with the last 3 responses for now + var p1 = lastPositionResponse[0].pos; + var p2 = lastPositionResponse[1].pos; + var p3 = lastPositionResponse[2].pos; + + var r1 = lastPositionResponse[0].dist; + var r2 = lastPositionResponse[1].dist; + var r3 = lastPositionResponse[2].dist; + + return trilateration(p1,p2,p3,r1,r2,r3); + + // var calculatedPositions: Array = []; + + // // Loop through all possible permutations of the last 3 responses + // for (i in 0...lastPositionResponse.length) { + // for (j in 0...lastPositionResponse.length) { + // if (i == j) continue; + // for (k in 0...lastPositionResponse.length) { + // if (i == k || j == k) continue; + + // var p1 = lastPositionResponse[i].pos; + // var p2 = lastPositionResponse[j].pos; + // var p3 = lastPositionResponse[k].pos; + + // var r1 = lastPositionResponse[i].dist; + // var r2 = lastPositionResponse[j].dist; + // var r3 = lastPositionResponse[k].dist; + + // calculatedPositions.push(trilateration(p1,p2,p3,r1,r2,r3)); + // } + // } + // } + + // var tainted = false; + // // Check if all calculated positions are the same + // for (i in 0...calculatedPositions.length) { + // for (j in 0...calculatedPositions.length) { + // if (i == j) continue; + // if (calculatedPositions[i] != calculatedPositions[j]){ + // Log.warn("GPS not all calculated positions are the same"); + // tainted = true; + // } + // } + // } + + // if (!tainted) return calculatedPositions[0]; + + // // Get the most common position + // var mostCommon:Pos3 = null; + // var mostCommonCount = 0; + // for (i in 0...calculatedPositions.length) { + // var count = 0; + // for (j in 0...calculatedPositions.length) { + // if (calculatedPositions[i] == calculatedPositions[j]) count++; + // } + // if (count > mostCommonCount) { + // mostCommon = calculatedPositions[i]; + // mostCommonCount = count; + // } + // } + + // return mostCommon; + } + + private function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pos3 { + var a2b = p2 - p1; + var a2c = p3 - p1; + + var d = a2b.length(); + var ex = a2b.normalize(); + var i = ex.dot(a2c); + var ey = (a2c - ex * i).normalize(); + var j = ey.dot(a2c); + var ez = 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); + + var result = p1 + ex * x + ey * y; + + var zSquared = r1 * r1 - x * x - y * y; + if (zSquared > 0) { + var z = Math.sqrt(zSquared); + result += ez * z; + } + + return result; + } +} diff --git a/src/kernel/net/Net.hx b/src/kernel/net/Net.hx index a6489ca..6d98729 100644 --- a/src/kernel/net/Net.hx +++ b/src/kernel/net/Net.hx @@ -1,5 +1,6 @@ package kernel.net; +import kernel.gps.GPS; import haxe.ds.ReadOnlyArray; import kernel.net.Package.NetworkID; import kernel.peripherals.Peripherals.Peripheral; @@ -9,7 +10,6 @@ import cc.OS; using tink.CoreApi; using Lambda; -using lib.Extender.LambdaExtender; /** Class responsible for everything network related. @@ -82,6 +82,13 @@ class Net { case RouteDiscover(_) | RouteDiscoverResponse(_) | RouteDiscoverUpdate(_): // Delegate to Routing Routing.instance.handleRoutePackage(pack,interf); + case GPSRequest | GPSResponse(_): + if (dist == null) { + Log.silly("Got a GPS package but no distance was provided"); + return; + } + // Delegate to GPS + GPS.instance.handlePackage(pack,dist,interf); } }else{ // New message received but its not ment for us. Forward if possible. @@ -260,4 +267,21 @@ class Net { return arr; } + + @:allow(kernel.gps.GPS) + private function brodcastGPSRequest() { + var pack: Package = { + fromID: networkID, + toID: Net.BRODCAST_PORT, + ttl: 0, // Prevent forwarding + msgID: generateMessageID(), + type: GPSRequest, + data: null, + }; + + for (modem in Peripheral.instance.getModems()) { + if (!modem.isWireless()) continue; + modem.send(Net.BRODCAST_PORT, Net.instance.networkID, pack); + } + } }