diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 49f4736fe7..6554751b71 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -406,7 +406,6 @@ static uint8_t 	ground_start_count	= 5;
 // on the ground or in the air.  Used to decide if a ground start is appropriate if we
 // booted with an air start.
 static int16_t     ground_start_avg;
-static int32_t          gps_base_alt;		
 
 ////////////////////////////////////////////////////////////////////////////////
 // Location & Navigation
@@ -510,7 +509,7 @@ static int16_t 		condition_rate;
 // Location structure defined in AP_Common
 ////////////////////////////////////////////////////////////////////////////////
 // The home location used for RTL.  The location is set when we first get stable GPS lock
-static struct 	Location home;
+static const struct	Location &home = ahrs.get_home();
 // Flag for if we have g_gps lock and have set the home location
 static bool	home_is_set;
 // The location of the previous waypoint.  Used for track following and altitude ramp calculations
@@ -572,6 +571,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { set_servos,             1,   1500 },
     { update_GPS_50Hz,        1,   2500 },
     { update_GPS_10Hz,        5,   2500 },
+    { update_alt,             5,   3400 },
     { navigate,               5,   1600 },
     { update_compass,         5,   2000 },
     { update_commands,        5,   1000 },
@@ -684,6 +684,14 @@ static void mount_update(void)
 #endif
 }
 
+static void update_alt()
+{
+    barometer.read();
+    if (should_log(MASK_LOG_IMU)) {
+        Log_Write_Baro();
+    }
+}
+
 /*
   check for GCS failsafe - 10Hz
  */
diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde
index c73174d699..438696f6d6 100644
--- a/APMrover2/Log.pde
+++ b/APMrover2/Log.pde
@@ -519,6 +519,11 @@ static void Log_Write_RC(void)
     DataFlash.Log_Write_RCOUT();
 }
 
+static void Log_Write_Baro(void)
+{
+    DataFlash.Log_Write_Baro(barometer);
+}
+
 static const struct LogStructure log_structure[] PROGMEM = {
     LOG_COMMON_STRUCTURES,
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 480373dd9a..ff2da2e84a 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -168,6 +168,7 @@ public:
         k_param_rcmap,
         k_param_L1_controller,
         k_param_steerController,
+        k_param_barometer,
 
         // 254,255: reserved
         };
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index b61172f440..70f1982982 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -427,6 +427,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
     GOBJECT(scheduler, "SCHED_", AP_Scheduler),
 
+    // barometer ground calibration. The GND_ prefix is chosen for
+    // compatibility with previous releases of ArduPlane
+    // @Group: GND_
+    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    GOBJECT(barometer, "GND_", AP_Baro),
+
     // @Group: RELAY_
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),
diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde
index a1da28d295..e4952514ad 100644
--- a/APMrover2/commands.pde
+++ b/APMrover2/commands.pde
@@ -151,12 +151,7 @@ void init_home()
 
 	gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
 
-	home.id 	= MAV_CMD_NAV_WAYPOINT;
-
-	home.lng 	= g_gps->longitude;				// Lon * 10**7
-	home.lat 	= g_gps->latitude;				// Lat * 10**7
-    gps_base_alt    = max(g_gps->altitude_cm, 0);
-    home.alt        = g_gps->altitude_cm;
+    ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
 	home_is_set = true;
 
 	// Save Home to EEPROM - Command 0
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index d594249f40..605286a3d5 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -360,10 +360,7 @@ static void do_set_home()
 	if(next_nonnav_command.p1 == 1 && have_position) {
 		init_home();
 	} else {
-		home.id 	= MAV_CMD_NAV_WAYPOINT;
-		home.lng 	= next_nonnav_command.lng;				// Lon * 10**7
-		home.lat 	= next_nonnav_command.lat;				// Lat * 10**7
-		home.alt 	= max(next_nonnav_command.alt, 0);
+        ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt);
 		home_is_set = true;
 	}
 }
diff --git a/APMrover2/config.h b/APMrover2/config.h
index 721ac63537..99a35ab8b1 100644
--- a/APMrover2/config.h
+++ b/APMrover2/config.h
@@ -60,31 +60,42 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
 # define CONFIG_INS_TYPE   CONFIG_INS_OILPAN
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# define CONFIG_BARO     AP_BARO_BMP085
 # define BATTERY_PIN_1	  0
 # define CURRENT_PIN_1	  1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
 # define CONFIG_INS_TYPE   CONFIG_INS_MPU6000
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# ifdef APM2_BETA_HARDWARE
+#  define CONFIG_BARO     AP_BARO_BMP085
+# else // APM2 Production Hardware (default)
+#  define CONFIG_BARO          AP_BARO_MS5611
+#  define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
+# endif
 # define BATTERY_PIN_1	  1
 # define CURRENT_PIN_1	  2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 # define CONFIG_INS_TYPE CONFIG_INS_HIL
 # define CONFIG_COMPASS  AP_COMPASS_HIL
+# define CONFIG_BARO     AP_BARO_HIL
 # define BATTERY_PIN_1	  1
 # define CURRENT_PIN_1	  2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
 # define CONFIG_INS_TYPE   CONFIG_INS_PX4
 # define CONFIG_COMPASS  AP_COMPASS_PX4
+# define CONFIG_BARO AP_BARO_PX4
 # define BATTERY_PIN_1	  -1
 # define CURRENT_PIN_1	  -1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
 # define CONFIG_INS_TYPE   CONFIG_INS_FLYMAPLE
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# define CONFIG_BARO AP_BARO_BMP085
 # define BATTERY_PIN_1     20
 # define CURRENT_PIN_1	   19
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 # define CONFIG_INS_TYPE   CONFIG_INS_L3G4200D
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# define CONFIG_BARO     AP_BARO_BMP085
 # define BATTERY_PIN_1     -1
 # define CURRENT_PIN_1	   -1
 #endif
diff --git a/APMrover2/defines.h b/APMrover2/defines.h
index 4f8e555d64..9bcd2bc4ec 100644
--- a/APMrover2/defines.h
+++ b/APMrover2/defines.h
@@ -173,6 +173,12 @@ enum mode {
 #define CONFIG_INS_FLYMAPLE 5
 #define CONFIG_INS_L3G4200D 6
 
+// barometer driver types
+#define AP_BARO_BMP085   1
+#define AP_BARO_MS5611   2
+#define AP_BARO_PX4      3
+#define AP_BARO_HIL      4
+
 // compass driver types
 #define AP_COMPASS_HMC5843   1
 #define AP_COMPASS_PX4       2
diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde
index 3852b57e85..52c2d92f7f 100644
--- a/APMrover2/sensors.pde
+++ b/APMrover2/sensors.pde
@@ -1,5 +1,12 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+static void init_barometer(void)
+{
+    gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));    
+    barometer.calibrate();
+    gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
+}
+
 static void init_sonar(void)
 {
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 723f8d8b7d..5d4524a2bd 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -117,6 +117,9 @@ static void init_ardupilot()
     // used to detect in-flight resets
     g.num_resets.set_and_save(g.num_resets+1);
 
+    // init baro before we start the GCS, so that the CLI baro test works
+    barometer.init();
+
 	// init the GCS
 	gcs[0].init(hal.uartA);
 
@@ -174,6 +177,9 @@ static void init_ardupilot()
 	// initialise sonar
     init_sonar();
 
+    // and baro for EKF
+    init_barometer();
+
 	// Do GPS init
 	g_gps = &g_gps_driver;
     // GPS initialisation