Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
setTickRate(20)
function sendCAN(can, id, data)
local res = txCAN(can, id, 0, data,100)
if res == 0 then println('send CAN res ' ..res) end
end
function split32(val)
return bit.band(val, 0xFF), bit.band(bit.rshift(val,8), 0xFF), bit.band(bit.rshift(val,16),0xFF),bit.band(bit.rshift(val,24),0xFF)
end
function split16(val)
return bit.band(val, 0xFF), bit.band(bit.rshift(val,8), 0xFF)
end
function sendGPS()
local lat,lon = getGpsPos()
local speed = getGpsSpeed()
local altitude = getGpsAltitude()
local qual = getGpsQuality()
local sats = getGpsSats()
gpsDOP = 1.7 --this is a test value, don't yet have getGpsDop() in firmware
--transform
lat = lat * 10000000
lon = lon * 10000000
speed = speed * 10
altitude = altitude * 10
gpsDOP = gpsDOP * 10
--split values
local lat1, lat2, lat3, lat4 = split32(lat)
local lon1, lon2, lon3, lon4 = split32(lon)
local speed1, speed2 = split16(speed)
--local speed1, speed2 = split16(val)
--println(speed1)
--println(speed2)
--println(val)
local altitude1, altitude2 = split16(altitude)
sendCAN(1, 1416,{lat1, lat2, lat3, lat4, lon1, lon2, lon3, lon4})
sendCAN(1, 1417,{speed1, speed2, altitude1, altitude2, qual, sats, gpsDOP})
end
function sendImu()
local x1,x2 = split16(getImu(0) * 100)
local y1,y2 = split16(getImu(1) * 100)
local z1,z2 = split16(getImu(2) * 100)
local yaw1,yaw2 = split16(getImu(3) * 10)
local pitch1,pitch2 = split16(getImu(4) * 10)
local roll1,roll2 = split16(getImu(5) * 10)
sendCAN(1, 1418, {x1,x2,y1,y2,z1,z2})
sendCAN(1, 1419, {yaw1,yaw2,pitch1,pitch2,roll1,roll2})
end
count = 0
sniffcan = 1
function onTick()
count = count + 1
repeat --will drain CAN buffer on each tick
id, e, data = rxCAN(0)
if id ~= nil then
txCAN(1, id, e, data)
if sniffcan == 1 then
print(id ..':')
for i=1,#data do
print(data[i] ..' ')
end
println('')
end
end
-- transmit GPS data
sendGPS()
sendImu()
until id == nil
println('end')
end
setTickRate(20)
function sendCAN(can, id, data)
local res = txCAN(can, id, 0, data,100)
if res == 0 then println('send CAN res ' ..res) end
end
function split32(val)
return bit.band(val, 0xFF), bit.band(bit.rshift(val,8), 0xFF), bit.band(bit.rshift(val,16),0xFF),bit.band(bit.rshift(val,24),0xFF)
end
function split16(val)
return bit.band(val, 0xFF), bit.band(bit.rshift(val,8), 0xFF)
end
function sendGPS()
local lat,lon = getGpsPos()
local speed = getGpsSpeed()
local altitude = getGpsAltitude()
local qual = getGpsQuality()
local sats = getGpsSats()
gpsDOP = 1.7 --this is a test value, don't yet have getGpsDop() in firmware
--transform
lat = lat * 10000000
lon = lon * 10000000
speed = speed * 10
altitude = altitude * 10
gpsDOP = gpsDOP * 10
--split values
local lat1, lat2, lat3, lat4 = split32(lat)
local lon1, lon2, lon3, lon4 = split32(lon)
local speed1, speed2 = split16(speed)
--local speed1, speed2 = split16(val)
--println(speed1)
--println(speed2)
--println(val)
local altitude1, altitude2 = split16(altitude)
sendCAN(1, 1416,{lat1, lat2, lat3, lat4, lon1, lon2, lon3, lon4})
sendCAN(1, 1417,{speed1, speed2, altitude1, altitude2, qual, sats, gpsDOP})
end
function sendImu()
local x1,x2 = split16(getImu(0) * 100)
local y1,y2 = split16(getImu(1) * 100)
local z1,z2 = split16(getImu(2) * 100)
local yaw1,yaw2 = split16(getImu(3) * 10)
local pitch1,pitch2 = split16(getImu(4) * 10)
local roll1,roll2 = split16(getImu(5) * 10)
sendCAN(1, 1418, {x1,x2,y1,y2,z1,z2})
sendCAN(1, 1419, {yaw1,yaw2,pitch1,pitch2,roll1,roll2})
end
sniffcan = 1
function onTick()
-- get the current uptime and then drain CAN buffer for 10ms
uptime = getUptime() -- val in miliseconds
exitloop = uptime + 10
-- send GPS data to AEM CD7
sendGPS()
sendImu()
repeat --will drain CAN buffer on each tick
id, e, data = rxCAN(0)
if id ~= nil then
txCAN(1, id, e, data)
if sniffcan == 1 then
print(id ..':')
for i=1,#data do
print(data[i] ..' ')
end
println('')
end
end
uptime = getUptime()
until (id == nil) or (uptime > exitloop)
println('exited CAN drain loop')
end
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment