|
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 |
