Merge in the harder MAVlink changes.

Working version (mostly).  Not the smoothest telemetry, but it
works.