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

fixed GPS

Browse Source
This commit is contained in:
Djeeberjr 2023-04-08 03:41:52 +02:00
parent ba05ff7645
commit b9fcbd9040
1 changed files with 46 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<Pos3> = [];
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;
// 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;
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
}
private function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pos3 {
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;
// TODO: mark the position as not so accurate if we use this method
var err1 = (result.a - result.a.round()).length();
var err2 = (result.b - result.b.round()).length();
if (err1 < err2 && err1 < 0.0001) {
return result.a;
} else if (err2 < 0.0001) {
return result.b;
}
return null;
}
/**
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<Pos3,Pos3> {
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);
}
}