Broadcast GPS and IMU data over CAN bus

RaceCapture_GPS_IMU_CAN_broadcast.lua

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