Я примерно знаю частоту сигнала (период считается таймером с разрешением 25нс, эта величина шумит 200нсrms). Затем делаю выборку входного сигнала, так, чтобы в неё попало примерно целое число периодов. Далее делаю свёртку выборки с синусом и косинусом вычисленной частоты.