tp: fix for G95 pausing bug from Rick

Conflicts:
	src/emc/tp/tp.c
Merge pull request #272 from mhaberler/g95-fix

tp: fix for G95 pausing bug from Rick