setGyroODR.cpp

void ReefwingLSM9DS1::setGyroODR(GyroODR rate) {
  uint8_t CTRL_REG1_G = readByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_CTRL_REG1_G);

  //  Clear the ODR_G bits (7, 6 & 5), maintain the rest
  //  0x1F = 0b00011111, ODR_G = 000, mode = power-down
  CTRL_REG1_G &= 0x1F;

  _config.gyro.powerDown = false;
  _config.gyroAccelOpMode = NORMAL;

  switch(rate) {
    case GyroODR::GODR_14_9Hz:   //  ODR_G = 001
      CTRL_REG1_G |= (0x01 << 5);
      if (_config.gyro.lowPowerEnabled) {
        _config.gyroAccelOpMode = LOW_POWER;
      }
      if (_config.accel.autoBandwidthEnable) {
        _config.accel.bandwidth = BW_408Hz;
      }
      break;
    case GyroODR::GODR_59_5Hz:   //  ODR_G = 010
      CTRL_REG1_G |= (0x02 << 5);
      if (_config.gyro.lowPowerEnabled) {
        _config.gyroAccelOpMode = LOW_POWER;
      }
      if (_config.accel.autoBandwidthEnable) {
        _config.accel.bandwidth = BW_408Hz;
      }
      break;
    case GyroODR::GODR_119Hz:   //  ODR_G = 011
      CTRL_REG1_G |= (0x03 << 5);
      if (_config.gyro.lowPowerEnabled) {
        _config.gyroAccelOpMode = LOW_POWER;
      }
      if (_config.accel.autoBandwidthEnable) {
        _config.accel.bandwidth = BW_50Hz;
      }
      break;
    case GyroODR::GODR_238Hz:   //  ODR_G = 100
      CTRL_REG1_G |= (0x04 << 5);
      if (_config.accel.autoBandwidthEnable) {
        _config.accel.bandwidth = BW_105Hz;
      }
      break;
    case GyroODR::GODR_476Hz:   //  ODR_G = 101
      CTRL_REG1_G |= (0x05 << 5);
      if (_config.accel.autoBandwidthEnable) {
        _config.accel.bandwidth = BW_211Hz;
      }
      break;
    case GyroODR::GODR_952Hz:   //  ODR_G = 110
      CTRL_REG1_G |= (0x06 << 5);
      if (_config.accel.autoBandwidthEnable) {
        _config.accel.bandwidth = BW_408Hz;
      }
      break;
    default:  //  ODR_G = 000, default rate is power-down
      _config.gyro.powerDown = true;
      _config.gyroAccelOpMode = ACCELEROMETER_ONLY;
      if (_config.accel.powerDown) {
        _config.gyroAccelOpMode = POWERED_DOWN;
      }
      break;
  }

  _config.gyro.sampleRate = rate;
  if (_config.gyroAccelOpMode != ACCELEROMETER_ONLY) {
    _config.accel.sampleRate = AODR_GYRO_VALUE;
  }
  writeByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_CTRL_REG1_G, CTRL_REG1_G);
}