added GPS
This commit is contained in:
parent
7fe52b1a8a
commit
4f8db600dc
@ -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();
|
||||
});
|
||||
|
||||
219
src/kernel/gps/GPS.hx
Normal file
219
src/kernel/gps/GPS.hx
Normal file
@ -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<Pos3> {
|
||||
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<Pos3> = kvstore.get("mpos"); // Manual set position
|
||||
var cPos:Null<Pos3> = 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<Pos3> {
|
||||
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<Pos3> = [];
|
||||
|
||||
// // 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;
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user