This commit is a working copy of the code tested on the APM board.