mavlink optimization

- mavlink_onboard: UART reading major optimization, CPU usage reduced from 11% to 2% when using with PX4FLOW at 115200
- minor cleanup

WIP, don't merge.