Integrated Speed and Height Control Algorithm

This patch implements am integrated speed and height controller using total energy principles. It has been derived from first principals using the basic concepts that throttle controls the rate of change of total energy and pitch angle controls the rate at which energy is exchanged between potential (height) and kinetic (speed). 

This  algorithm has undergone preliminary testing in the SITL with significantly reduced height and speed variation after tuning. A pdf block diagram description of the model is available on request.

Salient features of the algorithm are:

Height and speed demands are input directly. The algorithm can handle separate or simultaneous large step changes in demanded speed and height.

Vehicle performance is represented using a few simple parameters, eg:
- Max throttle climb rate
- Min throttle sink rate
- Max allowed throttle
- Min allowed throttle
- Max allowed speed
- Min allowed speed

Tuning the algorithm consists of adjusting a single time constant, with adjustment of a simple damping and integrator gain available for more advanced users.

A single parameter can be used to control the relative weighting of airspeed and height errors. This enables the following to be included:
- An  underspeed protection mode that gives speed priority if speed drops below the minimum and sets the throttle  to max to recover airspeed.
- A height only control mode that allows the algorithm to also be used when no airspeed sensor is fitted. This is used in conjunction with a simple rate of change of demanded total energy to throttle rule.

Complementary filters are used to fuse AHRS attitude, accelerometer, baro alt and airspeed measurements to obtain lower lag, lower noise estimates of height, height rate, airspeed and rate of change of airspeed.  This avoids the need to perform differentiation of potentially noisy signals with it associated problems. A second order filter is used for speed and a third order filter for height.

All of the height filter and some of the speed filter are run at 50Hz and need an update_50hz function to be called from the high speed loop. this avoids potential problems with aliasing of noise and also enables averaging to be applied to further reduce noise.