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

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

@ -1,11 +1,14 @@
package kernel.gps; package kernel.gps;
import kernel.log.Log;
import lib.KVStore; import lib.KVStore;
import kernel.net.Net; import kernel.net.Net;
import kernel.net.INetworkInterface; import kernel.net.INetworkInterface;
import kernel.net.Package; import kernel.net.Package;
import lib.Pos3; import lib.Pos3;
using tink.CoreApi;
/** /**
Determines the position of the computer based on the distance to other computers. 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. 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; public static var instance:GPS;
private var shouldRespond = true; 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 posAccuracy = 0; // 0 = unkown, 1 = (ins,best guess), 2 = (stored/manual,should be right), 3 = (gps,confirmed)
private var cachedPosition:Pos3; private var cachedPosition:Pos3;
private var lastPositionResponse: Array<{pos:Pos3,dist:Float}> = []; 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); var response = new Package(Net.instance.networkID,pack.fromID, pack.msgID, GPSResponse(cachedPosition),null,0);
iface.send(pack.fromID,Net.instance.networkID,response); iface.send(pack.fromID,Net.instance.networkID,response);
case GPSResponse(pos): case GPSResponse(pos):
if (lastPositionResponse.contains({pos:pos,dist:dist})) return; if (lastPositionResponse.contains({pos:pos,dist:dist})) return; // Ignore duplicate responses
lastPositionResponse.push({pos:pos,dist:dist});
if (lastPositionResponse.length > 5) lastPositionResponse.shift(); lastPositionResponse.push({pos:pos,dist:dist});
if (lastPositionResponse.length < 3) return;
// 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; if (calculatedPosition == null) return;
cachedPosition = calculatedPosition; cachedPosition = calculatedPosition;
posAccuracy = 3; posAccuracy = 3;
@ -137,62 +147,46 @@ class GPS {
var r2 = lastPositionResponse[1].dist; var r2 = lastPositionResponse[1].dist;
var r3 = lastPositionResponse[2].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 // If we have more than 3 responses, we can use the 4th response to determine which of the two positions is correct
// for (i in 0...lastPositionResponse.length) { // TODO: if this is a pocket computer we cant use this since it can move freely
// for (j in 0...lastPositionResponse.length) { if (lastPositionResponse.length > 3){
// if (i == j) continue; var err1 = Math.abs(result.a.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist);
// for (k in 0...lastPositionResponse.length) { var err2 = Math.abs(result.b.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist);
// if (i == k || j == k) continue;
// var p1 = lastPositionResponse[i].pos; if (Math.abs(err1 - err2) < 0.0001) {
// var p2 = lastPositionResponse[j].pos; Log.warn("Failed to determine GPS position via 4th fix");
// var p3 = lastPositionResponse[k].pos; return null; // The two positions are essentially the same, so we cant determine which one is correct
}
// var r1 = lastPositionResponse[i].dist; if (err1 < err2) return result.a;
// var r2 = lastPositionResponse[j].dist; if (err2 < err1) return result.b;
// var r3 = lastPositionResponse[k].dist; }
// 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; // TODO: mark the position as not so accurate if we use this method
// // 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]; var err1 = (result.a - result.a.round()).length();
var err2 = (result.b - result.b.round()).length();
// // Get the most common position if (err1 < err2 && err1 < 0.0001) {
// var mostCommon:Pos3 = null; return result.a;
// var mostCommonCount = 0; } else if (err2 < 0.0001) {
// for (i in 0...calculatedPositions.length) { return result.b;
// 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; 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<Pos3,Pos3> {
var a2b = p2 - p1; var a2b = p2 - p1;
var a2c = p3 - p1; var a2c = p3 - p1;
@ -211,9 +205,9 @@ class GPS {
var zSquared = r1 * r1 - x * x - y * y; var zSquared = r1 * r1 - x * x - y * y;
if (zSquared > 0) { if (zSquared > 0) {
var z = Math.sqrt(zSquared); var z = Math.sqrt(zSquared);
result += ez * z; return new Pair(result + (ez * z),result - (ez * z));
} }
return result; return new Pair(result,result);
} }
} }