diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index d49216ad86..cb3a3e6fd9 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -1123,7 +1123,7 @@ static void update_GPS_10Hz(void)
  */
 static void handle_auto_mode(void)
 {
-    switch(mission.get_current_do_cmd().id) {
+    switch(mission.get_current_nav_cmd().id) {
     case MAV_CMD_NAV_TAKEOFF:
         if (steer_state.hold_course_cd == -1) {
             // we don't yet have a heading to hold - just level
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index 0a13dd8e90..8b3e1563d8 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -273,7 +273,7 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd)
     set_next_WP(cmd);
     // pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
     takeoff_pitch_cd        = (int)cmd.p1 * 100;
-    takeoff_altitude_cm     = cmd.content.location.alt;
+    takeoff_altitude_cm     = next_WP.content.location.alt;
     next_WP.content.location.lat = home.lat + 10;
     next_WP.content.location.lng = home.lng + 10;
     takeoff_complete        = false;                            // set flag to use gps ground course during TO.  IMU will be doing yaw drift correction