O teu comando ainda funciona.
Por que razão substitui-lo?
O drift não significa necessariamente que o teu comando chegou ao fim.
Na maioria dos casos, o problema está nos módulos analógicos responsáveis pela leitura do movimento.
Em vez de deitares fora um comando ao qual já estás habituado, podes substituir essa parte por uma solução TMR.