diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c
index 6217715e4..ad611bb0c 100644
--- a/flight/Modules/Attitude/revolution/attitude.c
+++ b/flight/Modules/Attitude/revolution/attitude.c
@@ -288,6 +288,12 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
 		attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
 	}
 
+	// Hack for tweaking gyro gains with the old settings
+	scaling = gyroGain / 0.42;
+	attitudeRaw->gyros[ATTITUDERAW_GYROS_X] *= scaling;
+	attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] *= scaling;
+	attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] *= scaling;
+
 	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
 	// and make it average zero (weakly)
 	gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
@@ -333,18 +339,15 @@ float accel_mag;
 float qmag;
 static void updateAttitude(AttitudeRawData * attitudeRaw)
 {
-	float dT;
-	portTickType thisSysTime = xTaskGetTickCount();
-	static portTickType lastSysTime = 0;
+	static int32_t timeval;
+	float dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
+	timeval = PIOS_DELAY_GetRaw();
+	
 	float q[4];
 	
 	AttitudeActualData attitudeActual;
 	AttitudeActualGet(&attitudeActual);
 
-	
-	dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
-	lastSysTime = thisSysTime;
-
 	float gyro[3];
 	gyro[0] = attitudeRaw->gyros[0];
 	gyro[1] = attitudeRaw->gyros[1];
diff --git a/flight/PiOS/STM32F4xx/pios_mpu6000.c b/flight/PiOS/STM32F4xx/pios_mpu6000.c
index 2ceae6a88..9e677e0de 100644
--- a/flight/PiOS/STM32F4xx/pios_mpu6000.c
+++ b/flight/PiOS/STM32F4xx/pios_mpu6000.c
@@ -364,6 +364,31 @@ void PIOS_MPU6000_IRQHandler(void)
 		return;			
 	}
 	
+	// In the case where extras samples backed up grabbed an extra
+	if (mpu6000_count >= (sizeof(data) * 2)) {
+		if(PIOS_MPU6000_ClaimBus() != 0)
+			return;		
+		
+		uint8_t mpu6000_send_buf[1+sizeof(struct pios_mpu6000_data)] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
+		uint8_t mpu6000_rec_buf[1+sizeof(struct pios_mpu6000_data)];
+		
+		if(PIOS_SPI_TransferBlock(pios_spi_gyro, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
+			PIOS_MPU6000_ReleaseBus();
+			mpu6000_fails++;
+			return;
+		}
+		
+		PIOS_MPU6000_ReleaseBus();
+		
+		struct pios_mpu6000_data data;
+		
+		if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) {
+			mpu6000_fifo_full++;
+			return;			
+		}
+
+	}
+	
 	data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
 	data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
 	data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];