-- SFO LUA im Sender statt Arduino: Der Vogel wird leichter!

-- OpenTX Lua mix
-- Place this file in the Tx SD Card folder 
-- SD Card/SCRIPTS/MIXES/

-- DESCRIPTION
-- flapping the wings of a dual servo ornithopter (SFO) 

-- CONFIGURATION
-- Custom Scripts Screen
--   Select this script and configure inputs
--   Ampl=Dial, Ailr=Ail, Elev=Ele, Rudd=Rud, Freq=Thr
-- Inputs Screen
--   Ail & Thr must be 100%, Ele 30%, Rud 20% (signs not given here, check)
-- Mixer Screen
--   Create a mix using the inputs "Left" and "Right" which are the outputs
--   from this script. Add Ele & Ail Trims for tuning (check signs).
-- Outputs Screen
--   You may have to tune PPM Center on Left or Right for better turns

-- ------------------------------------------------------------------------


-- Global Lua environment variables used (Global between scripts)
-- None

-- Variables local to this script
local inputs = {
                 { "Ampl", SOURCE },
                 { "Ailr", SOURCE },
                 { "Elev", SOURCE },
                 { "Rudd", SOURCE },
                 { "Freq", SOURCE },
               }

local outputs = { "Left", "Right" }

local WingL = 0
local WingR = 0
local current_time = 0
local prev_time = 0
local flapping_time = 0
local GoUp = false  -- toggle between two half periods

local function init_func()
    flapping_time = 0
    prev_time = 0
    current_time = getTime()
    GoUp = false    -- first flap goes down
end

-- cyclic function, called approx. every 30ms
-- all inputs are from -1024 to 1024
local function run(Amplitude, Aileron, Elevator, Rudder, Frequency)
    
    -- Flapping period between 700ms and 230ms 
    -- half period is in units of 10ms and goes from 35 to 12.5
    local half_period = 12.5 + (1024 - Frequency) / 100
  
    prev_time = current_time
    current_time = getTime()  -- time in multiple of 10ms
    flapping_time = flapping_time + (current_time - prev_time) 
    
    if (Frequency > -980) then
        if (GoUp == false) then
            WingL = (1024 + Amplitude)/2  + Aileron + Elevator + Rudder
            WingR = (1024 + Amplitude)/-2 + Aileron - Elevator + Rudder
            
            if (flapping_time >= half_period) then
                GoUp = true
            end            
        else            
            WingL = (1024 + Amplitude)/-2 + Aileron + Elevator - Rudder
            WingR = (1024 + Amplitude)/2  + Aileron - Elevator - Rudder
        end    
    else
        -- gliding
        WingL = Aileron + Elevator
        WingR = Aileron - Elevator  
    end
  
    if (flapping_time >= 2*half_period) then -- one full flap is finished
        flapping_time = 0 -- start next flapping cycle
        GoUp = false
    end            
    
    return WingL, WingR
end

return { input=inputs, output=outputs, run=run, init=init_func }