diff --git a/main.c b/main.c
index 30af7d2..9cc5833 100644
--- a/main.c
+++ b/main.c
@@ -26,6 +26,9 @@ int main() {
         exit(1);
     }
 
+    mpu6050.cfg.gyro = MPU6050_GYRO_FS_2000;
+    mpu6050.cfg.acc = MPU6050_ACC_FS_16G;
+
     /* reset all signal paths */
     /* enable gyro, acc and temp */
     if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) {
@@ -60,7 +63,7 @@ int main() {
         exit(1);
     }
 
-    acc = MPU6050_ACC_FS_2G << 3;
+    acc = MPU6050_ACC_FS_16G << 3;
     if (i2c_write(REG_ACCEL_CONFIG, acc)) {
         exit(1);
     }
diff --git a/mpu6050.c b/mpu6050.c
index c09bb9d..3537f0a 100644
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -50,7 +50,7 @@ int mpu6050_read_acc(mpu6050_t *mpu6050) {
     shift = acc_i16_shift(mpu6050->cfg.acc);
 
     err |= mpu6050->dev.read(REG_ACCEL_XOUT_H, data, 6);
-    mpu6050->acc.x = ((int16_t)(data[0] << 8 | data[1])) >> shift;
+    mpu6050->acc.x = (int16_t)(data[0] << 8 | data[1]) >> shift;
     mpu6050->acc.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
     mpu6050->acc.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
 
@@ -60,10 +60,10 @@ int mpu6050_read_acc(mpu6050_t *mpu6050) {
 /* 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) {
-        case MPU6050_ACC_FS_2G:  return 2; /* 16384 LSb / g */
+        case MPU6050_ACC_FS_2G:  return 4; /* 16384 LSb / g */
         case MPU6050_ACC_FS_4G:  return 3; /* 8192 LSb / g */
-        case MPU6050_ACC_FS_8G:  return 4; /* 4096 LSb / g */
-        case MPU6050_ACC_FS_16G: return 5; /* 2048 LSb / g */
+        case MPU6050_ACC_FS_8G:  return 2; /* 4096 LSb / g */
+        case MPU6050_ACC_FS_16G: return 1; /* 2048 LSb / g */
         default:                 return 0;
     }
 }
\ No newline at end of file