FW manual rudder control in stabilize

Adds manual rudder input in stabilized mode, allowing e.g. intentional slip/skid on landing.
Also adds function "perf latency" to print the latency bucket counters, and code to calculate mean and variance to the INTERVAL performance counter. (currently prints sqrt(variance))
Also work in progress to calculate a dynamic offset between baro alt and gps altitude.