• API
• FAQ
• Tools
• Archive
SHARE
TWEET

# TrilateratorGPS

osmarks Nov 10th, 2019 (edited) 122 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. local config = dofile "config.lua"
2. local modems = {}
3. for name, location in pairs(config.modems) do
4.     modems[name] = peripheral.wrap(name)
5.     modems[name].location = location
6.     modems[name].open(gps.CHANNEL_GPS)
7. end
8.
9. local function timestamp()
10.     return os.date "!%X"
11. end
12.
13. -- Trilateration code from GPS and modified slightly
14.
15. local function trilaterate( A, B, C )
16.     local a2b = B.position - A.position
17.     local a2c = C.position - A.position
18.
19.     if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
20.         return nil
21.     end
22.
23.     local d = a2b:length()
24.     local ex = a2b:normalize( )
25.     local i = ex:dot( a2c )
26.     local ey = (a2c - (ex * i)):normalize()
27.     local j = ey:dot( a2c )
28.     local ez = ex:cross( ey )
29.
30.     local r1 = A.distance
31.     local r2 = B.distance
32.     local r3 = C.distance
33.
34.     local x = (r1*r1 - r2*r2 + d*d) / (2*d)
35.     local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
36.
37.     local result = A.position + (ex * x) + (ey * y)
38.
39.     local zSquared = r1*r1 - x*x - y*y
40.     if zSquared > 0 then
41.         local z = math.sqrt( zSquared )
42.         local result1 = result + (ez * z)
43.         local result2 = result - (ez * z)
44.
45.         local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 )
46.         if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
47.             return rounded1, rounded2
48.         else
49.             return rounded1
50.         end
51.     end
52.     return result:round( 0.01 )
53. end
54.
55. local function narrow( p1, p2, fix )
56.     local dist1 = math.abs( (p1 - fix.position):length() - fix.distance )
57.     local dist2 = math.abs( (p2 - fix.position):length() - fix.distance )
58.
59.     if math.abs(dist1 - dist2) < 0.01 then
60.         return p1, p2
61.     elseif dist1 < dist2 then
62.         return p1:round( 0.01 )
63.     else
64.         return p2:round( 0.01 )
65.     end
66. end
67.
68. local function short_float(x)
69.     return ("%.0f"):format(x)
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
88.         })
89.         table.insert(fixes, { position = vector.new(unpack(reply_modem.location)), distance = distance })
90.         if #fixes == 4 then
91.             local p1, p2 = trilaterate(fixes[1], fixes[2], fixes[3])
92.             if p1 and p2 then
93.                 local pos = narrow(p1, p2, fixes[4])
94.                 print(timestamp(), ("Ping from %.0f %.0f %.0f"):format(pos.x, pos.y, pos.z))
95.             end
96.             fixes = {}
97.         end
98.     end
99. end
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.
Top