2019-07-27 04:13:24 +00:00
|
|
|
local Util = require('opus.util')
|
|
|
|
|
2016-12-11 19:24:52 +00:00
|
|
|
local GPS = { }
|
2019-12-27 06:11:36 +00:00
|
|
|
GPS.CHANNEL_GPS = 65534
|
2016-12-11 19:24:52 +00:00
|
|
|
|
2017-10-08 21:45:01 +00:00
|
|
|
local device = _G.device
|
2019-12-27 06:11:36 +00:00
|
|
|
local vector = _G.vector
|
2017-10-08 21:45:01 +00:00
|
|
|
|
2016-12-11 19:24:52 +00:00
|
|
|
function GPS.locate(timeout, debug)
|
2019-12-27 06:11:36 +00:00
|
|
|
if not device.wireless_modem then
|
|
|
|
if debug then
|
|
|
|
print('No wireless modem attached')
|
|
|
|
end
|
|
|
|
return nil
|
|
|
|
end
|
|
|
|
|
|
|
|
if debug then
|
|
|
|
print('Finding position...')
|
|
|
|
end
|
|
|
|
|
|
|
|
local modem = device.wireless_modem
|
|
|
|
local closeChannel = false
|
|
|
|
local selfID = os.getComputerID()
|
|
|
|
if not modem.isOpen(selfID) then
|
|
|
|
modem.open(selfID)
|
|
|
|
closeChannel = true
|
|
|
|
end
|
|
|
|
|
|
|
|
modem.transmit(GPS.CHANNEL_GPS, selfID, "PING")
|
|
|
|
|
|
|
|
local fixes = {}
|
|
|
|
local pos = nil
|
|
|
|
local timer = os.startTimer(timeout or 1)
|
|
|
|
while true do
|
|
|
|
local e, side, chan, reply, msg, dist = os.pullEvent()
|
|
|
|
if e == "modem_message" then
|
|
|
|
if side == modem.side and chan == selfID and reply == GPS.CHANNEL_GPS and dist then
|
|
|
|
if type(msg) == "table" and #msg == 3 and tonumber(msg[1]) and tonumber(msg[2]) and tonumber(msg[3]) then
|
|
|
|
local fix = {
|
|
|
|
position = vector.new(unpack(msg)),
|
|
|
|
distance = dist,
|
|
|
|
}
|
|
|
|
if debug then
|
|
|
|
print(fix.distance..' meters from '..fix.position:tostring())
|
|
|
|
end
|
|
|
|
if fix.distance == 0 then
|
|
|
|
pos = fix.position
|
|
|
|
else
|
|
|
|
fixes[#fixes+1] = fix
|
|
|
|
if #fixes > 3 then
|
|
|
|
pos = GPS.trilaterate(fixes)
|
|
|
|
if pos then break end
|
|
|
|
end
|
|
|
|
end
|
|
|
|
end
|
|
|
|
end
|
|
|
|
elseif e == "timer" and side == timer then
|
|
|
|
break
|
|
|
|
end
|
|
|
|
end
|
|
|
|
|
|
|
|
if closeChannel then
|
|
|
|
modem.close(selfID)
|
|
|
|
end
|
|
|
|
if debug then
|
|
|
|
print("Position is "..pos.x..","..pos.y..","..pos.z)
|
2018-01-24 22:39:38 +00:00
|
|
|
end
|
2019-12-27 06:11:36 +00:00
|
|
|
return vector.new(pos.x, pos.y, pos.z)
|
2016-12-11 19:24:52 +00:00
|
|
|
end
|
|
|
|
|
|
|
|
function GPS.isAvailable()
|
2018-01-24 22:39:38 +00:00
|
|
|
return device.wireless_modem and GPS.locate()
|
2016-12-11 19:24:52 +00:00
|
|
|
end
|
|
|
|
|
|
|
|
function GPS.getPoint(timeout, debug)
|
2018-01-24 22:39:38 +00:00
|
|
|
local pt = GPS.locate(timeout, debug)
|
|
|
|
if not pt then
|
|
|
|
return
|
|
|
|
end
|
2016-12-11 19:24:52 +00:00
|
|
|
|
2018-01-24 22:39:38 +00:00
|
|
|
pt.x = math.floor(pt.x)
|
|
|
|
pt.y = math.floor(pt.y)
|
|
|
|
pt.z = math.floor(pt.z)
|
2016-12-11 19:24:52 +00:00
|
|
|
|
2018-01-24 22:39:38 +00:00
|
|
|
if _G.pocket then
|
|
|
|
pt.y = pt.y - 1
|
|
|
|
end
|
2016-12-11 19:24:52 +00:00
|
|
|
|
2018-01-24 22:39:38 +00:00
|
|
|
return pt
|
2016-12-11 19:24:52 +00:00
|
|
|
end
|
|
|
|
|
|
|
|
-- from stock gps API
|
2017-10-08 21:45:01 +00:00
|
|
|
local function trilaterate(A, B, C)
|
2018-01-24 22:39:38 +00:00
|
|
|
local a2b = B.position - A.position
|
|
|
|
local a2c = C.position - A.position
|
|
|
|
|
|
|
|
if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
|
|
|
|
local d = a2b:length()
|
|
|
|
local ex = a2b:normalize( )
|
|
|
|
local i = ex:dot( a2c )
|
|
|
|
local ey = (a2c - (ex * i)):normalize()
|
|
|
|
local j = ey:dot( a2c )
|
|
|
|
local ez = ex:cross( ey )
|
|
|
|
|
|
|
|
local r1 = A.distance
|
|
|
|
local r2 = B.distance
|
|
|
|
local r3 = C.distance
|
|
|
|
|
|
|
|
local x = (r1*r1 - r2*r2 + d*d) / (2*d)
|
|
|
|
local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
|
|
|
|
|
|
|
|
local result = A.position + (ex * x) + (ey * y)
|
|
|
|
|
|
|
|
local zSquared = r1*r1 - x*x - y*y
|
|
|
|
if zSquared > 0 then
|
|
|
|
local z = math.sqrt( zSquared )
|
|
|
|
local result1 = result + (ez * z)
|
|
|
|
local result2 = result - (ez * z)
|
|
|
|
|
2019-12-27 06:11:36 +00:00
|
|
|
local rounded1, rounded2 = result1:round(0.01), result2:round(0.01)
|
2018-01-24 22:39:38 +00:00
|
|
|
if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
|
|
|
|
return rounded1, rounded2
|
|
|
|
else
|
|
|
|
return rounded1
|
|
|
|
end
|
|
|
|
end
|
2019-12-27 06:11:36 +00:00
|
|
|
return result:round(0.01)
|
2016-12-11 19:24:52 +00:00
|
|
|
end
|
|
|
|
|
|
|
|
local function narrow( p1, p2, fix )
|
2018-01-24 22:39:38 +00:00
|
|
|
local dist1 = math.abs( (p1 - fix.position):length() - fix.distance )
|
|
|
|
local dist2 = math.abs( (p2 - fix.position):length() - fix.distance )
|
|
|
|
|
2019-12-27 06:11:36 +00:00
|
|
|
if math.abs(dist1 - dist2) < 0.01 then
|
2018-01-24 22:39:38 +00:00
|
|
|
return p1, p2
|
|
|
|
elseif dist1 < dist2 then
|
2019-12-27 06:11:36 +00:00
|
|
|
return p1:round(0.01)
|
2018-01-24 22:39:38 +00:00
|
|
|
else
|
2019-12-27 06:11:36 +00:00
|
|
|
return p2:round(0.01)
|
2018-01-24 22:39:38 +00:00
|
|
|
end
|
2016-12-11 19:24:52 +00:00
|
|
|
end
|
|
|
|
-- end stock gps api
|
|
|
|
|
|
|
|
function GPS.trilaterate(tFixes)
|
2019-07-27 04:13:24 +00:00
|
|
|
local attemps = 0
|
|
|
|
for tFixes in Util.permutation(tFixes) do
|
|
|
|
attemps = attemps + 1
|
|
|
|
local pos1, pos2 = trilaterate(tFixes[4], tFixes[3], tFixes[2])
|
|
|
|
if pos2 then
|
|
|
|
pos1, pos2 = narrow(pos1, pos2, tFixes[1])
|
|
|
|
end
|
2019-12-11 16:38:12 +00:00
|
|
|
if not pos2 and pos1 and not (pos1.x ~= pos1.x) then
|
2019-07-27 04:13:24 +00:00
|
|
|
return pos1, attemps
|
|
|
|
end
|
2018-01-24 22:39:38 +00:00
|
|
|
end
|
2016-12-11 19:24:52 +00:00
|
|
|
end
|
|
|
|
|
|
|
|
return GPS
|