sc300.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(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

function setLogging()
  if getGpsSpeed() >= 1 then
    startLogging()
  end
end

function sendLapData()
  -- last lap conversion
  lastlap_orig = getLapTime()
  local lastlap_seconds_portion = (lastlap_orig%1)*60
  local lastlap_minutes_portion = (math.floor(lastlap_orig))
  local lastlap_total_seconds = (lastlap_minutes_portion*60)+lastlap_seconds_portion
  local lastscale1,lastscale2,lastscale3,lastscale4 = split32(lastlap_total_seconds * 1000)
  
  -- predicted lap conversion
  local predlap_orig = getPredTime()
  local predlap_seconds_portion = (predlap_orig%1)*60
  local predlap_minutes_portion = (math.floor(predlap_orig))
  local predlap_total_seconds = (predlap_minutes_portion*60)+predlap_seconds_portion
  local predscale1,predscale2,predscale3,predscale4 = split32(predlap_total_seconds * 1000)

  -- calculate if last lap is better than best lap
  if (lastlap_orig < bestlap) and (lastlap_orig > 0) then
    bestlap = lastlap_orig
    bestlap_num = getLapCount()
  end

  local bestlap_seconds_portion = (bestlap%1)*60
  local bestlap_minutes_portion = (math.floor(bestlap))
  local bestlap_total_seconds = (bestlap_minutes_portion*60)+bestlap_seconds_portion
  local bestscale1,bestscale2,bestscale3,bestscale4 = split32(bestlap_total_seconds * 1000)

  -- delta between current predlap and bestlap in decimal minutes and seconds
  local lapdelta = predlap_orig - bestlap

  -- convert to real seconds
  local lapdelta_seconds = lapdelta*60
  local deltascale1,deltascale2,deltascale3,deltascale4 = split32(lapdelta_seconds * 1000)

  -- current elapsed lap time in miliseconds
  local elapsedtime = getUptime() - lapstarttime
  local elapsed1,elapsed2,elapsed3,elapsed4 = split32(elapsedtime)

  sendCAN(0, 1430, {lastscale1,lastscale2,lastscale3,lastscale4,predscale1,predscale2,predscale3,predscale4})
  sendCAN(0, 1431, {deltascale1,deltascale2,deltascale3,deltascale4,elapsed1,elapsed2,elapsed3,elapsed4})
  sendCAN(0, 1432, {bestscale1,bestscale2,bestscale3,bestscale4,bestlap_num,getLapCount()})
end

function sendGearPos()
-- 1 - 4.212
-- 2 - 3.537
-- 3 - 2.868
-- 4 - 2.204
-- 5 - 1.531
-- 6 - 0.855
-- N - 0.528
  local gear_sensor_v = getAnalog(4)
  local gear = 9
  if gear_sensor_v >= 4 then gear = 1
  elseif gear_sensor_v >= 3.2 and gear_sensor_v < 4 then gear = 2
  elseif gear_sensor_v >= 2.7 and gear_sensor_v < 3.2 then gear = 3
  elseif gear_sensor_v >= 2.0 and gear_sensor_v < 2.7 then gear = 4
  elseif gear_sensor_v >= 1.3 and gear_sensor_v < 2.0 then gear = 5
  elseif gear_sensor_v >= 0.7 and gear_sensor_v < 1.3 then gear = 6
  elseif gear_sensor_v >= 0.3 and gear_sensor_v < 0.7 then gear = 0
  else gear = 9
  end

  sendCAN(0, 1433, {gear})
end

bestlap = 999
bestlap_num = 0
lapstarttime = 0

function onTick()
  
  -- check whether or not we are moving and then enable logging
  setLogging()

  -- send GPS data to AEM CD7
  sendGPS()
  sendImu()

  -- check if we're at start finish to build lap runtime
  if getAtStartFinish() then
    -- start time is current time in miliseconds
    lapstarttime = getUptime()
  end

  -- send the lap data to the AEM CD7
  sendLapData()

  -- send the gear position
  sendGearPos()

end