diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
new file mode 100644
index 0000000000..d45273df4c
--- /dev/null
+++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
@@ -0,0 +1,51 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+//
+// Simple test for the AP_InertialSensor MPU6000 driver.
+//
+
+#include <FastSerial.h>
+#include <Wire.h>
+#include <SPI.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_PeriodicProcess.h>
+#include <AP_InertialSensor.h>
+#include <AP_Math.h>
+#include <AP_Common.h>
+
+FastSerialPort(Serial, 0);
+
+Arduino_Mega_ISR_Registry isr_registry;
+AP_TimerProcess  scheduler;
+AP_InertialSensor_MPU6000 ins( 53 ); /* chip select is pin 53 */
+
+void setup(void)
+{
+	Serial.begin(115200);
+	Serial.println("Doing INS startup...");
+
+	Wire.begin();
+    SPI.begin();
+    SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
+
+    isr_registry.init();
+    scheduler.init(&isr_registry);
+
+	ins.init(&scheduler);
+}
+
+void loop(void)
+{
+	float accel[3];
+	float gyro[3];
+	float temperature;
+
+	delay(20);
+	ins.update();
+	ins.get_gyros(gyro);
+	ins.get_accels(accel);
+	temperature = ins.temperature();
+
+	Serial.printf("AX: %f  AY: %f  AZ: %f  GX: %f  GY: %f  GZ: %f T=%f\n",
+				  accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], temperature);
+}
diff --git a/libraries/AP_InertialSensor/examples/MPU6000/Makefile b/libraries/AP_InertialSensor/examples/MPU6000/Makefile
new file mode 100644
index 0000000000..d1f40fd90f
--- /dev/null
+++ b/libraries/AP_InertialSensor/examples/MPU6000/Makefile
@@ -0,0 +1 @@
+include ../../../AP_Common/Arduino.mk