--FunctionScript to calculate LoverD (GlideRatio) to telemetry sensors to log it in the logfile
--Copyright LShems
--www.just.solutions
--GPL
--
--copy file to /SCRIPTS/FUNCTIONS
--

--!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
--!!!! create special function with switch ON to run the script 'logchx' !!!!
--!!!! make sure all units are the same: kts or m/s                      !!!!
--!!!! set new output sensors LD_A and LD_V to precision 0.00            !!!!
--!!!! add sensor by creating calculated sensor and name it Gdst         !!!!
--!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
--
--detect sensors.
--after detection, delete current logfile
--this to allow the new columns to be created correctly in the new logfile.

local GAlt          = function() return getValue("GAlt") or 0 end
local VSpd          = function() return getValue("VSpd") or 0 end
local GSpd          = function() return getValue("GSpd") or 0 end

--uncomment and adjust if you prefer sensor positions
--local GAlt        = function() return getValue("telem8") or 0 end
--local Vsp         = function() return getValue("telem5") or 0 end
--local GSpd        = function() return getValue("telem3") or 0 end

local rate    = 1   --1 sec update rate for calculations on GPS positions to get accurate results.
local filter  = 0.6 --some value between 0 and 1 to filter speed measurements
local ID      = 0   --101+ID and 102+ID will be used as sensor ID. Safe values between 0 and 50

--!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
--don't edit below this if you don't know what you are doing
--!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

local GAltVOld    = 0
--local LATLON      = {lat=0,lon=0}
local LATLONOld   = {lat=0,lon=0}
local timeOld     = getTime()
local GA_Alt      = 0
local GA_VSpd     = 0

return 
 {run=function()
    local VSpdV     = VSpd()
    local GSpdV     = GSpd()
    local time      = getTime()

    if GSpdV ~= 0 then --no division by zero please
      GA_VSpd = GA_VSpd*filter+(1-filter)*math.deg(math.atan(VSpdV/GSpdV))
    end
    
    if time > timeOld + rate*100 then --(wait for rate seconds to pass)
      
      --φ is latitude, λ is longitude, R is earth’s radius (mean radius = 6,371km)
      --const x = (λ2-λ1) * Math.cos((φ1+φ2)/2);
      --const y = (φ2-φ1);
      --const d = Math.sqrt(x*x + y*y) * R;
      
      local LATLON = getValue("GPS")
      if type(LATLON)~="table" then  --to prevent error on sensor lost
        LATLON = LATLONOld
      end
      local x = math.rad(LATLON.lon-LATLONOld.lon)*math.cos(math.rad(LATLON.lat))
      local y = math.rad(LATLON.lat-LATLONOld.lat)
      local dist = math.sqrt(x*x+y*y)*6371000
      
      local GAltV = GAlt()
      if dist ~= 0 then --no division by zero please
        GA_Alt = math.deg(math.atan((GAltV-GAltVOld)/dist))
      end
      
      GAltVOld  = GAltV
      timeOld   = time
      LATLONOld = LATLON
    end
    
    setTelemetryValue(0X0000,0,ID+100,GA_Alt*10,20,1,"GA_A")
    setTelemetryValue(0X0000,0,ID+101,GA_VSpd*10,20,1,"GA_V")
    
  end
  }