diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 292b69340e..8669a78f96 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1080,13 +1080,11 @@ static void update_current_flight_mode(void) case MAV_CMD_NAV_LAND: calc_nav_roll(); - if (airspeed.use()) { - calc_nav_pitch(); - calc_throttle(); - }else{ - calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle - calc_throttle(); // throttle based on altitude error - nav_pitch_cd = g.land_pitch_cd; // pitch held constant + calc_nav_pitch(); + calc_throttle(); + if (!airspeed.use() || land_complete) { + // hold pitch constant in final approach + nav_pitch_cd = g.land_pitch_cd; } if (land_complete) { @@ -1100,6 +1098,7 @@ static void update_current_flight_mode(void) // we are doing normal AUTO flight, the special cases // are for takeoff and landing hold_course = -1; + land_complete = false; calc_nav_roll(); calc_nav_pitch(); calc_throttle(); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 337f4e5855..975c1dc6b5 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -8,6 +8,7 @@ handle_process_nav_cmd() { // reset navigation integrators // ------------------------- + land_complete = false; reset_I(); gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); @@ -319,9 +320,9 @@ static bool verify_land() if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300)){ - land_complete = 1; + land_complete = true; - if(hold_course == -1) { + if (hold_course == -1) { // we have just reached the threshold of 2 seconds or 3 // meters to landing. We now don't want to do any radical // turns, as rolling could put the wings into the runway.