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