+ /* Set up the mag sensor */
+
+ /* make sure it's alive */
+ for (mag_tries = 0; mag_tries < MAG_TRIES; mag_tries++) {
+ if (_ao_mpu9250_mag_reg_read(MPU9250_MAG_WIA) == MPU9250_MAG_WIA_VALUE)
+ break;
+ }
+
+ if (mag_tries == MAG_TRIES)
+ ao_sensor_errors = 1;
+
+ /* Select continuous mode 2 (100Hz), 16 bit samples */
+
+ _ao_mpu9250_mag_reg_write(MPU9250_MAG_CNTL1,
+ (MPU9250_MAG_CNTL1_BIT_16 << MPU9250_MAG_CNTL1_BIT) |
+ (MPU9250_MAG_CNTL1_MODE_CONT_2 << MPU9250_MAG_CNTL1_MODE));
+
+ /* Set i2c master to delay shadowing data until read is
+ * complete (avoids tearing the data) */
+
+ _ao_mpu9250_reg_write(MPU9250_I2C_MST_DELAY_CTRL,
+ (1 << MPU9250_I2C_MST_DELAY_CTRL_DELAY_ES_SHADOW) |
+ (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV4_DLY_EN) |
+ (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV3_DLY_EN) |
+ (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV2_DLY_EN) |
+ (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV1_DLY_EN) |
+ (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV0_DLY_EN));
+
+ /* Set up i2c slave 0 to read the mag registers starting at HXL (3) */
+
+ _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_ADDR,
+ (1 << 7) | MPU9250_MAG_ADDR);
+
+ _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_REG,
+ MPU9250_MAG_HXL);
+
+ /* Byte swap so the mag values match the gyro/accel. Read 7 bytes
+ * to include the status register
+ */
+
+ _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_CTRL,
+ (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_EN) |
+ (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_BYTE_SW) |
+ (0 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_REG_DIS) |
+ (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_GRP) |
+ (MPU9250_MAG_ST2 - MPU9250_MAG_HXL + 1) << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_LENG);
+