CORE: Fixed some bugs related to MAVlink and our Coord2D Library
Merge pull request #735 from arthurbenemann/CoordBugFix

URGENT bugfix of MAVLink to Coord2D conversion