setGyroScale.cpp

void ReefwingLSM9DS1::setGyroScale(GyroScale scale) {
  uint8_t CTRL_REG1_G = readByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_CTRL_REG1_G);

  //  Clear the FS bits (3 & 4), maintain the rest
  //  0xE7 = 0b11100111, FS = 00, scale = 245 DPS
  CTRL_REG1_G &= 0xE7;

  switch(scale) {
    case GyroScale::FS_500DPS:
      CTRL_REG1_G |= (0x01 << 3);
      _config.gyro.scale = GyroScale::FS_500DPS;
      break;
    case GyroScale::FS_2000DPS:
      CTRL_REG1_G |= (0x03 << 3);
      _config.gyro.scale = GyroScale::FS_2000DPS;
      break;
    default:  //  Default scale is 245 DPS
      _config.gyro.scale = GyroScale::FS_245DPS;
      break;
  }

  writeByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_CTRL_REG1_G, CTRL_REG1_G);
}