SHOW:
|
|
- or go back to the newest paste.
1 | -- TrilateratorGPS, modified a bit to track Opus SNMP pings instead now that GPS is anonymized | |
2 | ||
3 | local filter = ... | |
4 | ||
5 | local config = dofile "config.lua" | |
6 | - | modems[name].open(gps.CHANNEL_GPS) |
6 | + | |
7 | for name, location in pairs(config.modems) do | |
8 | modems[name] = peripheral.wrap(name) | |
9 | modems[name].location = location | |
10 | modems[name].open(999) | |
11 | end | |
12 | ||
13 | local function timestamp() | |
14 | return os.date "!%X" | |
15 | end | |
16 | ||
17 | -- Trilateration code from GPS and modified slightly | |
18 | ||
19 | local function trilaterate( A, B, C ) | |
20 | local a2b = B.position - A.position | |
21 | local a2c = C.position - A.position | |
22 | ||
23 | if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then | |
24 | return nil | |
25 | end | |
26 | ||
27 | local d = a2b:length() | |
28 | local ex = a2b:normalize( ) | |
29 | local i = ex:dot( a2c ) | |
30 | local ey = (a2c - (ex * i)):normalize() | |
31 | local j = ey:dot( a2c ) | |
32 | local ez = ex:cross( ey ) | |
33 | ||
34 | local r1 = A.distance | |
35 | local r2 = B.distance | |
36 | local r3 = C.distance | |
37 | ||
38 | local x = (r1*r1 - r2*r2 + d*d) / (2*d) | |
39 | local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j) | |
40 | ||
41 | local result = A.position + (ex * x) + (ey * y) | |
42 | ||
43 | local zSquared = r1*r1 - x*x - y*y | |
44 | if zSquared > 0 then | |
45 | local z = math.sqrt( zSquared ) | |
46 | local result1 = result + (ez * z) | |
47 | local result2 = result - (ez * z) | |
48 | ||
49 | local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 ) | |
50 | if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then | |
51 | return rounded1, rounded2 | |
52 | else | |
53 | return rounded1 | |
54 | end | |
55 | end | |
56 | return result:round( 0.01 ) | |
57 | end | |
58 | ||
59 | local function narrow( p1, p2, fix ) | |
60 | local dist1 = math.abs( (p1 - fix.position):length() - fix.distance ) | |
61 | local dist2 = math.abs( (p2 - fix.position):length() - fix.distance ) | |
62 | ||
63 | if math.abs(dist1 - dist2) < 0.01 then | |
64 | return p1, p2 | |
65 | elseif dist1 < dist2 then | |
66 | return p1:round( 0.01 ) | |
67 | else | |
68 | - | local function short_float(x) |
68 | + | |
69 | - | return ("%.0f"):format(x) |
69 | + | |
70 | end | |
71 | ||
72 | local monitor = peripheral.find "monitor" | |
73 | if monitor then | |
74 | monitor.setTextScale(0.5) | |
75 | term.redirect(monitor) | |
76 | end | |
77 | ||
78 | print(timestamp(), "Initialized") | |
79 | ||
80 | local fixes = {} | |
81 | ||
82 | while true do | |
83 | local _, modem, channel, reply_channel, message, distance = os.pullEvent "modem_message" | |
84 | - | if distance and message == "PING" then |
84 | + | if distance then |
85 | - | local reply_modem = modems[modem] |
85 | + | if not fixes[reply_channel] then fixes[reply_channel] = {} end |
86 | - | reply_modem.transmit(reply_channel, gps.CHANNEL_GPS, { |
86 | + | local rc_fixes = fixes[reply_channel] |
87 | - | reply_modem.location[1], reply_modem.location[2], reply_modem.location[3], dimension = config.dimension, server = config.server |
87 | + | local recv_modem = modems[modem] |
88 | - | }) |
88 | + | table.insert(rc_fixes, { position = vector.new(unpack(recv_modem.location)), distance = distance }) |
89 | - | table.insert(fixes, { position = vector.new(unpack(reply_modem.location)), distance = distance }) |
89 | + | if #rc_fixes == 4 then |
90 | - | if #fixes == 4 then |
90 | + | local p1, p2 = trilaterate(rc_fixes[1], rc_fixes[2], rc_fixes[3]) |
91 | - | local p1, p2 = trilaterate(fixes[1], fixes[2], fixes[3]) |
91 | + | |
92 | local pos = narrow(p1, p2, rc_fixes[4]) | |
93 | - | local pos = narrow(p1, p2, fixes[4]) |
93 | + | local status, label = "?", "?" |
94 | - | print(timestamp(), ("Ping from %.0f %.0f %.0f"):format(pos.x, pos.y, pos.z)) |
94 | + | if type(message) == "table" then status = tostring(message.status) label = tostring(message.label) end |
95 | if (not filter) or (label:match(filter)) then | |
96 | - | fixes = {} |
96 | + | print(timestamp(), ("%05d %s (%.0f %.0f %.0f) %s"):format(reply_channel, label, pos.x, pos.y, pos.z, status)) |
97 | end | |
98 | end | |
99 | fixes[reply_channel] = {} | |
100 | end | |
101 | end | |
102 | end |