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.

192 lines
5.6KB

  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. assert -0.5 <= amount <= 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(0.0, block=True)
  82. self.queue.put(None, block=True)
  83. self.thread.join()
  84. self.serial.close()
  85. class Stabilizer:
  86. def __init__(self, freq_sample, block_size, freq_target, cb):
  87. self.freqs = np.fft.fftfreq(block_size, d=1/freq_sample)
  88. self.freq_target = freq_target
  89. self.cb = cb
  90. self.amp_counter = 0
  91. self.lock_counter = 0
  92. self.unlock_counter = 0
  93. self.wiggle = 0.0
  94. self.tuning = 0.0
  95. self.amp_threshold = 80.0
  96. self.k = 30.0e-6
  97. self.tolerance = 10e3
  98. self.amp_counter_threshold = 60
  99. self.lock_counter_threshold = 60
  100. self.unlock_counter_threshold = 500
  101. self.wiggle_amplitude = 0.15
  102. def input(self, samples):
  103. spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples))))
  104. i = np.argmax(spectrum)
  105. amplitude = spectrum[i]
  106. success = False
  107. if amplitude > self.amp_threshold:
  108. freq = self.freqs[i]
  109. delta = freq - self.freq_target
  110. self.amp_counter += 1
  111. if self.amp_counter > self.amp_counter_threshold:
  112. self.tuning = delta*self.k
  113. if abs(delta) < self.tolerance:
  114. success = True
  115. else:
  116. freq = None
  117. self.amp_counter = 0
  118. max_tuning_abs = 0.5 - self.wiggle_amplitude - 1e-9
  119. self.tuning = max(min(self.tuning, max_tuning_abs), -max_tuning_abs)
  120. if success:
  121. self.lock_counter += 1
  122. else:
  123. self.lock_counter = 0
  124. if self.locked():
  125. self.unlock_counter = 0
  126. else:
  127. self.unlock_counter += 1
  128. if not success and (self.unlock_counter > self.unlock_counter_threshold):
  129. self.wiggle = self.wiggle_amplitude*np.random.uniform(-1.0, 1.0)
  130. print("wiggle", self.wiggle)
  131. self.unlock_counter = 0
  132. self.amp_counter = 0
  133. self.cb(spectrum, freq, self.locked(), self.tuning + self.wiggle)
  134. def locked(self):
  135. return self.lock_counter > self.lock_counter_threshold
  136. def continuous_unwrap(last_phase, last_phase_unwrapped, p):
  137. # note: np.unwrap always preserves first element of array
  138. p = np.unwrap(p)
  139. glue = np.array([last_phase_unwrapped, last_phase_unwrapped + (p[0] - last_phase)])
  140. new_p0 = np.unwrap(glue)[1]
  141. return new_p0 + p - p[0]
  142. class PositionTracker:
  143. def __init__(self):
  144. self.reset()
  145. def reset(self):
  146. self.last_phase = 0.0
  147. self.last_position = 0.0
  148. def input(self, ref, meas):
  149. phase = np.angle(meas*ref.conj())
  150. position = continuous_unwrap(self.last_phase, self.last_position, phase)
  151. self.last_phase = phase[-1]
  152. self.last_position = position[-1]
  153. return position