General SGCS work

More fixes
Added MAVLinkProtocol