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
You can’t perform that action at this time.