niklas/cc-haxe
1
0
Fork 0
Code Issues 14 Pull Requests Projects Releases Wiki Activity

BIG FORMATING COMMIT

Browse Source
This commit is contained in:
Djeeberjr
2023-07-30 15:55:22 +02:00
parent 088fce0aaa
commit 91972107eb
103 changed files with 1610 additions and 1585 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
src/kernel/gps/GPS.hx
View File

@@ -19,9 +19,9 @@ class GPS {
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:Pos3;
private static var lastPositionResponse: Array<{pos:Pos3,dist:Float}> = [];
private static var lastPositionResponse:Array<{pos:Pos3, dist:Float}> = [];
private static var futureResolve: (pos:Null<Pos3>) -> Void = null;
private static var futureResolve:(pos:Null<Pos3>) -> Void = null;
@:allow(kernel.Init)
private static function init() {
@@ -30,7 +30,7 @@ class GPS {
public static function setManualPosition(pos:Pos3) {
var kvstore = new KVStore("gps");
kvstore.set("mpos",pos);
kvstore.set("mpos", pos);
kvstore.save();
if (cachedPosition == null) {
@@ -61,7 +61,7 @@ class GPS {
public static function locate():Future<Null<Pos3>> {
// TODO: implenet a timeout
// TODO: dont send a request twice if the last one is still pending or we moved
return new Future<Null<Pos3>>((resolve)->{
return new Future<Null<Pos3>>((resolve) -> {
futureResolve = resolve;
sendPositionRequest();
return null;
@@ -73,10 +73,11 @@ class GPS {
}
private static function persistCachedPositon() {
if (cachedPosition == null) return;
if (cachedPosition == null)
return;
var kvstore = new KVStore("gps");
kvstore.set("cpos",cachedPosition);
kvstore.set("cpos", cachedPosition);
kvstore.save();
}
@@ -94,7 +95,7 @@ class GPS {
return;
}
if (mPos != null && cPos != null && mPos != cPos){
if (mPos != null && cPos != null && mPos != cPos) {
// Both are different, so we can use the manual position
cachedPosition = mPos;
posAccuracy = 1;
@@ -114,7 +115,6 @@ class GPS {
posAccuracy = 2;
return;
}
}
private static function sendPositionRequest() {
@@ -122,33 +122,39 @@ class GPS {
}
@:allow(kernel.net.Net)
private static function handlePackage(pack:Package<Noise>, dist: Float,iface: INetworkInterface) {
private static function handlePackage(pack:Package<Noise>, dist:Float, iface:INetworkInterface) {
switch (pack.type) {
case GPSRequest:
if (!shouldRespond) return;
if (posAccuracy < 2) return;
if (cachedPosition == null) return;
if (!shouldRespond)
return;
if (posAccuracy < 2)
return;
if (cachedPosition == null)
return;
var response = new Package(Net.networkID,pack.fromID, pack.msgID, GPSResponse(cachedPosition),null,0);
iface.send(pack.fromID,Net.networkID,response);
var response = new Package(Net.networkID, pack.fromID, pack.msgID, GPSResponse(cachedPosition), null, 0);
iface.send(pack.fromID, Net.networkID, response);
case GPSResponse(pos):
if (lastPositionResponse.contains({pos:pos,dist:dist})) return; // Ignore duplicate responses
if (lastPositionResponse.contains({pos: pos, dist: dist}))
return; // Ignore duplicate responses
lastPositionResponse.push({pos:pos,dist:dist});
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
if (lastPositionResponse.length < 4)
return; // We need at least 3 responses to calculate the position
var calculatedPosition = calculatePosition();
if (calculatedPosition != null){
if (calculatedPosition != null) {
calculatedPosition = calculatedPosition.round();
}
lastPositionResponse = []; // Reset the response array
if (calculatedPosition == null) return;
if (calculatedPosition == null)
return;
cachedPosition = calculatedPosition;
posAccuracy = 3;
@@ -158,7 +164,8 @@ class GPS {
}
private static function calculatePosition():Null<Pos3> {
if (lastPositionResponse.length < 3) return null;
if (lastPositionResponse.length < 3)
return null;
// do a simple trilateration with the last 3 responses for now
var p1 = lastPositionResponse[0].pos;
@@ -169,13 +176,14 @@ class GPS {
var r2 = lastPositionResponse[1].dist;
var r3 = lastPositionResponse[2].dist;
var result = trilateration(p1,p2,p3,r1,r2,r3);
var result = trilateration(p1, p2, p3, r1, r2, r3);
if (result.a.close(result.b)) return result.a;
if (result.a.close(result.b))
return result.a;
// 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){
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);
@@ -184,12 +192,15 @@ class GPS {
return null; // The two positions are essentially the same, so we cant determine which one is correct
}
if (err1 < err2) return result.a;
if (err2 < err1) return result.b;
if (err1 < err2)
return result.a;
if (err2 < err1)
return result.b;
}
// last resort, just use the position that is closest to a whole number (whithin a resonable margin of error)
if (!shouldDoWholeNumberCheck) return null;
if (!shouldDoWholeNumberCheck)
return null;
// TODO: mark the position as not so accurate if we use this method
@@ -208,7 +219,7 @@ class GPS {
/**
Determines the position(s) of a point given 3 other points and the distance to each of them.
**/
private static function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pair<Pos3,Pos3> {
private static function trilateration(p1:Pos3, p2:Pos3, p3:Pos3, r1:Float, r2:Float, r3:Float):Pair<Pos3, Pos3> {
var a2b = p2 - p1;
var a2c = p3 - p1;
@@ -227,9 +238,9 @@ class GPS {
var zSquared = r1 * r1 - x * x - y * y;
if (zSquared > 0) {
var z = Math.sqrt(zSquared);
return new Pair(result + (ez * z),result - (ez * z));
return new Pair(result + (ez * z), result - (ez * z));
}
return new Pair(result,result);
return new Pair(result, result);
}
}