readGyro.cpp

ScaledData ReefwingLSM9DS1::readGyro() {
  RawData gyr;
  ScaledData result;

  //  Read the signed 16-bit RAW values
  gyr = readGyroRaw();

  //  Subtract the bias offsets
  gyr.rx -= _config.gyro.bias.x;
  gyr.ry -= _config.gyro.bias.y;
  gyr.rz -= _config.gyro.bias.z;

  //  Scale to DPS
  result.sx = gyr.rx * _gRes;
  result.sy = gyr.ry * _gRes;
  result.sz = gyr.rz * _gRes;

  return result;
}

RawData ReefwingLSM9DS1::readGyroRaw() {
  uint8_t regValue[6];
  RawData gyr;

  //  Read the six 8-bit gyro axis rate values
  readBytes(LSM9DS1AG_ADDRESS, LSM9DS1AG_OUT_X_L_G, 6, regValue);

  // Convert to the RAW signed 16-bit readings
  gyr.rx = (regValue[1] << 8) | regValue[0];
  gyr.ry = (regValue[3] << 8) | regValue[2];
  gyr.rz = (regValue[5] << 8) | regValue[4];

  return gyr;
}