Plane: Fixed bug where NAV_LOITER_TO_ALT was not verifying heading.

NAV_LOITER_TO_ALT now verifying that the plane was heading
towards the next waypoint before breaking out of loiter IF the
first parameter of the mavlink message is not 0.

ArduPlane/commands_logic.cpp

@@ -690,6 +690,11 @@ bool Plane::verify_loiter_to_alt()
         if (labs(condition_value - current_loc.alt) < 500 && loiter.sum_cd > 1) {
             // primary goal completed, initialize secondary heading goal
             condition_value = 0;
+            if (mission.get_current_nav_cmd().content.location.flags.loiter_hdg_chk != 0) {
+                loiter.sum_cd = 3; //don't immediately stop loitering;
+                //wait for heading toward next wp or 3 complete loiters,
+                // whichever comes first.
+            }
             result = verify_loiter_heading(true);
         }
     } else {