Better handling of lock/unlock

Wait until position achieved to move to next state
Only allow pause from places it makes sense to resume to
More logging in motion
More careful handling of PVT pause
Immediately prime PVT system on mode switch
Don't overcorrect when PVT driving steering