Refactored part of MAVLinkProtocol::receiveBytes() as I was trying to debug within it. Nothing major, though I did remove an unnecessary loop.