Work on tests, refactor of serial driver somewhat