--gheadng.lua --Written by Fig Newton (Isaac Davis) 6/5/2014 --This lua script will give you a heading output variable you can use. --It's accuracy depends on how well the gps is locked in, and also how much --you are moving. The more you move, the more accurate this is. --V1.0 6/6/14 --V1.1 6/7/14 --V1.2 6/8/14 - Added in multiple headings and average. --V1.3 6/8/14 - Fixed a dumb mistake in the averaging calculation. --V1.4 6/9/14 - Added in arrays so you can choose how many location points you want to average. Also changed -- the output names. Found an even bigger mistake in the heading averaging. --V1.5 7/19/14 - Corrected the constants for Pilot-Latitude and Pilot-Longitude. Script was erroring out in OpenTX v.2.0.7. It's -- now v2.0.7+ compatible. local outputs = { "PHDG", "Hout", "Hbak","Lon", "Lat", "Timr" } --Lets the user pick the sampling rate. Every 30 cycles is about a second. It all depends on --how fast the lua model scripts run. --Sample num is the number of location points taken and calculated to compute heading local inputs = { {"SamplDelay", VALUE, 10, 150, 30}, {"Sample Num", VALUE, 2, 8, 3} } local headtmp local timz local lat = {0,0,0,0,0,0,0,0,0,0} local lon = {0,0,0,0,0,0,0,0,0,0} local heading = {0,0,0,0,0,0,0,0,0,0} --Set up the variables. local function init() headtmp=0 headtoh=0 headfromh=0 z1=0 z2=0 timz = 0 pilotlat = getValue("pilot-latitude") pilotlon = getValue("pilot-longitude") end --Function to average headings. Note I found this has to be above the run function otherwise it seems the interpreter cannot --find it. local function avpts(heading1, heading2) if heading1-heading2 > 180 then heading2=heading2+360 end if heading2-heading1 > 180 then heading1=heading1+360 end local averageheading = (heading1+heading2)/2 if averageheading >= 360 then averageheading = averageheading - 360 end return averageheading end local function run_func( sample, snum ) --local smpl = sample --Increment the sample delay timz=timz+1 --It's time to take a location reading if timz >= sample then headtmp = 0 --Since the pilot location could have been blank on the init, let's grab it again to be safe. pilotlat = getValue("pilot-latitude") pilotlon = getValue("pilot-longitude") --Push all the locations previously read to the back. This scrolls the locations. for loop=1 , snum-1 do lat[loop] = lat[loop+1] lon[loop] = lon[loop+1] end lat[snum] = getValue("latitude") lon[snum] = getValue("longitude") -- Test Values -- pilotlat=45.5660 -- pilotlon=120.5665 -- lat[1]=45.5635 -- lat[2]=45.5645 -- lat[3]=45.5645 -- lon[1]=120.5665 -- lon[2]=120.5665 -- lon[3]=120.5666 -- Check the current and previous locations to make sure they aren't 0. 0 means no gps lock. if lat[snum-1]~=0 and lat[snum]~=0 and lon[snum-1]~=0 and lon[snum]~=0 then --Loop through the locations and calculate a heading for each pair for calcloop = 1 , snum-1 do z1 = math.sin(math.rad(lon[calcloop+1]) - math.rad(lon[calcloop])) * math.cos(math.rad(lat[calcloop+1])) z2 = math.cos(math.rad(lat[calcloop])) * math.sin(math.rad(lat[calcloop+1])) - math.sin(math.rad(lat[calcloop])) * math.cos(math.rad(lat[calcloop+1])) * math.cos(math.rad(lon[calcloop+1]) - math.rad(lon[calcloop])) heading[calcloop] = math.deg(math.atan2(z1, z2)) --Fix negative values if heading[calcloop] < 0 then heading[calcloop]=heading[calcloop]+360 end end -- take the average of the headings headtmp = heading[1] --Loop through the next headings if there are more than 3 sample locations if snum>2 then for avgloop = 2 , snum-1 do headtmp = avpts(headtmp, heading[avgloop]) end end else --GPS readings are still 0, so no GPS lock has been obtained. Return 0. headtmp=0 end --Then we calculate the heading from the pilot position to the current position. if pilotlat~=0 and lat[snum]~=0 and pilotlon~=0 and lon[snum]~=0 then z1 = math.sin(math.rad(lon[snum]) - math.rad(pilotlon)) * math.cos(math.rad(lat[snum])) z2 = math.cos(math.rad(pilotlat)) * math.sin(math.rad(lat[snum])) - math.sin(math.rad(pilotlat)) * math.cos(math.rad(lat[snum])) * math.cos(math.rad(lon[snum]) - math.rad(pilotlon)) headfromh = math.deg(math.atan2(z1, z2)) if headfromh < 0 then headfromh=headfromh+360 end --Subtract 180 and get the heading to get back home. headtoh = headfromh-180 if headtoh < 0 then headtoh = headtoh+360 end else headfromh = 0 headtoh = 0 end --Zero out the timer for the sample delay. timz = 0 end --Return: Current heading, current latitude, current longitude, heading from home, heading to home return headtmp*10.24, headfromh*10.24, headtoh*10.24, lon[snum]*10.24, lat[snum]*10.24, timz*10.24 end -- Return statement return { run=run_func, output=outputs, init=init, input=inputs}