diff --git a/flight/INS/ins.c b/flight/INS/ins.c
index e1fd1e940..caf205092 100644
--- a/flight/INS/ins.c
+++ b/flight/INS/ins.c
@@ -607,8 +607,6 @@ bool get_accel_gyro_data()
 	int32_t accel_accum[3] = {0, 0, 0};
 	accel_samples = 0;
 	
-	int16_t gyro[3];
-
 	t_fifo_buffer * accel_fifo = PIOS_BMA180_GetFifo();
 	while(fifoBuf_getUsed(accel_fifo) < sizeof(accel));
 	while(fifoBuf_getUsed(accel_fifo) >= sizeof(accel)) {	
@@ -622,7 +620,20 @@ bool get_accel_gyro_data()
 	accel[1] = accel_accum[1] / accel_samples;
 	accel[2] = accel_accum[2] / accel_samples;
 	
-	PIOS_IMU3000_ReadGyros(gyro);
+	int16_t gyro_fifo[4 * 100];
+	uint8_t gyro_len;
+	int32_t gyro_accum[3] = {0, 0, 0};
+	int16_t gyro[3];	
+	gyro_len = PIOS_IMU3000_ReadFifo((uint8_t *) gyro_fifo, sizeof(gyro_fifo));
+	gyro_len /= 8;
+	for(int i = 0; i < gyro_len; i++) {
+		gyro_accum[0] += gyro_fifo[i * 4];
+		gyro_accum[1] += gyro_fifo[1 + i * 4];
+		gyro_accum[2] += gyro_fifo[2 + i * 4];
+	}
+	gyro[0] = gyro_accum[0] / gyro_len;
+	gyro[1] = gyro_accum[1] / gyro_len;
+	gyro[2] = gyro_accum[2] / gyro_len;
 	
 	// Not the swaping of channel orders
 	accel_data.filtered.x = accel[0] * PIOS_BMA180_GetScale();
diff --git a/flight/PiOS/Common/pios_imu3000.c b/flight/PiOS/Common/pios_imu3000.c
index d19ac8883..e9774fc98 100644
--- a/flight/PiOS/Common/pios_imu3000.c
+++ b/flight/PiOS/Common/pios_imu3000.c
@@ -156,6 +156,69 @@ int32_t PIOS_IMU3000_ReadID()
 	return id;
 }
 
+/**
+ * @brief Reads the data from the IMU3000 FIFO
+ * \param[out] buffer destination buffer
+ * \param[in] len maximum number of bytes which should be read
+ * \return number of bytes transferred if operation was successful
+ * \return -1 if error during I2C transfer
+ */
+int32_t PIOS_IMU3000_ReadFifo(uint8_t * buffer, uint16_t len)
+{
+	uint16_t fifo_level;
+
+	uint8_t addr_buffer[] = {
+		0x3A,
+	};
+	
+
+	const struct pios_i2c_txn txn_list[] = {
+		{
+			.info = __func__,
+			.addr = PIOS_IMU3000_I2C_ADDR,
+			.rw = PIOS_I2C_TXN_WRITE,
+			.len = sizeof(addr_buffer),
+			.buf = addr_buffer,
+		}
+		,
+		{
+			.info = __func__,
+			.addr = PIOS_IMU3000_I2C_ADDR,
+			.rw = PIOS_I2C_TXN_READ,
+			.len = 2,
+			.buf = (uint8_t *) &fifo_level,
+		}
+	};
+	
+	// Get the number of bytes in the fifo
+	PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list, NELEMENTS(txn_list));
+	addr_buffer[0] = 0x3C;
+	
+	
+	if(len > fifo_level)
+		len = fifo_level;
+	len &= 0x01f8; // only read chunks of 8 bytes (includes footer)
+	
+	const struct pios_i2c_txn txn_list2[] = {
+		{
+			.info = __func__,
+			.addr = PIOS_IMU3000_I2C_ADDR,
+			.rw = PIOS_I2C_TXN_WRITE,
+			.len = sizeof(addr_buffer),
+			.buf = addr_buffer,
+		}
+		,
+		{
+			.info = __func__,
+			.addr = PIOS_IMU3000_I2C_ADDR,
+			.rw = PIOS_I2C_TXN_READ,
+			.len = len,
+			.buf = buffer,
+		}
+	};	
+	return PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list2, NELEMENTS(txn_list)) ? len : -1;
+}
+
 /**
 * @brief Reads one or more bytes from IMU3000 into a buffer
 * \param[in] address IMU3000 register address (depends on size)
diff --git a/flight/PiOS/inc/pios_imu3000.h b/flight/PiOS/inc/pios_imu3000.h
index 04666729f..2676ca799 100644
--- a/flight/PiOS/inc/pios_imu3000.h
+++ b/flight/PiOS/inc/pios_imu3000.h
@@ -119,6 +119,7 @@ struct pios_imu3000_data {
 /* Public Functions */
 extern void PIOS_IMU3000_Init(void);
 extern bool PIOS_IMU3000_NewDataAvailable(void);
+extern int32_t PIOS_IMU3000_ReadFifo(uint8_t * buffer, uint16_t len);
 extern int32_t PIOS_IMU3000_ReadGyros(int16_t * data);
 extern int32_t PIOS_IMU3000_ReadID();
 extern uint8_t PIOS_IMU3000_Test();