You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

noptica.py 4.9KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165
  1. import serial
  2. import queue
  3. import threading
  4. import SoapySDR
  5. import numpy as np
  6. from scipy.signal import blackmanharris
  7. class BufferedSDR:
  8. def __init__(self, sdr, channels, bufsize, nbufs):
  9. self.sdr = sdr
  10. self.channels = channels
  11. self.bufsize = bufsize
  12. self.stream = None
  13. self.terminate = False
  14. self.thread = None
  15. self.queue = queue.Queue(nbufs)
  16. self.available_buffers = queue.Queue(nbufs)
  17. for _ in range(nbufs):
  18. self.available_buffers.put([np.array([0]*bufsize, np.complex64) for _ in self.channels])
  19. def start(self):
  20. self.stream = self.sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32, self.channels)
  21. try:
  22. self.thread = threading.Thread(target=self.thread_target)
  23. self.thread.start()
  24. except:
  25. self.sdr.closeStream(self.stream)
  26. raise
  27. def stop(self):
  28. self.terminate = True
  29. self.thread.join()
  30. self.sdr.closeStream(self.stream)
  31. def get(self):
  32. return self.queue.get()
  33. def dispose(self, buffers):
  34. self.available_buffers.put(buffers)
  35. def thread_target(self):
  36. self.sdr.activateStream(self.stream)
  37. try:
  38. while not self.terminate:
  39. buffers = self.available_buffers.get()
  40. sr = self.sdr.readStream(self.stream, buffers, self.bufsize)
  41. if sr.ret != self.bufsize:
  42. print("SDR sampling error")
  43. return
  44. self.queue.put(buffers)
  45. finally:
  46. self.sdr.deactivateStream(self.stream)
  47. class DummyInductionHeater:
  48. def __init__(self, port, induction_min, induction_max):
  49. pass
  50. def start(self):
  51. pass
  52. def set(self, amount):
  53. print("induction", amount)
  54. def stop(self):
  55. pass
  56. class InductionHeater:
  57. """Interface to the MHS5200A function generator driving the LC tank"""
  58. def __init__(self, port, induction_min, induction_max):
  59. self.port = port
  60. self.induction_min = induction_min
  61. self.induction_max = induction_max
  62. self.queue = queue.Queue(1)
  63. def start(self):
  64. self.serial = serial.Serial(self.port, 57600)
  65. self.thread = threading.Thread(target=self.thread_target)
  66. self.thread.start()
  67. def thread_target(self):
  68. while True:
  69. amount = self.queue.get()
  70. if amount is None:
  71. break
  72. amount = max(min(amount, 0.5), -0.5)
  73. freq = ((self.induction_min + self.induction_max)/2
  74. + amount*(self.induction_max - self.induction_min))
  75. command = ":s1f{:010d}\n".format(int(freq*1e2))
  76. self.serial.write(command.encode())
  77. self.serial.readline()
  78. def set(self, amount):
  79. self.queue.put(amount, block=False)
  80. def stop(self):
  81. self.queue.put(None, block=True)
  82. self.thread.join()
  83. self.serial.close()
  84. # https://gist.github.com/endolith/255291
  85. def parabolic(f, x):
  86. xv = 1/2. * (f[x-1] - f[x+1]) / (f[x-1] - 2 * f[x] + f[x+1]) + x
  87. yv = f[x] - 1/4. * (f[x-1] - f[x+1]) * (xv - x)
  88. return (xv, yv)
  89. class Stabilizer:
  90. def __init__(self, freq_sample, amp_threshold, freq_target, k, tuner):
  91. self.freq_sample = freq_sample
  92. self.amp_threshold = amp_threshold
  93. self.freq_target = freq_target
  94. self.k = k
  95. self.tuner = tuner
  96. def input(self, samples):
  97. spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples)))[0:len(samples)//2])
  98. for i in range(len(spectrum)//100):
  99. spectrum[i] = 0
  100. spectrum[-i] = 0
  101. i = np.argmax(spectrum)
  102. true_i, amplitude = parabolic(spectrum, i)
  103. freq = 0.5 * self.freq_sample * true_i / len(spectrum)
  104. if amplitude > self.amp_threshold:
  105. tuning = (freq - self.freq_target)*self.k
  106. else:
  107. tuning = 0.0
  108. self.tuner.set(tuning)
  109. def continuous_unwrap(last_phase, last_phase_unwrapped, p):
  110. # note: np.unwrap always preserves first element of array
  111. p = np.unwrap(p)
  112. glue = np.array([last_phase_unwrapped, last_phase_unwrapped + (p[0] - last_phase)])
  113. new_p0 = np.unwrap(glue)[1]
  114. return new_p0 + p - p[0]
  115. class PositionTracker:
  116. def __init__(self, leakage_avg):
  117. self.last_phase = 0.0
  118. self.last_position = 0.0
  119. self.leakage = np.zeros(leakage_avg)
  120. self.leakage_ptr = 0
  121. def input(self, ref, meas):
  122. demod = np.conjugate(ref)*meas
  123. self.leakage[self.leakage_ptr] = np.real(np.sum(demod)/len(demod))
  124. self.leakage_ptr = (self.leakage_ptr + 1) % len(self.leakage)
  125. leakage = np.sum(self.leakage)/len(self.leakage)
  126. phase = np.angle(demod - leakage)
  127. position = continuous_unwrap(self.last_phase, self.last_position, phase)/(2.0*np.pi)
  128. self.last_phase = phase[-1]
  129. self.last_position = position[-1]
  130. return position, leakage