selfTestMag.cpp

MagTestResults ReefwingLSM9DS1::selfTestMag() {
  MagTestResults results;
  BiasOffsets mag_noST, mag_ST;

  //  Write 0x1C = 0b0001 1100 to CTRL_REG1_M
  //  ODR = 80 Hz, OM = Low Performance, FAST_ODR disabled, Self Test disabled
  writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x1C);

  //  Write 0x40 = 0b0100 0000 to CTRL_REG2_M
  //  FS = ± 12 gauss
  writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, 0x40);
  delay(20);

  //  Write 0x00 to CTRL_REG3_M
  //  Low Power Mode Disabled, Op Mode = Continuous
  writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00);
  delay(20);

  mag_noST = averageMagOffsets();

  //  Write 0x1D = 0b0001 1101 to CTRL_REG1_M
  //  ODR = 80 Hz, OM = Low Performance, FAST_ODR disabled, Self Test enabled
  writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x1D);
  delay(60);

  mag_ST = averageMagOffsets();

  //  Calculate difference and scale to _mRes = ± 12 gauss
  results.magdx = (mag_ST.x - mag_noST.x) * SENSITIVITY_MAGNETOMETER_12;
  results.magdy = (mag_ST.y - mag_noST.y) * SENSITIVITY_MAGNETOMETER_12;
  results.magdz = (mag_ST.z - mag_noST.z) * SENSITIVITY_MAGNETOMETER_12;

  writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x1C);  //  Disable Self Test
  writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x03);  //  Power Down mode

  return results;
}