Created
November 15, 2017 23:57
-
-
Save brentpicasso/0ccfb49690e8de6f98c216e80c86694c to your computer and use it in GitHub Desktop.
Broadcast GPS and IMU data over CAN bus
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
--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