diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
index 76c367fada..a1ff95ac38 100644
--- a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
+++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
@@ -1,12 +1,21 @@
 
 #include <stdint.h>
+#include <AP_Common.h>
+#include <AP_Math.h>		// ArduPilot Mega Vector/Matrix math Library
 #include <FastSerial.h>
+#include <I2C.h>
 #include <SPI.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_PeriodicProcess.h>
 #include <AP_Baro.h> // ArduPilot Mega ADC Library
 
 FastSerialPort0(Serial);
 
 AP_Baro_MS5611 baro;
+Arduino_Mega_ISR_Registry isr_registry;
+AP_TimerProcess  scheduler;
+
+unsigned long timer;
 
 void setup()
 {  
@@ -15,54 +24,41 @@ void setup()
 
 	delay(1000);
 
-  pinMode(63, OUTPUT);
-  digitalWrite(63, HIGH);
-  SPI.begin();
-  SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later
+	isr_registry.init();
+	scheduler.init(&isr_registry);
 
-  baro.init();
+	pinMode(63, OUTPUT);
+	digitalWrite(63, HIGH);
+	SPI.begin();
+	SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later
+
+	baro.init(&scheduler);
+	timer = micros();
 }
 
 void loop()
 {
-    int32_t pres;
-    int32_t temp;
-
-    Serial.println("Start Conversions");
-
-    baro._start_conversion_D1();
-    delay(10);
-    bool res1 = baro._adc_read(&pres);
-    baro._start_conversion_D2();
-    delay(10);
-    bool res2 = baro._adc_read(&temp);
-
-    if (res1) {
-        Serial.printf("Pressure raw value %ld\r\n",pres);
-    } else {
-        Serial.println("ADC conversion D1 unsuccessful");
-    }
-
-    if (res2) {
-        Serial.printf("Temp raw value %ld\r\n",pres);
-    } else {
-        Serial.println("ADC conversion D2 unsuccessful");
-    }
-    
-    Serial.println("---");
-    delay(250);
-}
-
-void update_and_print()
-{
-    int32_t pres;
-    float temp;
-
-    baro.update();
-
-    pres = baro.get_pressure();
-    temp = baro.get_temp();
-    Serial.printf("p: %ld t: %f \r\n", pres, temp);
-
-    delay(100);
+	float tmp_float;
+	float Altitude;
+	
+	if((micros()- timer) > 50000L){
+		timer = micros();
+		baro.read();
+		unsigned long read_time = micros() - timer;
+		if (!baro.healthy) {
+			Serial.println("not healthy");
+			return;
+		}
+		Serial.print("Pressure:");
+		Serial.print(baro.get_pressure());
+		Serial.print(" Temperature:");
+		Serial.print(baro.get_temperature());
+		Serial.print(" Altitude:");
+		tmp_float = (baro.get_pressure() / 101325.0);
+		tmp_float = pow(tmp_float, 0.190295);
+		Altitude = 44330 * (1.0 - tmp_float);
+		Serial.print(Altitude);
+		Serial.printf(" t=%u", (unsigned)read_time);
+		Serial.println();
+	}
 }