MPU-6050 initialization using nRF51822

mpu6050_extended.c

struct Mpu6050ControllerStruct {
  bool initialized;
};

typedef struct Mpu6050ControllerStruct * Mpu6050Controller;

static struct Mpu6050ControllerStruct instance = {
  .initialized = false,
};

bool _mpu6050_extended_init();

Mpu6050Controller get_mpu6050_instance() {
  if (!instance.initialized) {
    if (!_mpu6050_extended_init()) {
      return NULL;
    }
    instance.initialized = true;
  }
  return (Mpu6050Controller) &instance;
}

bool mpu6050_extended_register_write(Mpu6050Controller mpu6050_c, uint8_t register_address, const uint8_t value) {
  if (mpu6050_c == NULL) return false;
  return mpu6050_register_write(register_address, value);
}

bool mpu6050_extended_register_read(Mpu6050Controller mpu6050_c, uint8_t register_address, uint8_t *destination, uint8_t number_of_bytes) {
  if (mpu6050_c == NULL) return false;
  return mpu6050_register_read(register_address, destination, number_of_bytes);
}

bool mpu6050_extended_verify_product_id(Mpu6050Controller mpu6050_c) {
  if (mpu6050_c == NULL) return false;
  return mpu6050_verify_product_id();
}

bool mpu6050_extended_write_byte_bitwise(Mpu6050Controller mpu6050_c, uint8_t register_address, uint8_t bit_start, uint8_t length, uint8_t data) {
  if (mpu6050_c == NULL) return false;
  bool success;
  uint8_t reg_read_data;

  if (mpu6050_register_read(register_address, &reg_read_data, 1)) {
    uint8_t mask = ((1 << length) - 1) << ((bit_start + 1) - length);
    data <<= (bit_start + 1 - length); // shift data into correct position
    data &= mask; // zero all non-important bits in data
    reg_read_data &= ~(mask); // zero all important bits in existing byte
    reg_read_data |= data; // combine data with existing byte

    success = mpu6050_register_write(register_address, reg_read_data);
  } else {
    success = false;
  }

  return success;
}

bool mpu6050_extended_init_int_handler(Mpu6050Controller mpu6050_c, nrf_drv_gpiote_evt_handler_t handler) {
  if (mpu6050_c == NULL) return false;

  // GPIOTE CONFIGURATION STRUCTURE
  nrf_drv_gpiote_in_config_t config = GPIOTE_CONFIG_IN_SENSE_LOTOHI(false);
  config.pull = NRF_GPIO_PIN_PULLDOWN;

  // Hardware interruption for accelerometer.
  bool success = mpu6050_extended_register_write(mpu6050_c, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN | MPU6050_XG_FIFO_EN | MPU6050_YG_FIFO_EN | MPU6050_ZG_FIFO_EN); //Enable accel and gyro output to FIFO

  // Initializing GPIOTE.
  if (!nrf_drv_gpiote_is_init()) {
    if(nrf_drv_gpiote_init() != NRF_SUCCESS) {
      return false;
    }
  }

  if (!success || (nrf_drv_gpiote_in_init(MPU_6050_PIN_INT, &config, handler) != NRF_SUCCESS)) {
    return false;
  }

  // Enabling the ints
  nrf_drv_gpiote_in_event_enable(MPU_6050_PIN_INT, true);
  return mpu6050_extended_register_write(mpu6050_c, MPU6050_RA_INT_ENABLE, MPU6050_INT_FIFO_OVERFLOW_EN | MPU6050_INT_DATA_READY_EN);
}

bool _mpu6050_extended_init(void) {
  bool success;

  success = mpu6050_init(MPU_6050_ADDRESS);

  success = success && mpu6050_extended_write_byte_bitwise(&instance, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
  success = success && mpu6050_extended_write_byte_bitwise(&instance, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000);
  success = success && mpu6050_extended_write_byte_bitwise(&instance, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_16);

  success = success && mpu6050_extended_register_write(&instance, MPU6050_RA_SMPLRT_DIV, MPU6050_SMPLRT_DIV_80); //Sample Rate = 8khz/80 = 100hz

  success = success && mpu6050_extended_write_byte_bitwise(&instance, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, MPU6050_SINGLE_BIT_LENGTH, MPU6050_SLEEP_DISABLE);

  return success;
}