Prevent unstable controllers from rocketing copters into the sky.

This time with much more testing.

Note: this patch contains quite a few floating point operations that must run at 100hz. It might be possible to optimize it more.