code merge from razor-9dof-ahrs and RCencoder