SmartyCAM GPS CAN mapping research

SmartyCAM_GPS_mapping.lua

--SmartyCAM GPS channels research
--Latitude / Longitude are confirmed
--GPS Satellite and GPS DOP (dilution of precision) are somewhat confident
--Need GPS lock status
--Need UTC / Timestamp

latId = addChannel('SC_GPSLat', 10, 6, -180, 180)
lonId = addChannel('SC_GPSLon', 10, 6, -180, 180)
gsat1Id = addChannel('SC_GPSSats', 10, 0, 0, 20)
gpsdopId = addChannel('SC_GPSDOP', 10, 0, 0, 10)

last_gps = 0

function extract32bitFloat(d)
 v = bit.lshift(d[5], 24) + bit.lshift(d[4], 16) + bit.lshift(d[3], 8) + d[2]
 return v / 10000000.0
end 

function processCAN(id, data)
 if id == 40 then
  local subId = data[1]

  -- Latitude
  if subId == 0 then
   setChannel(latId, extract32bitFloat(data))
  end

  -- Longitude
  if subId == 1 then
   setChannel(lonId, extract32bitFloat(data))
  end

  if subId == 1 or subId == 2 then 
   ut = getUptime()
   println('Got AIM Lat/Lon at: ' ..ut ..'; ms since last: ' .. (ut - last_gps))
   last_gps = ut
  end

  -- number of satellites (maybe)
  if subId == 8 then
   setChannel(gsat1Id, data[4])
  end 
 
  -- GPS DOP (maybe)
  if subId == 20 then
   setChannel(gpsdopId, data[2])
  end

  --dump raw data to log console
 end
 --dumpCAN(id, data)
end

function num(n)
  return string.sub('' ..n, 0, -3)
end

function dumpCAN(id, data)
 print(num(id) ..', ')
 for i = 1,#data do
  print(num(data[i]) ..', ')
 end
 println('')
end


--this function drains all pending CAN messages
--and outputs messages to the log
function outputCAN()
 repeat 
  id, ext, data = rxCAN(0, 1000)
  if id ~= nil then
   processCAN(id, data)
  end
 until id == nil
    println('Timeout: no CAN data')
end
 
function onTick()
 outputCAN()
end

initCAN(0, 1000000)
setTickRate(30)