Wenn der Befehl Waitms richtig ausgeführt wird, dann stimmt wohl die dem Compiler mitgeteilte Taktfrequenz mit der realen Taktfrequenz überein. Daher vermute ich ein Softwareproblem. Poste doch mal den aktuellen Code, der auf dem Controller läuft.

Grüße
Thomas