diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c
index d1eeafdca..7b591451f 100644
--- a/flight/CopterControl/System/pios_board.c
+++ b/flight/CopterControl/System/pios_board.c
@@ -105,11 +105,12 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
 	.exti_cfg = &pios_exti_mpu6000_cfg,
 	.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
 	// Clock at 8 khz, downsampled by 8 for 1khz
-	.Smpl_rate_div = 15, 
+	.Smpl_rate_div = 15,
 	.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
 	.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
 	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
 	.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
+	.accel_range = PIOS_MPU6000_ACCEL_8G,
 	.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
 	.filter = PIOS_MPU6000_LOWPASS_256_HZ
 };
diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c
index ad6d3dbbf..421ff7117 100644
--- a/flight/PiOS/Common/pios_mpu6000.c
+++ b/flight/PiOS/Common/pios_mpu6000.c
@@ -155,7 +155,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
 	// FIFO storage
 #if defined(PIOS_MPU6000_ACCEL)
 	// Set the accel to 8g mode
-	while(PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, 0x10) != 0);
+	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, cfg->accel_range) != 0);
 	
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0);
 #else
@@ -332,7 +332,17 @@ float PIOS_MPU6000_GetScale()
 
 float PIOS_MPU6000_GetAccelScale()
 {
-	return GRAV / 2048.0f / 2.0f;
+	switch (dev->cfg->accel_range) {
+		case PIOS_MPU6000_ACCEL_2G:
+			return GRAV / 16384.0f;
+		case PIOS_MPU6000_ACCEL_4G:
+			return GRAV / 8192.0f;
+		case PIOS_MPU6000_ACCEL_8G:
+			return GRAV / 4096.0f;
+		case PIOS_MPU6000_ACCEL_16G:
+			return GRAV / 2048.0f;
+	}
+	return 0;
 }
 
 /**
@@ -340,11 +350,10 @@ float PIOS_MPU6000_GetAccelScale()
  * \return 0 if test succeeded
  * \return non-zero value if test succeeded
  */
-int32_t mpu6000_id;
 uint8_t PIOS_MPU6000_Test(void)
 {
 	/* Verify that ID matches (MPU6000 ID is 0x69) */
-	mpu6000_id = PIOS_MPU6000_ReadID();
+	int32_t mpu6000_id = PIOS_MPU6000_ReadID();
 	if(mpu6000_id < 0)
 		return -1;
 	
diff --git a/flight/PiOS/inc/pios_mpu6000.h b/flight/PiOS/inc/pios_mpu6000.h
index 6de8941e7..2a301bc4c 100644
--- a/flight/PiOS/inc/pios_mpu6000.h
+++ b/flight/PiOS/inc/pios_mpu6000.h
@@ -115,6 +115,13 @@ enum pios_mpu6000_filter {
 	PIOS_MPU6000_LOWPASS_5_HZ   = 0x06
 };
 
+enum pios_mpu6000_accel_range {
+	PIOS_MPU6000_ACCEL_2G = 0x00,
+	PIOS_MPU6000_ACCEL_4G = 0x08,
+	PIOS_MPU6000_ACCEL_8G = 0x10,
+	PIOS_MPU6000_ACCEL_16G = 0x18
+};
+
 struct pios_mpu6000_data {
 	int16_t gyro_x;
 	int16_t gyro_y;
@@ -136,6 +143,7 @@ struct pios_mpu6000_cfg {
 	uint8_t interrupt_en;	/* Interrupt configuration (See datasheet page 35 for more details) */
 	uint8_t User_ctl;		/* User control settings (See datasheet page 41 for more details)  */
 	uint8_t Pwr_mgmt_clk;	/* Power management and clock selection (See datasheet page 32 for more details) */
+	enum pios_mpu6000_accel_range accel_range;
 	enum pios_mpu6000_range gyro_range;
 	enum pios_mpu6000_filter filter;
 };