Alt hold with LUA (PID)

[Diablol]

Hi

I like the idea to auto hold altitude with Vario data and mixers. At the moment I realized it with mixers. A proportional pitch is added to vario data.
And an "integral" pitch according to absolute hight. Well it kinda works!

I want to improve it with a real PI and PID controller with LUA.
similar to how the APM does it
https://code.google.com/p/arducopter/wi ... t_hold_PID

Unfortunately I will need some time to get more into LUA working with OpenTx.

So if anyone needs a task or has ideas I would appreciate. Else I hope to get results to present soon.

[bertrand35]

It shouldn't be too hard, let-me know if you need help!

[tilmanb]

I wonder if the latency might be a problem.

[Diablol]

help

my programming skills are very old I realized.

The problem is that I only can test on the target and mostly I get errors..and I dont know why.
I am still trying to only get values like the altitude. ..and return them or just do anything..lol

If you could tell me how a script start would look like that reads continiously the alt and would add pitch if a switch is activated...like that.
Or maybe you have further ideas. I think I need hours or days to understand more. Maybe you need some seconds

thanks for the support! I just need a start..

I wonder if the latency might be a problem.

sure it is

but it is more for fun and to learn. But in a small window it works I think.

[bertrand35]

I don't think you will have to much latency, but we will measure it to be sure! Do you need a sketch for the script? Which are the inputs and outputs? I can prepare everything and you will work on the maths between the inputs and outputs!

[Diablol]

Oh that would be very kind.

For a start I would like to have the altitude and Pitch as input.
I will calculate the PID with that and the result will be a new pitch (as output)

further I would need at least 4 Gvars for inflight PID tuning.

and what I havent understood as well is how to activate the script/math with a switch...

thanks

[bertrand35]

Ok I will prepare something for you

[bertrand35]

Something like this?

Code: Select all

local inputs = { { "Src example", SOURCE },
                 { "Ratio", VALUE, -100, 100, 0 },
               }
                 
local outputs = { "Ptch" }

local currentPitch = 0

local function run(srcExample, ratio)
  -- relative altitude (unit = centimeters)
  local altitude = getValue("altitude")
  local pitch = currentPitch
  
  -- here you calculate the pitch using the previous value (currentPitch) and the altitude
  if altitude < 990 then
    pitch = currentPitch + 1
  elseif altitude > 1010 then
    pitch = currentPitch - 1
  end
  
  -- here you save the pitch for the next run
  currentPitch = pitch
  
  -- and at the end you return the new pitch :)
  return pitch    
end

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

And as I like screenshots, here is the result:

snapshot_01.png (1.69 KiB) Viewed 16883 times

[Diablol]

looks good! And I think I see what I did wrong..

thank you again... now it is my turn

[Diablol]

What do you think will be the update frequency? Lua all 30ms? Variometer data 10hz or faster? All together?

[Kilrah]

Sensor data comes through telemetry at a variable, unknown interval depending on what sensors are connected to the bus, link quality etc...
Can pretty much be anywhere between about 2 and 20Hz and change continuously.

[bertrand35]

The frequency of the Lua scripts should be displayed somewhere so that you can check at any time. I will add it.

[bertrand35]

I just added some metrics to the stats screen. A simple Delta mix written in a Lua script runs every 20ms. I guess it will be almost the same for yours.

[bertrand35]

I just added some metrics to the Debug screen. A simple Delta mix written in a Lua script runs every 20ms. I guess it will be almost the same for yours.

[Diablol]

If the radio gets telemetry data, does it then gets all data or is it possible that it gets one time only vario or only altitude?
Do you know what I mean?

That decides if it would be usefull to use vario data additionly to altimeter. But I think it is the same..?
First tests were positive.
A pity that theres no smart port accelerometer out there yet, that would improve it a lot...very fun to program thoser controllers though..

[bertrand35]

I am not sure that I understand what you are asking for, do you need the vario (VSpeed) as input to the Lua algorithm?

[Diablol]

currently I just use the altitude for corrections.
theoretically the control loop gets better the more inputs you use. Like altitude, vario, acceleration...

So if I would use the vario additionally it would be better, in therorie.
But the vario data emerges from the altitude I think, it is the same sensor.(?) So it wouldnt make any difference. But it would do differnce if I get data more often it I use the vario as well.

But if the telemetrie data contains always all data it makes no difference again..

hard to explain I think.

[bertrand35]

Ok I see, and no vario is calculated inside the sensor, with more altitude samples than the one we receive through the S.PORT.
It's already accessible, but I just introduced a getValue("vario") function at it will simplify things.

[Diablol]

ok. that means an implementaion of the vario data could improve the performance. I am on it..

[Diablol]

getValue("vario") isnt working yet, right? I understood you wrong. So I wait for it.

Here my second version with another loop with vario integrated.

btw. is it right I could add a frsky accelerometer to a smartport vario?

Code: Select all

 ----------Alitdude Hold Loop------------
     
     local inputs = { { "Pitch CH", SOURCE }, --CH3
                     { "Thr_P", SOURCE },    --GV1  0.4
                     { "Rate_P", SOURCE },   --GV2  0.3
                     { "Rate_I", SOURCE },   --GV3  0.0 
                     { "Rate_D", SOURCE },   --GV4  0.002
                     { "Max", VALUE, -100, 100, 0 },
                   }
                     
  --  local outputs = { "Ptch","P","I","D","sumE","alti" }
    local outputs = { "Ptch","sumE","alti","old" }
      
   
    local rate_error_old = 0
    local sumError = 0
    --local rate = 0
       
    local function run(srcExample, tp, rp, ri, rd, ratio)
       
    
        local thr_p = tp/100
		local rate_p= rp/100
		local rate_i = ri/100
		local rate_d = rd/1000
		
	     
      local altitude = getValue("altitude")*100 -- in cm
      
                
      --requires telemetry reset -> error=alt
      local error = altitude
      
      -- get desired climb rate
      local climb_rate = error * thr_p
      
      -- limit climb rate
      if climb_rate > 250 then climb_rate = 250
      end
      if climb_rate < 250 then climb_rate = 250
      end 
   
      -- Rate Error
      local rate = getValue("vario")*100       --waiting for it
      local rate_error = climb_rate - rate
      
      --summed error
      sumError = sumError + rate_error
            
      --calculate Pitch
      local PitchP = rate_error * rate_p
      local PitchI = sumError * rate_i
      local PitchD = (rate_error - rate_error_old) * rate_d
      local Pitch = PitchP + PitchI + PitchD
      
      rate_error_old = rate_error
      
      
      --limit pitch
     if Pitch >= ratio * 10 then Pitch = ratio*10
     end	
     if Pitch <= ratio * (-10) then Pitch = ratio*(-10)
     end
      
      return Pitch, sumError, error, rate_error_old --debug monitor
    end

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

I am no pro programmer so excuse a maybe dirty code.

[tilmanb]

What is next? GPS based IFL landing?

[Diablol]

not completely impossible ..

I would like to do GPS hold.. but we have no Compass and the update rate of frsky GPS is very low..

[bertrand35]

May I take the token for your script, I change it a little and upload here so you can continue, ok?

[Diablol]

go ahead

[Diablol]

getValue(STICK_THROTTLE) results in an error, possible?

[bertrand35]

I didn't have time to modify anything but I have a couple of suggestions
1) you add a switch as input to start the mechanism
- in the run method, you could check that this switch is enabled, if not enabled you store the altitude as a reference
- you retrieve the current altitude and you compare it with your reference
2) climb rate is always 250?

[Diablol]

1) yes I will do that ..still in testing stage
2) climb rate is limited to 250 means max 1m/s

some questions from me:

1) I cant have more than 6 inputs? How many outs?
2) getValue(STICK_...) results in error
3) sometimes the memory in opentx runs in loops from 0 to 256, sometimes/some codes not?

[bertrand35]

1) 10 inputs, 6 outputs
2) you have to call getValue(MIXSRC_Ele)
3) depends on your code, I don't think you will reach the limit with this script, the wizard goes up to ~20KBytes ...

[Diablol]

ok thanks.
But if I have e.g. 7 inputs only 6 are shown for select. 8 or more inputs result in Opentx crash at startup. Only battery disconnections helps.

[bertrand35]

Good catch!
https://github.com/opentx/opentx/commit ... 49af38b321