Implement YPR with FreeIMU code