diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 98e90b993a..17b55bf2d4 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -217,8 +217,7 @@ namespace OpenSim.Region.Framework.Scenes
         private string m_nextSitAnimation = String.Empty;
 
         //PauPaw:Proper PID Controler for autopilot************
-        private bool m_moveToPositionInProgress;
-
+        public bool MovingToTarget { get; private set; }
         public Vector3 MoveToPositionTarget { get; private set; }
 
         private bool m_followCamAuto;
@@ -1385,7 +1384,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                 if (agentData.UseClientAgentPosition)
                 {
-                    m_moveToPositionInProgress = (agentData.ClientAgentPosition - AbsolutePosition).Length() > 0.2f;
+                    MovingToTarget = (agentData.ClientAgentPosition - AbsolutePosition).Length() > 0.2f;
                     MoveToPositionTarget = agentData.ClientAgentPosition;
                 }
 
@@ -1484,7 +1483,7 @@ namespace OpenSim.Region.Framework.Scenes
                         i++;
                     }
 
-                    if (m_moveToPositionInProgress)
+                    if (MovingToTarget)
                     {
                         // If the user has pressed a key then we want to cancel any move to target.
                         if (DCFlagKeyPressed)
@@ -1539,7 +1538,7 @@ namespace OpenSim.Region.Framework.Scenes
                     && ((flags & AgentManager.ControlFlags.AGENT_CONTROL_SIT_ON_GROUND) == 0)
                     && (m_parentID == 0)
                     && !SitGround
-                    && !m_moveToPositionInProgress)
+                    && !MovingToTarget)
                     Animator.UpdateMovementAnimations();
             }
 
@@ -1706,7 +1705,20 @@ namespace OpenSim.Region.Framework.Scenes
             float terrainHeight = (float)m_scene.Heightmap[(int)pos.X, (int)pos.Y];
             pos.Z = Math.Max(terrainHeight, pos.Z);
 
-            m_moveToPositionInProgress = true;
+            // Fudge factor.  It appears that if one clicks "go here" on a piece of ground, the go here request is
+            // always slightly higher than the actual terrain height.
+            // FIXME: This constrains NOC movements as well, so should be somewhere else.
+            if (pos.Z - terrainHeight < 0.2)
+                pos.Z = terrainHeight;
+
+//            m_log.DebugFormat(
+//                "[SCENE PRESENCE]: Avatar {0} set move to target {1} (terrain height {2}) in {3}",
+//                Name, pos, terrainHeight, m_scene.RegionInfo.RegionName);
+
+            if (pos.Z > terrainHeight)
+                PhysicsActor.Flying = true;
+
+            MovingToTarget = true;
             MoveToPositionTarget = pos;
 
             Vector3 agent_control_v3 = new Vector3();
@@ -1721,7 +1733,7 @@ namespace OpenSim.Region.Framework.Scenes
         {
             m_log.DebugFormat("[SCENE PRESENCE]: Resetting move to target for {0}", Name);
 
-            m_moveToPositionInProgress = false;
+            MovingToTarget = false;
             MoveToPositionTarget = Vector3.Zero;
         }
 
diff --git a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
index 221549599c..30e81fc72e 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
@@ -62,7 +62,7 @@ namespace OpenSim.Region.OptionalModules.World.NPC
         {
             lock (m_avatars)
             {
-                if (m_avatars.ContainsKey(presence.UUID))
+                if (m_avatars.ContainsKey(presence.UUID) && presence.MovingToTarget)
                 {
                     double distanceToTarget = Util.GetDistanceTo(presence.AbsolutePosition, presence.MoveToPositionTarget);
 //                            m_log.DebugFormat(
@@ -72,8 +72,8 @@ namespace OpenSim.Region.OptionalModules.World.NPC
                     // Check the error term of the current position in relation to the target position
                     if (distanceToTarget <= ScenePresence.SIGNIFICANT_MOVEMENT)
                     {
-//                        m_log.DebugFormat("[NPC MODULE]: Stopping movement of npc {0} {1}", presence.Name, presence.UUID);
-                        // We are close enough to the target for now
+                        m_log.DebugFormat("[NPC MODULE]: Stopping movement of npc {0}", presence.Name);
+                        // We are close enough to the target
                         presence.Velocity = Vector3.Zero;
                         presence.AbsolutePosition = presence.MoveToPositionTarget;
                         presence.ResetMoveToTarget();
@@ -86,6 +86,10 @@ namespace OpenSim.Region.OptionalModules.World.NPC
                     }
                     else
                     {
+                        m_log.DebugFormat(
+                            "[NPC MODULE]: Updating npc {0} at {1} for next movement to {2}",
+                            presence.Name, presence.AbsolutePosition, presence.MoveToPositionTarget);
+
                         Vector3 agent_control_v3 = new Vector3();
                         presence.HandleMoveToTargetUpdate(ref agent_control_v3, presence.Rotation);
                         presence.AddNewMovement(agent_control_v3, presence.Rotation);