Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Broadcast GPS and IMU data over CAN bus
--This script broadcasts current GPS and IMU channels over CAN
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 altitude1, altitude2 = split16(altitude)
sendCAN(0, 1416,{lat1, lat2, lat3, lat4, lon1, lon2, lon3, lon4})
sendCAN(0, 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(0, 1418, {x1,x2,y1,y2,z1,z2})
sendCAN(0, 1419, {yaw1,yaw2,pitch1,pitch2,roll1,roll2})
end
count = 0
function onTick()
count = count + 1
-- send GPS 1/2 as fast as IMU
if count % 2 == 0 then sendGPS() end
sendImu()
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.