Ну я делал так. Надеюсь все понятно будет
К некому циклу (процедуре) привязывается счетчик, который приращивается на столько, сколько занимает "должная" задержка (допустим есть некий цикл с задержкой 25 миллисекунд в конце. Следовательно, каждый такт приращивает счетчик на 25).
Как только счетчик равен или больше 1000 - Сбрасываем счетчик (отнимаем 1000), вычисляем реальное время, прошедшее от того момента, когда счетчик был равен нулю, до того, когда он перевалил за 1000.
далее следует такая формула:
задержка цикла*1000/время реальной задержки.
Пример:
Цикл должен был выполниться 40 раз за секунду (25 миллисекунд задержка цикла). Но это произошло за две
реальных секунды (по секундомеру).
25*1000/2000=12.5 ФПС.
И да, процедуру проверки заполненности счетчика лучше вынести в отдельный поток. Так то!