https://github.com/diydrones/apm_planner/issues/788
Previous code was horribly wrong.
- Also added new adjustMavlinkMessage support