Florin and Peter,
(Just to save Matt some work, Matt I looked at the code, I will try to help Florin.)
I see that Matt has some altitude/speed control code that he developed, but you need to specifically select his code. Otherwise, you get the code that I developed, which you can look at here:
Here is how it works:
For throttle control, there is an error signal that is developed that is equal to the sum of the altitude error plus the equivalent height of the speed error. Normally, this signal will put the throttle to full throttle during a climbout, because both height and speed are below target values.
For pitch control, there is an error signal that is the difference between altitude error minus the speed error.
So, if you set the target speed too high, here is what I would expect would happen: The throttle would go to maximum, but pitch control would direct the plane downward to gain some speed.
So, I mis-spoke in one of my earlier emails to Florin: the controls use both throttle and pitch together to control altitude and speed together. If you specify a reasonable value of target speed, then altitude control will take precedence. But if the speed target is too large, then I would expect the plane to descend until the altitude error was equal to the energy of the speed error.
A numerical example might help.
Suppose you set a target of 100 meters/second for a plane that could only achieve 10 meters/second. The equivalent height of the speed deficit is 495 meters!!!
So, the plane would fly 495 meters below the target height in an effort to gain speed!
Lets take a more reasonable example, where the target speed is closer to what the plane will achieve. Suppose the target is 12 meters/second, and the plane is flying at 10 meters/second. In that case, the height of the energy deficit is only 2.2 meters, so the plane will only drop 2.2 meters in an effort to pick up speed.
So, the bottom line is you must set a reasonable value for the target speed if you wish to use both altitude and speed control, because the controls are based on total energy.
Best regards,
Bill