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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment