diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 58f4e29028..d09513cc8c 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -171,7 +171,7 @@ static AP_Int8                *flight_modes = &g.flight_mode1;
 	AP_GPS_HIL              g_gps_driver(NULL);
 	AP_Compass_HIL          compass; // never used
 	AP_IMU_Shim             imu; // never used
-
+    static int32_t          gps_base_alt;
 #else
 	#error Unrecognised HIL_MODE setting.
 #endif // HIL MODE
@@ -1314,7 +1314,7 @@ static void update_alt()
 	altitude_sensor = BARO;
 
 	#if HIL_MODE == HIL_MODE_ATTITUDE
-	current_loc.alt = g_gps->altitude;
+	current_loc.alt = g_gps->altitude - gps_base_alt;
 	return;
 	#else
 
diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde
index a1624363ac..c6af9d082f 100644
--- a/ArduCopterMega/GCS_Mavlink.pde
+++ b/ArduCopterMega/GCS_Mavlink.pde
@@ -899,9 +899,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // set gps hil sensor
             g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
             packet.v,packet.hdg,0,0);
+            if (gps_base_alt == 0) {
+                gps_base_alt = packet.alt*100;
+            }
             current_loc.lng = packet.lon * T7;
             current_loc.lat = packet.lat * T7;
-            current_loc.alt = packet.alt * 10;
+            current_loc.alt = g_gps->altitude - gps_base_alt;
+            if (!home_is_set) {
+                init_home();
+            }
             break;
         }
 #if HIL_MODE == HIL_MODE_ATTITUDE