From b9fcbd9040726f04fc8b59e7427474d4d1b7857f Mon Sep 17 00:00:00 2001 From: Djeeberjr Date: Sat, 8 Apr 2023 03:41:52 +0200 Subject: [PATCH] fixed GPS --- src/kernel/gps/GPS.hx | 98 ++++++++++++++++++++----------------------- 1 file changed, 46 insertions(+), 52 deletions(-) diff --git a/src/kernel/gps/GPS.hx b/src/kernel/gps/GPS.hx index 14a8d08..5697d30 100644 --- a/src/kernel/gps/GPS.hx +++ b/src/kernel/gps/GPS.hx @@ -1,11 +1,14 @@ package kernel.gps; +import kernel.log.Log; import lib.KVStore; import kernel.net.Net; import kernel.net.INetworkInterface; import kernel.net.Package; import lib.Pos3; +using tink.CoreApi; + /** 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. @@ -15,6 +18,7 @@ class GPS { public static var instance:GPS; private var shouldRespond = true; + private var shouldDoWholeNumberCheck = 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}> = []; @@ -112,12 +116,18 @@ class GPS { 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; + if (lastPositionResponse.contains({pos:pos,dist:dist})) return; // Ignore duplicate responses + + lastPositionResponse.push({pos:pos,dist:dist}); + + // TODO: wait for a few seconds before calculating the position, so we can get more responses + + if (lastPositionResponse.length < 4) return; // We need at least 3 responses to calculate the position + + var calculatedPosition = calculatePosition().round(); + + lastPositionResponse = []; // Reset the response array - var calculatedPosition = calculatePosition(); if (calculatedPosition == null) return; cachedPosition = calculatedPosition; posAccuracy = 3; @@ -137,62 +147,46 @@ class GPS { var r2 = lastPositionResponse[1].dist; var r3 = lastPositionResponse[2].dist; - return trilateration(p1,p2,p3,r1,r2,r3); + var result = trilateration(p1,p2,p3,r1,r2,r3); - // var calculatedPositions: Array = []; + if (result.a.close(result.b)) return result.a; - // // 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; + // If we have more than 3 responses, we can use the 4th response to determine which of the two positions is correct + // TODO: if this is a pocket computer we cant use this since it can move freely + if (lastPositionResponse.length > 3){ + var err1 = Math.abs(result.a.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist); + var err2 = Math.abs(result.b.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist); - // var p1 = lastPositionResponse[i].pos; - // var p2 = lastPositionResponse[j].pos; - // var p3 = lastPositionResponse[k].pos; + if (Math.abs(err1 - err2) < 0.0001) { + Log.warn("Failed to determine GPS position via 4th fix"); + return null; // The two positions are essentially the same, so we cant determine which one is correct + } - // var r1 = lastPositionResponse[i].dist; - // var r2 = lastPositionResponse[j].dist; - // var r3 = lastPositionResponse[k].dist; + if (err1 < err2) return result.a; + if (err2 < err1) return result.b; + } - // calculatedPositions.push(trilateration(p1,p2,p3,r1,r2,r3)); - // } - // } - // } + // last resort, just use the position that is closest to a whole number (whithin a resonable margin of error) + if (!shouldDoWholeNumberCheck) return null; - // 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; - // } - // } - // } + // TODO: mark the position as not so accurate if we use this method - // if (!tainted) return calculatedPositions[0]; + var err1 = (result.a - result.a.round()).length(); + var err2 = (result.b - result.b.round()).length(); - // // 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; - // } - // } + if (err1 < err2 && err1 < 0.0001) { + return result.a; + } else if (err2 < 0.0001) { + return result.b; + } - // return mostCommon; + return null; } - private function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pos3 { + /** + Determines the position(s) of a point given 3 other points and the distance to each of them. + **/ + private function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pair { var a2b = p2 - p1; var a2c = p3 - p1; @@ -211,9 +205,9 @@ class GPS { var zSquared = r1 * r1 - x * x - y * y; if (zSquared > 0) { var z = Math.sqrt(zSquared); - result += ez * z; + return new Pair(result + (ez * z),result - (ez * z)); } - return result; + return new Pair(result,result); } }