From 802ab79c84912c4ea3e77dcf188e284b406168c4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Mar 2014 17:35:13 +0900 Subject: [PATCH] Plane: bug fix for setting flight_stage based on current command --- ArduPlane/ArduPlane.pde | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 1bdea60b89..f07e3b5a23 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -460,13 +460,6 @@ static uint8_t ground_start_count = 5; // true if we have a position estimate from AHRS static bool have_position; -//////////////////////////////////////////////////////////////////////////////// -// Location & Navigation -//////////////////////////////////////////////////////////////////////////////// - -// There may be two active commands in Auto mode. -// This is the command type (eg navigate to waypoint) of the active navigation command -static uint8_t nav_command_ID = NO_COMMAND; //////////////////////////////////////////////////////////////////////////////// // Airspeed @@ -1396,9 +1389,9 @@ static void update_alt() if (control_mode==AUTO) { if (takeoff_complete == false) { flight_stage = AP_SpdHgtControl::FLIGHT_TAKEOFF; - } else if (nav_command_ID == MAV_CMD_NAV_LAND && land_complete == true) { + } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && land_complete == true) { flight_stage = AP_SpdHgtControl::FLIGHT_LAND_FINAL; - } else if (nav_command_ID == MAV_CMD_NAV_LAND) { + } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { flight_stage = AP_SpdHgtControl::FLIGHT_LAND_APPROACH; } }