Added nav timeout, but not fully tested.

Seems to be a problem when there is a clear_schedule call during nav which doesn't respond to prempt immediately. It actually seems to be due to the (faked) nav returning normal when it should be preempted.