Fixed incorrect register addresses & added small test.