diff --git a/main.c b/main.c
index cd8e675..0a1f1f8 100644
--- a/main.c
+++ b/main.c
@@ -23,68 +23,34 @@ int main() {
 
     mpu6050.cfg.gyro = MPU6050_GYRO_FS_250;
     mpu6050.cfg.acc = MPU6050_ACC_FS_2G;
+    mpu6050.cfg.dlpl = 6;
+    mpu6050.cfg.sdiv = 200;
     mpu6050.cfg.int_enable.data_rdy = 1;
 
-    /* reset all signal paths */
-    /* enable gyro, acc and temp */
-    if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) {
+    if (mpu6050_configure(&mpu6050)) {
         exit(1);
     }
 
-    /* set sample rate divisor to 150 */
-    /* if LP-filter = 0 or 7 => sample rate = 8 Khz, otherwise 1 Khz */
-    if (i2c_write(REG_SMPLRT_DIV, 150)) {
-        exit(1);
+    for ( ;; ) {
+
+        if (mpu6050_read(&mpu6050)) {
+            exit(1);
+        }
+
+        printf("[GYRO °/s] x:%4.1f  y:%4.1f  z:%4.1f ",
+            (float)mpu6050.data.gyro.x / 10.f,
+            (float)mpu6050.data.gyro.y / 10.f,
+            (float)mpu6050.data.gyro.z / 10.f);
+
+        printf("[ACC g] x:%4.3f  y:%4.3f  z:%4.3f ",
+            (float)mpu6050.data.acc.x / 1000.f,
+            (float)mpu6050.data.acc.y / 1000.f,
+            (float)mpu6050.data.acc.z / 1000.f);
+
+        printf("t:%4.1f°C\n", (float)mpu6050.data.temp / 10.f);
     }
 
-    /* set up digital LP-filter */
-    if (i2c_write(REG_CONFIG, 0x05)) {
-        exit(1);
-    }
-
-    /* set sleep to 0 */
-    if (i2c_write(REG_PWR_MGMT1, 0x00)) {
-        exit(1);
-    }
-
-    /* enable DATA_RDY interrupt (polling for now) */
-    /* active when internal writes to all out data regs are done */
-    if (i2c_write(REG_INT_ENABLE, mpu6050.cfg.int_enable.data_rdy & 1)) {
-        exit(1);
-    }
-
-    if (i2c_write(REG_GYRO_CONFIG, mpu6050.cfg.gyro << 3)) {
-        exit(1);
-    }
-
-    if (i2c_write(REG_ACCEL_CONFIG, mpu6050.cfg.acc << 3)) {
-        exit(1);
-    }
-
-    usleep(200 * 1000);
-
-LOOP:
-
-    if (mpu6050_read(&mpu6050)) {
-        exit(1);
-    }
-
-    printf("[GYRO °/s] x:%4.1f  y:%4.1f  z:%4.1f ",
-        (float)mpu6050.data.gyro.x / 10.f,
-        (float)mpu6050.data.gyro.y / 10.f,
-        (float)mpu6050.data.gyro.z / 10.f);
-
-    printf("[ACC g] x:%4.3f  y:%4.3f  z:%4.3f ",
-        (float)mpu6050.data.acc.x / 1000.f,
-        (float)mpu6050.data.acc.y / 1000.f,
-        (float)mpu6050.data.acc.z / 1000.f);
-
-    printf("t:%4.1f°C\n", (float)mpu6050.data.temp / 10.f);
-
-    usleep(100 * 1000);
-    goto LOOP;
-
-    i2c_deinit();
+    mpu6050_deinit(&mpu6050);
 
     return 0;
 }
\ No newline at end of file
diff --git a/mpu6050.c b/mpu6050.c
index a162ca0..efa8ca3 100644
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -86,9 +86,9 @@ int mpu6050_read_gyro(mpu6050_t *mpu6050) {
 
     err |= mpu6050->dev.read(REG_GYRO_XOUT_H, data, 6);
 
-    mpu6050->data.gyro.x = ((int16_t)(data[0] << 8 | data[1]) >> shift);
-    mpu6050->data.gyro.y = ((int16_t)(data[2] << 8 | data[3]) >> shift);
-    mpu6050->data.gyro.z = ((int16_t)(data[4] << 8 | data[5]) >> shift);
+    mpu6050->data.gyro.x = (int16_t)(data[0] << 8 | data[1]) >> shift;
+    mpu6050->data.gyro.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
+    mpu6050->data.gyro.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
 
     return err;
 }
@@ -133,13 +133,59 @@ int mpu6050_read(mpu6050_t *mpu6050) {
     /* 6-7 temp */
     mpu6050->data.temp = (int16_t)(data[6] << 8 | data[7])/34 + 365;
     /* 8-13 gyro */
-    mpu6050->data.gyro.x = ((int16_t)(data[ 8] << 8 | data[ 9]) >> shift_gyro);
-    mpu6050->data.gyro.y = ((int16_t)(data[10] << 8 | data[11]) >> shift_gyro);
-    mpu6050->data.gyro.z = ((int16_t)(data[12] << 8 | data[13]) >> shift_gyro);
+    mpu6050->data.gyro.x = (int16_t)(data[8] << 8 | data[9]) >> shift_gyro;
+    mpu6050->data.gyro.y = (int16_t)(data[10] << 8 | data[11]) >> shift_gyro;
+    mpu6050->data.gyro.z = (int16_t)(data[12] << 8 | data[13]) >> shift_gyro;
 
     return err;
 }
 
+/* write configuration to device */
+/* then sleep for 200 ms */
+int mpu6050_configure(mpu6050_t *mpu6050) {
+    uint8_t sig_path, dlpl, sleep, inten;
+    uint8_t acc, gyro;
+    int err = 0;
+    
+    assert(mpu6050);
+
+    /* enable accel, gyro and temp */
+    sig_path = 0x07;
+
+    /* digital low-pass filter level */
+    dlpl = mpu6050->cfg.dlpl & 0x07;
+
+    /* INT_ENABLE */
+    inten = mpu6050->cfg.int_enable.data_rdy & 1;
+    inten |= (mpu6050->cfg.int_enable.i2c_mst & 1) << 3;
+    inten |= (mpu6050->cfg.int_enable.fifo_overflow & 1) << 4;
+    inten |= (mpu6050->cfg.int_enable.mot & 1) << 6;
+
+    /* PWR_MGMT1 */
+    sleep = 0;
+
+    /* gyro */
+    gyro = (mpu6050->cfg.gyro & 0x02) << 3;
+
+    /* accelerometer */
+    acc = (mpu6050->cfg.acc & 0x02) << 3;
+
+    err |= mpu6050->dev.write(REG_SMPLRT_DIV, mpu6050->cfg.sdiv);
+    err |= mpu6050->dev.write(REG_SIGNAL_PATH_RESET, sig_path);
+    err |= mpu6050->dev.write(REG_INT_ENABLE, inten);
+    err |= mpu6050->dev.write(REG_CONFIG, dlpl);
+    err |= mpu6050->dev.write(REG_ACCEL_CONFIG, acc);
+    err |= mpu6050->dev.write(REG_GYRO_CONFIG, gyro);
+    err |= mpu6050->dev.write(REG_PWR_MGMT1, sleep);
+
+    if (!err) {
+        mpu6050->dev.sleep(200000); /* 200 ms */
+    }
+
+    return err;
+}
+
+
 /* How much to shift down an i16 accel sample depending on full-scale mode set */
 static uint8_t acc_i16_shift(uint8_t fs) {
     switch(fs) {
diff --git a/mpu6050.h b/mpu6050.h
index 104d495..bc128c1 100644
--- a/mpu6050.h
+++ b/mpu6050.h
@@ -35,6 +35,8 @@ struct mpu6050_int_enable {
 struct mpu6050_config {
     uint8_t gyro;
     uint8_t acc;
+    uint8_t dlpl; /* digital low-pass filter level [0-7]*/
+    uint8_t sdiv; /* sample rate divider. ~ lpl; lpl=(0,7) => divides 8KHz else 1 KHz*/
     struct mpu6050_int_enable int_enable;
 };
 
@@ -73,5 +75,6 @@ int mpu6050_read_acc(mpu6050_t *mpu6050);
 int mpu6050_read_gyro(mpu6050_t *mpu6050);
 int mpu6050_read_temp(mpu6050_t *mpu6050);
 int mpu6050_read(mpu6050_t *mpu6050);
+int mpu6050_configure(mpu6050_t *mpu6050);
 
 #endif
\ No newline at end of file