diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index bbd6ae8df6..38bab85f39 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1210,8 +1210,10 @@ static void update_altitude()
 	altitude_sensor = BARO;
 
 	#if HIL_MODE == HIL_MODE_ATTITUDE
-	current_loc.alt = g_gps->altitude - gps_base_alt;
-	return;
+		current_loc.alt = g_gps->altitude - gps_base_alt;
+		climb_rate = (g_gps->altitude - old_baro_alt) * 10;
+		old_baro_alt = g_gps->altitude;
+		return;
 	#else
 
 	// calc the vertical accel rate
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index afdd6f5309..36f3c90de6 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -262,10 +262,10 @@ static void NOINLINE send_gps_status(mavlink_channel_t chan)
     mavlink_msg_gps_status_send(
         chan,
         g_gps->num_sats,
-        NULL,
-        NULL,
-        NULL,
-        NULL,
+        next_WP.alt,
+        nav_throttle,
+        angle_boost,
+        manual_boost,
         NULL);
 }