Bug fixes to SerialPeer. Made a software in the loop replacement for ardupilot