Completed initialization function, implementing remainder of main sanity checks and magnetometer access setup.  Added magnetometer support to driver.  Added test code to test initialization function