======CORDIC для вычисления амплитуды и фазы======
В сети достаточно информации о применении CORDIC для расчета синуса и косинуса. Но не часто встречается описание того, как получить из "синуса" и "косинуса" амплитуду и фазу. Такое вычисление часто требуется при построении синхронного детектора (LockIn Amplifier).
====LockIn Amplifier====
Суть синхронного детектора заключается в том, что на исследуемый объект подается раскачка с какого-то генератора. Например генератор можно подключить к пьезокерамике, которая далее может механически раскачивать какую-то балку. Как у пьезокерамики, так и у балки есть свои резонансные частоты. Если подать генератором именно эту резонансную частоту, то колебания значительно усилятся. Это можно использовать для нахождения резонансной частоты объектов - т.е. подать меняющуюся частоту и смотреть отклик.
Либо, можно подать частоту на балку и когда балка будет касаться каких-то предметов или взаимодействовать по емкостной связи, то частота колебаний балки и фаза будут отражать это взаимодействие.
Обе эти задачи встречаются в сканирующей зондовой микроскопии (СЗМ), где балка с очень острым шипом на конце, называемая кантелевер, касается молекул и атомов и по колебаниям балки детектируется рельеф.
Синхронный детектор хорошо описан тут - [[https://hubstub.ru/programming/214-vosstanrvoenie-signala-metodom-digital-lock-in-amplifier.html|ВОССТАНОВЛЕНИЕ СИГНАЛА МЕТОДОМ DIGITAL LOCK-IN AMPLIFIER]].
{{spм:lockin_math.png}}
Сильно упрощая, мы подаем на объект частоту с генератора, и снимаем с объекта отклик на эту частоту. В этом периодическом отклике содержится амплитуда колебаний вызванных генератором и фаза относительно раскачивающего сигнала.
* Если этот периодический отклик умножить на сигнал с генератора, то получается сигнал MagSin.
* Если этот отклик умножить на сигнал с генератора но сдвинутый на четверть периода, то получается сигнал MagCos.
Далее, следуя простым тригонометрическим формулам можно посчитать из сигналов MagSin и MagCos значение амплитуды и фазы. Но работа с тригонометрическими формулами напрямую занимает много ресурсов микроконтроллера/FPGA. Поэтому часто используют CORDIC алгоритм. Этот алгоритм позволяет свести вычисление к серии сложений и сдвигов.
====CORDIC for LockIn====
Код вычисления амплитуды и фазы с доступными объяснениями можно найти на данной странице
[[https://dspguru.com/dsp/faqs/cordic/|CORDIC FAQ]]
Данный код был использован мной для проверки работоспособности в python. Код переписан под fixed-point арифметику, т.к. целые числа считаются значительно быстрее, и это ближе к железу.
import math
g_cordic_table = list()
g_cordic_table_len = 0
g_mag_scale = 1
HALF_PI = math.pi / 2
def cordic_init(max_len, fix_scale, fix_offset):
global g_mag_scale
g_cordic_table_len = max_len
K = 1.0
for i in range(0, max_len):
phase_rads = math.atan(K)
g_cordic_table.append(round(fix_scale * phase_rads) )
K *= 0.5
g_mag_scale = 1 * fix_scale
mag, dummy_phase = cordic_calc_mag_phase(fix_scale, 0, fix_scale, fix_offset, False)
g_mag_scale = round(fix_scale / mag * fix_scale)
def cordic_calc_mag_phase(msin, mcos, fix_scale, fix_offset, show_log):
# rotate by an initial +/- 90 degrees
I = mcos
Q = msin
if I < 0:
if Q > 0:
I = Q #subtract 90 degrees
Q = -mcos
acc_phase_rads = round(-HALF_PI * fix_scale)
else:
I = -Q #add 90 degrees
Q = mcos
acc_phase_rads = round(HALF_PI * fix_scale)
else:
acc_phase_rads = 0
# rotate using "1 + jK" factors
shift = 0
for phase_rads in g_cordic_table:
tmp_I = I
if Q >= 0:
I += Q >> shift
Q -= tmp_I >> shift
acc_phase_rads -= phase_rads
else:
I -= Q >> shift
Q += tmp_I >> shift
acc_phase_rads += phase_rads
shift += 1
if show_log:
print('{0}, {1}, {2} '.format(I, Q, acc_phase_rads))
mag = (I * g_mag_scale) >> fix_offset
return (mag, -acc_phase_rads)
def cordic_test_mag_phase(phase_step_rads, mag, prec, fix_scale, fix_offset):
phase = -math.pi
angle_cnt = math.floor((2 * math.pi) / phase_step_rads)
for i in range(0, angle_cnt):
msin_i = round(math.sin(phase) * mag * fix_scale)
mcos_i = round(math.cos(phase) * mag * fix_scale)
out_mag_i, out_phase_i = cordic_calc_mag_phase(msin_i, mcos_i, fix_scale, fix_offset, False)
out_mag = out_mag_i / fix_scale
out_phase = out_phase_i / fix_scale
mag_err = abs(mag - out_mag)
phase_err = abs(phase - out_phase)
log = 'Mag: {0:.{6}f}, {1:.{6}f}, {2:.{6}f}, Phase: {3:.{6}f}, {4:.{6}f}, {5:.{6}f}' \
.format(mag, out_mag, mag_err, \
math.degrees(phase), math.degrees(out_phase), math.degrees(phase_err), prec)
print(log)
phase += phase_step_rads
# ============== TEST ==============
FIX_SCALE = 32767
FIX_OFFS = 15
cordic_init(16, FIX_SCALE, FIX_OFFS)
cordic_test_mag_phase(math.radians(15), 10.0, 5, FIX_SCALE, FIX_SCALE)
Результат получается следующий:
# inp_amp out_amp amp_error inp_phase out_phase phase_error
Mag: 10.00000, 9.99982, 0.00018, Phase: -180.00000, -180.00076, 0.00076
Mag: 10.00000, 9.99979, 0.00021, Phase: -165.00000, -164.99793, 0.00207
Mag: 10.00000, 9.99979, 0.00021, Phase: -150.00000, -149.99859, 0.00141
Mag: 10.00000, 9.99982, 0.00018, Phase: -135.00000, -134.99926, 0.00074
Mag: 10.00000, 9.99979, 0.00021, Phase: -120.00000, -119.99992, 0.00008
Mag: 10.00000, 9.99982, 0.00018, Phase: -105.00000, -105.00059, 0.00059
Mag: 10.00000, 9.99982, 0.00018, Phase: -90.00000, -90.00125, 0.00125
Mag: 10.00000, 9.99979, 0.00021, Phase: -75.00000, -74.99842, 0.00158
Mag: 10.00000, 9.99979, 0.00021, Phase: -60.00000, -59.99909, 0.00091
Mag: 10.00000, 9.99982, 0.00018, Phase: -45.00000, -44.99975, 0.00025
Mag: 10.00000, 9.99979, 0.00021, Phase: -30.00000, -30.00042, 0.00042
Mag: 10.00000, 9.99982, 0.00018, Phase: -15.00000, -15.00108, 0.00108
Mag: 10.00000, 9.99982, 0.00018, Phase: 0.00000, -0.00175, 0.00175
Mag: 10.00000, 9.99979, 0.00021, Phase: 15.00000, 15.00108, 0.00108
Mag: 10.00000, 9.99979, 0.00021, Phase: 30.00000, 30.00042, 0.00042
Mag: 10.00000, 9.99982, 0.00018, Phase: 45.00000, 44.99975, 0.00025
Mag: 10.00000, 9.99979, 0.00021, Phase: 60.00000, 59.99909, 0.00091
Mag: 10.00000, 9.99982, 0.00018, Phase: 75.00000, 74.99842, 0.00158
Mag: 10.00000, 9.99976, 0.00024, Phase: 90.00000, 90.00125, 0.00125
Mag: 10.00000, 9.99979, 0.00021, Phase: 105.00000, 105.00059, 0.00059
Mag: 10.00000, 9.99979, 0.00021, Phase: 120.00000, 119.99992, 0.00008
Mag: 10.00000, 9.99982, 0.00018, Phase: 135.00000, 134.99926, 0.00074
Mag: 10.00000, 9.99979, 0.00021, Phase: 150.00000, 149.99859, 0.00141
Mag: 10.00000, 9.99982, 0.00018, Phase: 165.00000, 164.99793, 0.00207
Здесь тест при амплитуде 10 проверяет расчет фазы от -180 до 180 с шагом в 15 градусов. Используются 16-ти битные fixed-point значения, где 15-й бит это знак числа, а оставшиеся биты - это дробная часть.
Собственно, суть нахождения амплитуды и фазы заключена в этом коде:
shift = 0
for phase_rads in g_cordic_table:
tmp_I = I
if Q >= 0:
I += Q >> shift
Q -= tmp_I >> shift
acc_phase_rads -= phase_rads
else:
I -= Q >> shift
Q += tmp_I >> shift
acc_phase_rads += phase_rads
shift += 1
**Мы берем значение сигнала MagCos, которое здесь I, и крутим CORDIC-ом к значению 0 (кладем на ось Х)!** Накопленная фаза до сдвига в 0, это и есть начальная фаза сигнала, а амплитуда - это получившееся значение I на оси Х. По коду стоит отметить, что сдвиг на ">> shift" это и есть замена деления/умножения на операцию сдвига. Т.е. значения фазы в таблице CORDIC подбираются так, чтобы быть кратными двойке. Следующий шаг "кручения" CORDIC в 2 раза меньше предыдущего, так получается сходимость к правильному результату. Чем длиннее таблица CORDIC, тем выше точность вычисления.
{{spм:cordic_magphase.png}}
//magSin_in// и //magCos_in// на картинке обозначают входные значения сигналов, для которых происходит расчет амплитуды (MAG) и фазы (PHASE).
=====Пример=====
Для примера будем увеличивать частоту подающего генератора и снимать сигналы magSin и magCos с АЦП. Допустим мы сняли значения этих сигналов для 2000 значений генератора. Если подсунуть массивы этих точек в функцию //cordic_test_mag_phase()// то мы получим сигнал амплитуды и фазы:
{{spм:cordicresfreq.png}}
Значения по оси X на картинках это индексы в массивах. Но зная с какой частоты генератора мы начали измерение сигналов и с каким шагом увеличивали частоту можно отразить эту ось Х в значениях частоты. И тогда можно по графику MAG можно оценить на какой частоте происходит резонанс системы. При съеме сигналов фаза специально была подкручена так, чтобы на резонансе значение было равно 0. Отклонение фазы от 0 позволяет отслеживать как уходит резонанс при изменении условий в системе.
{{spм:cordicresfreqkhz.png}}
=====Полоса пропускания синхронного детектора=====
В общем случае, частота снимаемого сигнала может оказаться отличной от частоты возбуждения. Балка имеет свой собственный резонанс, и чем ближе частота раскачки к резонансной частоте балки, тем сильнее отклик.
{{spм:lockin_mathex.png}}
По формулам видно, что синхронный детектор формирует сигналы MagSin и MagCos, которые являются разностными. Т.е. они зависят от разности между частотой раскачки и частотой отклика. Получается, что фильтр низкой частоты пропускает только сигналы с частотой близкой к частоте раскачки. Если сигнал будет отличаться от частоты раскачки больше, чем частота среза фильтра, то фильтр не пропустит такой сигнал.
Таким образом, синхронный детектор работает в полосе частот, ширина которой равна удвоенной частоте фильтра низких частот.