The ms5611 driver has been partly rewritten to enable pressure measurements at 50-100hz.
All pressure readings are converted to an altitude above sea level. When entering hover mode, we set the target altitude.
We can then define a PID controller that should take the flie from its current altitude to the target altitude. The P part is just the difference, eg 1 meter too high. For the D term we use the vertical speed…here the code is ugly. First one needs to compute the vertical acceleration, then subtract gravity. This vertical acceleration is then integrated to get a speed estimate. To stop it accumulating error forever, it converges to the speed estimate from the barometer. This is also computed in a non mathematical way: some factor * (super_smoothed_altitude-smoothed_altitude). The I term is just the integrated error - and is very very very important as it makes up for the voltage drop. The P and D term are reset every time hover mode is entered, and the I term is only reset when you start charging the flie. The default I value right now is set up to be a pretty good value for a stock flie at 80% battery. The default values takes around 1-2 to converge on a flie with a depleted battery during which time the flie might oscillate within a meter range or so.
Note that hover mode only works well in pressure stable environments. Trying to hover with people opening/closing windows/doors or during a thunderstorm does not work very well!