--Speedup of GPS reader/parser seems to work...now clocking in under 7ms.  Fixed waypoint logging to go to log buffer instead of directly to socket (keeps messages in order).  Also fixed problem with motors and log still running after last point is reached.  Used a couple of flags to ensure that read_estimate_and_move doesn't run if the route is complete and to keep read_buffer_and_send from continuing to send the same data until the thread is joined.