Removed more test code. Initialise yaw, pitch and roll defaults on every reset.