Fixed error getting YPR - This bug introduces bad values into the system