Fix Mode_Switch bit shifting problems. Abondon Gyro, dead reconing.