retransmit-plus-gps-borked.lua

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

retransmit-plus-gps-fixed.lua

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