Add waiting for initialization of IMU before reading heading. Improve method naming...