2 Commits

View File

@ -85,6 +85,116 @@ def _parse_spec_clip(spec: Optional[str]) -> Optional[Tuple[float, float]]:
return None
def _normalize_sweep_simple(raw: np.ndarray, calib: np.ndarray) -> np.ndarray:
"""Простая нормировка: поэлементное деление raw/calib."""
w = min(raw.size, calib.size)
if w <= 0:
return raw
out = np.full_like(raw, np.nan, dtype=np.float32)
with np.errstate(divide="ignore", invalid="ignore"):
out[:w] = raw[:w] / calib[:w]
out = np.nan_to_num(out, nan=np.nan, posinf=np.nan, neginf=np.nan)
return out
def _build_calib_envelopes(calib: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
"""Оценить нижнюю/верхнюю огибающие калибровочной кривой."""
n = int(calib.size)
if n <= 0:
empty = np.zeros((0,), dtype=np.float32)
return empty, empty
y = np.asarray(calib, dtype=np.float32)
finite = np.isfinite(y)
if not np.any(finite):
zeros = np.zeros_like(y, dtype=np.float32)
return zeros, zeros
if not np.all(finite):
x = np.arange(n, dtype=np.float32)
y = y.copy()
y[~finite] = np.interp(x[~finite], x[finite], y[finite]).astype(np.float32)
if n < 3:
return y.copy(), y.copy()
dy = np.diff(y)
s = np.sign(dy).astype(np.int8, copy=False)
if np.any(s == 0):
for i in range(1, s.size):
if s[i] == 0:
s[i] = s[i - 1]
for i in range(s.size - 2, -1, -1):
if s[i] == 0:
s[i] = s[i + 1]
s[s == 0] = 1
max_idx = np.where((s[:-1] > 0) & (s[1:] < 0))[0] + 1
min_idx = np.where((s[:-1] < 0) & (s[1:] > 0))[0] + 1
x = np.arange(n, dtype=np.float32)
def _interp_nodes(nodes: np.ndarray) -> np.ndarray:
if nodes.size == 0:
idx = np.array([0, n - 1], dtype=np.int64)
else:
idx = np.unique(np.concatenate(([0], nodes, [n - 1]))).astype(np.int64)
return np.interp(x, idx.astype(np.float32), y[idx]).astype(np.float32)
upper = _interp_nodes(max_idx)
lower = _interp_nodes(min_idx)
swap = lower > upper
if np.any(swap):
tmp = upper[swap].copy()
upper[swap] = lower[swap]
lower[swap] = tmp
return lower, upper
def _normalize_sweep_projector(raw: np.ndarray, calib: np.ndarray) -> np.ndarray:
"""Нормировка через проекцию между огибающими калибровки в диапазон [-1, +1]."""
w = min(raw.size, calib.size)
if w <= 0:
return raw
out = np.full_like(raw, np.nan, dtype=np.float32)
raw_seg = np.asarray(raw[:w], dtype=np.float32)
lower, upper = _build_calib_envelopes(np.asarray(calib[:w], dtype=np.float32))
span = upper - lower
finite_span = span[np.isfinite(span) & (span > 0)]
if finite_span.size > 0:
eps = max(float(np.median(finite_span)) * 1e-6, 1e-9)
else:
eps = 1e-9
valid = (
np.isfinite(raw_seg)
& np.isfinite(lower)
& np.isfinite(upper)
& (span > eps)
)
if np.any(valid):
proj = np.empty_like(raw_seg, dtype=np.float32)
proj[valid] = ((2.0 * (raw_seg[valid] - lower[valid]) / span[valid]) - 1.0) * 1000.0
proj[valid] = np.clip(proj[valid], -1000.0, 1000.0)
proj[~valid] = np.nan
out[:w] = proj
return out
def _normalize_by_calib(raw: np.ndarray, calib: np.ndarray, norm_type: str) -> np.ndarray:
"""Нормировка свипа по выбранному алгоритму."""
nt = str(norm_type).strip().lower()
if nt == "simple":
return _normalize_sweep_simple(raw, calib)
return _normalize_sweep_projector(raw, calib)
def try_open_pyserial(path: str, baud: int, timeout: float):
try:
import serial # type: ignore
@ -92,14 +202,6 @@ def try_open_pyserial(path: str, baud: int, timeout: float):
return None
try:
ser = serial.Serial(path, baudrate=baud, timeout=timeout)
# ВРЕМЕННО ОТКЛЮЧЕН: hardware flow control для проверки
# ser.rtscts = True
# Увеличиваем буфер приема ядра до 64KB
try:
ser.set_buffer_size(rx_size=65536, tx_size=4096)
except (AttributeError, NotImplementedError):
# Не все платформы/версии pyserial поддерживают set_buffer_size
pass
return ser
except Exception:
return None
@ -113,8 +215,7 @@ class FDReader:
self._fd = fd
raw = os.fdopen(fd, "rb", closefd=False)
self._file = raw
# Увеличен размер буфера до 256KB для предотвращения потерь
self._buf = io.BufferedReader(raw, buffer_size=262144)
self._buf = io.BufferedReader(raw, buffer_size=65536)
def fileno(self) -> int:
return self._fd
@ -222,11 +323,10 @@ class SerialLineSource:
class SerialChunkReader:
"""Быстрое неблокирующее чтение чанков из serial/raw TTY для максимального дренажа буфера."""
def __init__(self, src: SerialLineSource, error_counter: Optional[list] = None):
def __init__(self, src: SerialLineSource):
self._src = src
self._ser = src._pyserial
self._fd: Optional[int] = None
self._error_counter = error_counter # Список с 1 элементом для передачи по ссылке
if self._ser is not None:
# Неблокирующий режим для быстрой откачки
try:
@ -249,15 +349,11 @@ class SerialChunkReader:
try:
n = int(getattr(self._ser, "in_waiting", 0))
except Exception:
if self._error_counter:
self._error_counter[0] += 1
n = 0
if n > 0:
try:
return self._ser.read(n)
except Exception:
if self._error_counter:
self._error_counter[0] += 1
return b""
return b""
if self._fd is None:
@ -274,8 +370,6 @@ class SerialChunkReader:
except BlockingIOError:
break
except Exception:
if self._error_counter:
self._error_counter[0] += 1
break
return bytes(out)
@ -302,15 +396,6 @@ class SweepReader(threading.Thread):
self._sweep_idx: int = 0
self._last_sweep_ts: Optional[float] = None
self._n_valid_hist = deque()
# Счетчик потерь данных (выброшенных свипов из-за переполнения очереди)
self._dropped_sweeps: int = 0
# Диагностика потери точек внутри свипа
self._total_lines_received: int = 0 # Всего принято строк с данными
self._total_parse_errors: int = 0 # Ошибок парсинга строк
self._total_empty_lines: int = 0 # Пустых строк
self._max_buf_size: int = 0 # Максимальный размер буфера парсинга
self._read_errors: int = 0 # Ошибок чтения из порта
self._last_diag_time: float = 0.0 # Время последнего вывода диагностики
def _finalize_current(self, xs, ys, channels: Optional[set[int]]):
if not xs:
@ -402,31 +487,12 @@ class SweepReader(threading.Thread):
"mean": mean,
"std": std,
"dt_ms": dt_ms,
"dropped": self._dropped_sweeps,
"lines": self._total_lines_received,
"parse_err": self._total_parse_errors,
"read_err": self._read_errors,
"max_buf": self._max_buf_size,
}
# Периодический вывод детальной диагностики в stderr (каждые 10 секунд)
now = time.time()
if now - self._last_diag_time > 10.0:
self._last_diag_time = now
sys.stderr.write(
f"[DIAG] sweep={self._sweep_idx} n_valid={n_valid:.1f} "
f"lines={self._total_lines_received} parse_err={self._total_parse_errors} "
f"read_err={self._read_errors} max_buf={self._max_buf_size} "
f"dropped={self._dropped_sweeps}\n"
)
sys.stderr.flush()
# Кладём готовый свип (если очередь полна — выбрасываем самый старый)
try:
self._q.put_nowait((sweep, info))
except Full:
# Счетчик потерь для диагностики
self._dropped_sweeps += 1
try:
_ = self._q.get_nowait()
except Exception:
@ -452,22 +518,15 @@ class SweepReader(threading.Thread):
try:
# Быстрый неблокирующий дренаж порта с разбором по байтам
# Передаем счетчик ошибок чтения как список для изменения по ссылке
error_counter = [0]
chunk_reader = SerialChunkReader(self._src, error_counter)
chunk_reader = SerialChunkReader(self._src)
buf = bytearray()
while not self._stop.is_set():
data = chunk_reader.read_available()
# Обновляем счетчик ошибок чтения
self._read_errors = error_counter[0]
if data:
buf += data
# Отслеживаем максимальный размер буфера парсинга
if len(buf) > self._max_buf_size:
self._max_buf_size = len(buf)
else:
# Короткая уступка CPU, если нет новых данных (уменьшена до 0.1ms)
time.sleep(0.0001)
# Короткая уступка CPU, если нет новых данных
time.sleep(0.0005)
continue
# Обрабатываем все полные строки
@ -480,7 +539,6 @@ class SweepReader(threading.Thread):
if line.endswith(b"\r"):
line = line[:-1]
if not line:
self._total_empty_lines += 1
continue
if line.startswith(b"Sweep_start"):
@ -511,24 +569,16 @@ class SweepReader(threading.Thread):
x = int(parts[1], 10)
y = int(parts[2], 10) # поддержка знака: "+…" и "-…"
except Exception:
self._total_parse_errors += 1
continue
if cur_channel is None:
cur_channel = ch
cur_channels.add(ch)
xs.append(x)
ys.append(y)
self._total_lines_received += 1
else:
# Строка не в формате "s X Y"
self._total_parse_errors += 1
else:
# Строка слишком короткая
self._total_parse_errors += 1
# Защита от переполнения буфера при отсутствии переводов строки (снижен порог)
if len(buf) > 262144:
del buf[:-131072]
# Защита от переполнения буфера при отсутствии переводов строки
if len(buf) > 1_000_000:
del buf[:-262144]
finally:
try:
# Завершаем оставшийся свип
@ -542,106 +592,6 @@ class SweepReader(threading.Thread):
pass
def apply_temporal_unwrap(
current_phase: np.ndarray,
prev_phase: Optional[np.ndarray],
phase_offset: Optional[np.ndarray],
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Применяет улучшенный phase unwrapping для FMCW радара с адаптивным порогом.
Алгоритм учитывает особенности косинусоидального сигнала и заранее корректирует
фазу при приближении к границам ±π для получения монотонно растущей абсолютной фазы.
Args:
current_phase: Текущая фаза (развернутая по частоте) для всех бинов
prev_phase: Предыдущая фаза, может быть None при первом вызове
phase_offset: Накопленные смещения для каждого бина, может быть None
Returns:
(unwrapped_phase, new_prev_phase, new_phase_offset)
unwrapped_phase - абсолютная развёрнутая фаза (может быть > 2π)
new_prev_phase - обновлённая предыдущая фаза (для следующего вызова)
new_phase_offset - обновлённые смещения (для следующего вызова)
"""
n_bins = current_phase.size
# Инициализация при первом вызове
if prev_phase is None:
prev_phase = current_phase.copy()
phase_offset = np.zeros(n_bins, dtype=np.float32)
# При первом вызове просто возвращаем текущую фазу
return current_phase.copy(), prev_phase, phase_offset
if phase_offset is None:
phase_offset = np.zeros(n_bins, dtype=np.float32)
# Адаптивный порог для обнаружения приближения к границам
THRESHOLD = 0.8 * np.pi
# Вычисляем разницу между текущей и предыдущей фазой
delta = current_phase - prev_phase
# Обнаруживаем скачки и корректируем offset
# Используем улучшенный алгоритм с адаптивным порогом
# Метод 1: Стандартная коррекция для больших скачков (> π)
# Это ловит случаи, когда фаза уже перескочила границу
phase_offset = phase_offset - 2.0 * np.pi * np.round(delta / (2.0 * np.pi))
# Метод 2: Адаптивная коррекция при приближении к границам
# Проверяем текущую развернутую фазу
unwrapped_phase = current_phase + phase_offset
# Если фаза близка к нечетным π (π, 3π, 5π...), проверяем направление
# и корректируем для обеспечения монотонности
phase_mod = np.mod(unwrapped_phase + np.pi, 2.0 * np.pi) - np.pi # Приводим к [-π, π]
# Обнаруживаем точки, близкие к границам
near_upper = phase_mod > THRESHOLD # Приближение к +π
near_lower = phase_mod < -THRESHOLD # Приближение к -π
# Для точек, приближающихся к границам, анализируем тренд
if np.any(near_upper) or np.any(near_lower):
# Если delta положительна и мы около +π, готовимся к переходу
should_add = near_upper & (delta > 0)
# Если delta отрицательна и мы около -π, готовимся к переходу
should_sub = near_lower & (delta < 0)
# Применяем дополнительную коррекцию только там, где нужно
# (этот код срабатывает редко, только при быстром движении объекта)
pass # Основная коррекция уже сделана выше
# Финальная развернутая фаза
unwrapped_phase = current_phase + phase_offset
# Сохраняем текущую фазу как предыдущую для следующего свипа
new_prev_phase = current_phase.copy()
new_phase_offset = phase_offset.copy()
return unwrapped_phase, new_prev_phase, new_phase_offset
def phase_to_distance(phase: np.ndarray, center_freq_hz: float = 6e9) -> np.ndarray:
"""Преобразует развернутую фазу в расстояние для FMCW радара.
Формула: Δl = φ * c / (4π * ν)
где:
φ - фаза (радианы)
c - скорость света (м/с)
ν - центральная частота свипа (Гц)
Args:
phase: Развернутая фаза в радианах
center_freq_hz: Центральная частота диапазона в Гц (по умолчанию 6 ГГц для 2-10 ГГц)
Returns:
Расстояние в метрах
"""
c = 299792458.0 # Скорость света в м/с
distance = phase * c / (4.0 * np.pi * center_freq_hz)
return distance.astype(np.float32)
def main():
parser = argparse.ArgumentParser(
description=(
@ -692,6 +642,12 @@ def main():
default="auto",
help="Графический бэкенд: pyqtgraph (pg) — быстрее; matplotlib (mpl) — совместимый. По умолчанию auto",
)
parser.add_argument(
"--norm-type",
choices=["projector", "simple"],
default="projector",
help="Тип нормировки: projector (по огибающим в [-1,+1]) или simple (raw/calib)",
)
args = parser.parse_args()
@ -720,12 +676,12 @@ def main():
reader = SweepReader(args.port, args.baud, q, stop_event, fancy=bool(args.fancy))
reader.start()
# Графика (3 ряда x 2 колонки = 6 графиков)
fig, axs = plt.subplots(3, 2, figsize=(12, 12))
(ax_line, ax_img), (ax_fft, ax_spec), (ax_phase, ax_phase_wf) = axs
# Графика
fig, axs = plt.subplots(2, 2, figsize=(12, 8))
(ax_line, ax_img), (ax_fft, ax_spec) = axs
fig.canvas.manager.set_window_title(args.title) if hasattr(fig.canvas.manager, "set_window_title") else None
# Увеличим расстояния и оставим место справа под ползунки оси Y B-scan
fig.subplots_adjust(wspace=0.25, hspace=0.35, left=0.07, right=0.90, top=0.95, bottom=0.05)
fig.subplots_adjust(wspace=0.25, hspace=0.35, left=0.07, right=0.90, top=0.92, bottom=0.08)
# Состояние для отображения
current_sweep_raw: Optional[np.ndarray] = None
@ -744,11 +700,6 @@ def main():
ring_fft = None # type: Optional[np.ndarray]
y_min_fft, y_max_fft = None, None
freq_shared: Optional[np.ndarray] = None
# Phase состояние
ring_phase = None # type: Optional[np.ndarray]
prev_phase_per_bin: Optional[np.ndarray] = None
phase_offset_per_bin: Optional[np.ndarray] = None
y_min_phase, y_max_phase = None, None
# Параметры контраста водопада спектров
spec_clip = _parse_spec_clip(getattr(args, "spec_clip", None))
spec_mean_sec = float(getattr(args, "spec_mean_sec", 0.0))
@ -757,6 +708,7 @@ def main():
ymax_slider = None
contrast_slider = None
calib_enabled = False
norm_type = str(getattr(args, "norm_type", "projector")).strip().lower()
cb = None
# Статусная строка (внизу окна)
@ -775,7 +727,7 @@ def main():
line_calib_obj, = ax_line.plot([], [], lw=1, color="tab:red")
line_norm_obj, = ax_line.plot([], [], lw=1, color="tab:green")
ax_line.set_title("Сырые данные", pad=1)
ax_line.set_xlabel("F")
ax_line.set_xlabel("ГГц")
ax_line.set_ylabel("")
channel_text = ax_line.text(
0.98,
@ -791,8 +743,8 @@ def main():
# Линейный график спектра текущего свипа
fft_line_obj, = ax_fft.plot([], [], lw=1)
ax_fft.set_title("FFT", pad=1)
ax_fft.set_xlabel("X")
ax_fft.set_ylabel("Амплитуда, дБ")
ax_fft.set_xlabel("Время")
ax_fft.set_ylabel("дБ")
# Диапазон по Y для последнего свипа: авто по умолчанию (поддерживает отрицательные значения)
fixed_ylim: Optional[Tuple[float, float]] = None
@ -816,7 +768,7 @@ def main():
)
ax_img.set_title("Сырые данные", pad=12)
ax_img.set_xlabel("")
ax_img.set_ylabel("частота")
ax_img.set_ylabel("ГГц")
# Не показываем численные значения по времени на водопаде сырых данных
try:
ax_img.tick_params(axis="x", labelbottom=False)
@ -839,15 +791,9 @@ def main():
ax_spec.tick_params(axis="x", labelbottom=False)
except Exception:
pass
def _normalize_sweep(raw: np.ndarray, calib: np.ndarray) -> np.ndarray:
w = min(raw.size, calib.size)
if w <= 0:
return raw
out = np.full_like(raw, np.nan, dtype=np.float32)
with np.errstate(divide="ignore", invalid="ignore"):
out[:w] = raw[:w] / calib[:w]
out = np.nan_to_num(out, nan=np.nan, posinf=np.nan, neginf=np.nan)
return out
return _normalize_by_calib(raw, calib, norm_type=norm_type)
def _set_calib_enabled():
nonlocal calib_enabled, current_sweep_norm
@ -860,33 +806,6 @@ def main():
else:
current_sweep_norm = None
# График фазы текущего свипа
phase_line_obj, = ax_phase.plot([], [], lw=1)
ax_phase.set_title("Фаза спектра (развернутая)", pad=1)
ax_phase.set_xlabel("Бин")
ax_phase.set_ylabel("Фаза, радианы")
# Добавим второй Y axis для расстояния
ax_phase_dist = ax_phase.twinx()
ax_phase_dist.set_ylabel("Расстояние, м", color='green')
# Водопад фазы
img_phase_obj = ax_phase_wf.imshow(
np.zeros((1, 1), dtype=np.float32),
aspect="auto",
interpolation="nearest",
origin="lower",
cmap=args.cmap,
)
ax_phase_wf.set_title("Водопад фазы", pad=12)
ax_phase_wf.set_xlabel("")
ax_phase_wf.set_ylabel("Бин")
# Не показываем численные значения по времени
try:
ax_phase_wf.tick_params(axis="x", labelbottom=False)
except Exception:
pass
# Слайдеры для управления осью Y B-scan (мин/макс) и контрастом
try:
ax_smin = fig.add_axes([0.92, 0.55, 0.02, 0.35])
@ -923,19 +842,18 @@ def main():
def ensure_buffer(_w: int):
nonlocal ring, width, head, x_shared, ring_fft, freq_shared, ring_time
nonlocal ring_phase, prev_phase_per_bin, phase_offset_per_bin
if ring is not None:
return
width = WF_WIDTH
x_shared = np.arange(width, dtype=np.int32)
x_shared = np.linspace(3.3, 14.3, width, dtype=np.float32)
ring = np.full((max_sweeps, width), np.nan, dtype=np.float32)
ring_time = np.full((max_sweeps,), np.nan, dtype=np.float64)
head = 0
# Обновляем изображение под новые размеры: время по X (горизонталь), X по Y
img_obj.set_data(np.zeros((width, max_sweeps), dtype=np.float32))
img_obj.set_extent((0, max_sweeps - 1, 0, width - 1 if width > 0 else 1))
img_obj.set_extent((0, max_sweeps - 1, 3.3, 14.3))
ax_img.set_xlim(0, max_sweeps - 1)
ax_img.set_ylim(0, max(1, width - 1))
ax_img.set_ylim(3.3, 14.3)
# FFT буферы: время по X, бин по Y
ring_fft = np.full((max_sweeps, fft_bins), np.nan, dtype=np.float32)
img_fft_obj.set_data(np.zeros((fft_bins, max_sweeps), dtype=np.float32))
@ -943,14 +861,6 @@ def main():
ax_spec.set_xlim(0, max_sweeps - 1)
ax_spec.set_ylim(0, max(1, fft_bins - 1))
freq_shared = np.arange(fft_bins, dtype=np.int32)
# Phase буферы: время по X, бин по Y
ring_phase = np.full((max_sweeps, fft_bins), np.nan, dtype=np.float32)
prev_phase_per_bin = np.zeros(fft_bins, dtype=np.float32)
phase_offset_per_bin = np.zeros(fft_bins, dtype=np.float32)
img_phase_obj.set_data(np.zeros((fft_bins, max_sweeps), dtype=np.float32))
img_phase_obj.set_extent((0, max_sweeps - 1, 0, fft_bins - 1))
ax_phase_wf.set_xlim(0, max_sweeps - 1)
ax_phase_wf.set_ylim(0, max(1, fft_bins - 1))
def _visible_levels_matplotlib(data: np.ndarray, axis) -> Optional[Tuple[float, float]]:
"""(vmin, vmax) по текущей видимой области imshow (без накопления по времени)."""
@ -986,7 +896,6 @@ def main():
def push_sweep(s: np.ndarray):
nonlocal ring, head, ring_fft, y_min_fft, y_max_fft, ring_time
nonlocal ring_phase, prev_phase_per_bin, phase_offset_per_bin, y_min_phase, y_max_phase
if s is None or s.size == 0 or ring is None:
return
# Нормализуем длину до фиксированной ширины
@ -998,14 +907,13 @@ def main():
if ring_time is not None:
ring_time[head] = time.time()
head = (head + 1) % ring.shape[0]
# FFT строка (дБ) и фаза
# FFT строка (дБ)
if ring_fft is not None:
bins = ring_fft.shape[1]
# Подготовка входа FFT_LEN, замена NaN на 0
take_fft = min(int(s.size), FFT_LEN)
if take_fft <= 0:
fft_row = np.full((bins,), np.nan, dtype=np.float32)
phase_row = np.full((bins,), np.nan, dtype=np.float32)
else:
fft_in = np.zeros((FFT_LEN,), dtype=np.float32)
seg = s[:take_fft]
@ -1017,25 +925,12 @@ def main():
# Окно Хэннинга
win = np.hanning(take_fft).astype(np.float32)
fft_in[:take_fft] = seg * win
spec = np.fft.rfft(fft_in)
spec = np.fft.ifft(fft_in)
mag = np.abs(spec).astype(np.float32)
fft_row = 20.0 * np.log10(mag + 1e-9)
if fft_row.shape[0] != bins:
# rfft длиной FFT_LEN даёт bins == FFT_LEN//2+1
fft_row = fft_row[:bins]
# Расчет фазы
phase = np.angle(spec).astype(np.float32)
if phase.shape[0] > bins:
phase = phase[:bins]
# Unwrapping по частоте (внутри свипа)
phase_unwrapped_freq = np.unwrap(phase)
# Unwrapping по времени (между свипами)
phase_unwrapped_time, prev_phase_per_bin, phase_offset_per_bin = apply_temporal_unwrap(
phase_unwrapped_freq, prev_phase_per_bin, phase_offset_per_bin
)
phase_row = phase_unwrapped_time
ring_fft[(head - 1) % ring_fft.shape[0], :] = fft_row
# Экстремумы для цветовой шкалы
fr_min = np.nanmin(fft_row)
@ -1046,17 +941,6 @@ def main():
if y_max_fft is None or (not np.isnan(fr_max) and fr_max > y_max_fft):
y_max_fft = float(fr_max)
# Сохраняем фазу в буфер
if ring_phase is not None:
ring_phase[(head - 1) % ring_phase.shape[0], :] = phase_row
# Экстремумы для цветовой шкалы фазы
ph_min = np.nanmin(phase_row)
ph_max = np.nanmax(phase_row)
if y_min_phase is None or (not np.isnan(ph_min) and ph_min < y_min_phase):
y_min_phase = float(ph_min)
if y_max_phase is None or (not np.isnan(ph_max) and ph_max > y_max_phase):
y_max_phase = float(ph_max)
def drain_queue():
nonlocal current_sweep_raw, current_sweep_norm, current_info, last_calib_sweep
drained = 0
@ -1125,12 +1009,6 @@ def main():
base = ring_fft if head == 0 else np.roll(ring_fft, -head, axis=0)
return base.T # (bins, time)
def make_display_ring_phase():
if ring_phase is None:
return np.zeros((1, 1), dtype=np.float32)
base = ring_phase if head == 0 else np.roll(ring_phase, -head, axis=0)
return base.T # (bins, time)
def update(_frame):
nonlocal frames_since_ylim_update
changed = drain_queue() > 0
@ -1150,8 +1028,8 @@ def main():
line_norm_obj.set_data(xs[: current_sweep_norm.size], current_sweep_norm)
else:
line_norm_obj.set_data([], [])
# Лимиты по X постоянные под текущую ширину
ax_line.set_xlim(0, max(1, current_sweep_raw.size - 1))
# Лимиты по X: 3.3 ГГц .. 14.3 ГГц
ax_line.set_xlim(3.3, 14.3)
# Адаптивные Y-лимиты (если не задан --ylim)
if fixed_ylim is None:
y0 = float(np.nanmin(current_sweep_raw))
@ -1175,7 +1053,7 @@ def main():
seg = np.nan_to_num(sweep_for_fft[:take_fft], nan=0.0).astype(np.float32, copy=False)
win = np.hanning(take_fft).astype(np.float32)
fft_in[:take_fft] = seg * win
spec = np.fft.rfft(fft_in)
spec = np.fft.ifft(fft_in)
mag = np.abs(spec).astype(np.float32)
fft_vals = 20.0 * np.log10(mag + 1e-9)
xs_fft = freq_shared
@ -1184,30 +1062,9 @@ def main():
fft_line_obj.set_data(xs_fft[: fft_vals.size], fft_vals)
# Авто-диапазон по Y для спектра
if np.isfinite(np.nanmin(fft_vals)) and np.isfinite(np.nanmax(fft_vals)):
ax_fft.set_xlim(0, max(1, xs_fft.size - 1))
ax_fft.set_xlim(0, max(1, xs_fft.size - 1) * 1.5)
ax_fft.set_ylim(float(np.nanmin(fft_vals)), float(np.nanmax(fft_vals)))
# Расчет и отображение фазы текущего свипа
phase = np.angle(spec).astype(np.float32)
if phase.size > xs_fft.size:
phase = phase[: xs_fft.size]
# Unwrapping по частоте
phase_unwrapped = np.unwrap(phase)
phase_line_obj.set_data(xs_fft[: phase_unwrapped.size], phase_unwrapped)
# Авто-диапазон по Y для фазы
if np.isfinite(np.nanmin(phase_unwrapped)) and np.isfinite(np.nanmax(phase_unwrapped)):
ax_phase.set_xlim(0, max(1, xs_fft.size - 1))
phase_min = float(np.nanmin(phase_unwrapped))
phase_max = float(np.nanmax(phase_unwrapped))
ax_phase.set_ylim(phase_min, phase_max)
# Обновляем вторую ось Y с расстоянием
try:
dist_min = phase_to_distance(np.array([phase_min]))[0]
dist_max = phase_to_distance(np.array([phase_max]))[0]
ax_phase_dist.set_ylim(dist_min, dist_max)
except Exception:
pass
# Обновление водопада
if changed and ring is not None:
disp = make_display_ring()
@ -1255,24 +1112,6 @@ def main():
vmax_eff = vmin_v + c * (vmax_v - vmin_v)
img_fft_obj.set_clim(vmin=vmin_v, vmax=vmax_eff)
# Обновление водопада фазы
if changed and ring_phase is not None:
disp_phase = make_display_ring_phase()
img_phase_obj.set_data(disp_phase)
# Автодиапазон для фазы
try:
mean_phase = np.nanmean(disp_phase, axis=1)
vmin_p = float(np.nanmin(mean_phase))
vmax_p = float(np.nanmax(mean_phase))
except Exception:
vmin_p = vmax_p = None
# Фолбэк к отслеживаемым минимум/максимумам
if (vmin_p is None or not np.isfinite(vmin_p)) or (vmax_p is None or not np.isfinite(vmax_p)) or vmin_p == vmax_p:
if y_min_phase is not None and y_max_phase is not None and np.isfinite(y_min_phase) and np.isfinite(y_max_phase) and y_min_phase != y_max_phase:
vmin_p, vmax_p = y_min_phase, y_max_phase
if vmin_p is not None and vmax_p is not None and vmin_p != vmax_p:
img_phase_obj.set_clim(vmin=vmin_p, vmax=vmax_p)
if changed and current_info:
status_text.set_text(_format_status_kv(current_info))
chs = current_info.get("chs") if isinstance(current_info, dict) else None
@ -1341,7 +1180,7 @@ def run_pyqtgraph(args):
pg.setConfigOptions(useOpenGL=True, antialias=False)
app = pg.mkQApp(args.title)
win = pg.GraphicsLayoutWidget(show=True, title=args.title)
win.resize(1200, 900)
win.resize(1200, 600)
# Плот последнего свипа (слева-сверху)
p_line = win.addPlot(row=0, col=0, title="Сырые данные")
@ -1349,7 +1188,7 @@ def run_pyqtgraph(args):
curve = p_line.plot(pen=pg.mkPen((80, 120, 255), width=1))
curve_calib = p_line.plot(pen=pg.mkPen((220, 60, 60), width=1))
curve_norm = p_line.plot(pen=pg.mkPen((60, 180, 90), width=1))
p_line.setLabel("bottom", "X")
p_line.setLabel("bottom", "ГГц")
p_line.setLabel("left", "Y")
ch_text = pg.TextItem("", anchor=(1, 1))
ch_text.setZValue(10)
@ -1364,18 +1203,18 @@ def run_pyqtgraph(args):
p_img.getAxis("bottom").setStyle(showValues=False)
except Exception:
pass
p_img.setLabel("left", "X (0 снизу)")
p_img.setLabel("left", "ГГц")
img = pg.ImageItem()
p_img.addItem(img)
# FFT (слева-средний ряд)
# FFT (слева-снизу)
p_fft = win.addPlot(row=1, col=0, title="FFT")
p_fft.showGrid(x=True, y=True, alpha=0.3)
curve_fft = p_fft.plot(pen=pg.mkPen((255, 120, 80), width=1))
p_fft.setLabel("bottom", "Бин")
p_fft.setLabel("left", "Амплитуда, дБ")
p_fft.setLabel("bottom", "Время")
p_fft.setLabel("left", "дБ")
# Водопад спектров (справа-средний ряд)
# Водопад спектров (справа-снизу)
p_spec = win.addPlot(row=1, col=1, title="B-scan (дБ)")
p_spec.invertY(True)
p_spec.showGrid(x=False, y=False)
@ -1394,43 +1233,6 @@ def run_pyqtgraph(args):
cb_proxy.setWidget(calib_cb)
win.addItem(cb_proxy, row=2, col=1)
# График фазы (слева-снизу)
p_phase = win.addPlot(row=2, col=0, title="Фаза спектра (развернутая)")
p_phase.showGrid(x=True, y=True, alpha=0.3)
curve_phase = p_phase.plot(pen=pg.mkPen((120, 255, 80), width=1))
p_phase.setLabel("bottom", "Бин")
p_phase.setLabel("left", "Фаза, радианы")
# Добавим вторую ось Y для расстояния
p_phase_dist_axis = pg.ViewBox()
p_phase.showAxis("right")
p_phase.scene().addItem(p_phase_dist_axis)
p_phase.getAxis("right").linkToView(p_phase_dist_axis)
p_phase_dist_axis.setXLink(p_phase)
p_phase.setLabel("right", "Расстояние, м")
def updateViews():
try:
p_phase_dist_axis.setGeometry(p_phase.vb.sceneBoundingRect())
p_phase_dist_axis.linkedViewChanged(p_phase.vb, p_phase_dist_axis.XAxis)
except Exception:
pass
updateViews()
p_phase.vb.sigResized.connect(updateViews)
# Водопад фазы (справа-снизу)
p_phase_wf = win.addPlot(row=2, col=1, title="Водопад фазы")
p_phase_wf.invertY(True)
p_phase_wf.showGrid(x=False, y=False)
p_phase_wf.setLabel("bottom", "Время, с (новое справа)")
try:
p_phase_wf.getAxis("bottom").setStyle(showValues=False)
except Exception:
pass
p_phase_wf.setLabel("left", "Бин (0 снизу)")
img_phase = pg.ImageItem()
p_phase_wf.addItem(img_phase)
# Статусная строка (внизу окна)
status = pg.LabelItem(justify="left")
win.addItem(status, row=3, col=0, colspan=2)
@ -1451,15 +1253,11 @@ def run_pyqtgraph(args):
ring_fft: Optional[np.ndarray] = None
freq_shared: Optional[np.ndarray] = None
y_min_fft, y_max_fft = None, None
# Phase состояние
ring_phase: Optional[np.ndarray] = None
prev_phase_per_bin: Optional[np.ndarray] = None
phase_offset_per_bin: Optional[np.ndarray] = None
y_min_phase, y_max_phase = None, None
# Параметры контраста водопада спектров (процентильная обрезка)
spec_clip = _parse_spec_clip(getattr(args, "spec_clip", None))
spec_mean_sec = float(getattr(args, "spec_mean_sec", 0.0))
calib_enabled = False
norm_type = str(getattr(args, "norm_type", "projector")).strip().lower()
# Диапазон по Y: авто по умолчанию (поддерживает отрицательные значения)
fixed_ylim: Optional[Tuple[float, float]] = None
if args.ylim:
@ -1472,14 +1270,7 @@ def run_pyqtgraph(args):
p_line.setYRange(fixed_ylim[0], fixed_ylim[1], padding=0)
def _normalize_sweep(raw: np.ndarray, calib: np.ndarray) -> np.ndarray:
w = min(raw.size, calib.size)
if w <= 0:
return raw
out = np.full_like(raw, np.nan, dtype=np.float32)
with np.errstate(divide="ignore", invalid="ignore"):
out[:w] = raw[:w] / calib[:w]
out = np.nan_to_num(out, nan=np.nan, posinf=np.nan, neginf=np.nan)
return out
return _normalize_by_calib(raw, calib, norm_type=norm_type)
def _set_calib_enabled():
nonlocal calib_enabled, current_sweep_norm
@ -1499,31 +1290,24 @@ def run_pyqtgraph(args):
def ensure_buffer(_w: int):
nonlocal ring, ring_time, head, width, x_shared, ring_fft, freq_shared
nonlocal ring_phase, prev_phase_per_bin, phase_offset_per_bin
if ring is not None:
return
width = WF_WIDTH
x_shared = np.arange(width, dtype=np.int32)
x_shared = np.linspace(3.3, 14.3, width, dtype=np.float32)
ring = np.full((max_sweeps, width), np.nan, dtype=np.float32)
ring_time = np.full((max_sweeps,), np.nan, dtype=np.float64)
head = 0
# Водопад: время по оси X, X по оси Y
# Водопад: время по оси X, X по оси Y (ось Y: 3.3..14.3 ГГц)
img.setImage(ring.T, autoLevels=False)
p_img.setRange(xRange=(0, max_sweeps - 1), yRange=(0, max(1, width - 1)), padding=0)
p_line.setXRange(0, max(1, width - 1), padding=0)
img.setRect(0, 3.3, max_sweeps, 14.3 - 3.3)
p_img.setRange(xRange=(0, max_sweeps - 1), yRange=(3.3, 14.3), padding=0)
p_line.setXRange(3.3, 14.3, padding=0)
# FFT: время по оси X, бин по оси Y
ring_fft = np.full((max_sweeps, fft_bins), np.nan, dtype=np.float32)
img_fft.setImage(ring_fft.T, autoLevels=False)
p_spec.setRange(xRange=(0, max_sweeps - 1), yRange=(0, max(1, fft_bins - 1)), padding=0)
p_fft.setXRange(0, max(1, fft_bins - 1), padding=0)
freq_shared = np.arange(fft_bins, dtype=np.int32)
# Phase: время по оси X, бин по оси Y
ring_phase = np.full((max_sweeps, fft_bins), np.nan, dtype=np.float32)
prev_phase_per_bin = np.zeros(fft_bins, dtype=np.float32)
phase_offset_per_bin = np.zeros(fft_bins, dtype=np.float32)
img_phase.setImage(ring_phase.T, autoLevels=False)
p_phase_wf.setRange(xRange=(0, max_sweeps - 1), yRange=(0, max(1, fft_bins - 1)), padding=0)
p_phase.setXRange(0, max(1, fft_bins - 1), padding=0)
def _visible_levels_pyqtgraph(data: np.ndarray) -> Optional[Tuple[float, float]]:
"""(vmin, vmax) по текущей видимой области ImageItem (без накопления по времени)."""
@ -1558,7 +1342,6 @@ def run_pyqtgraph(args):
def push_sweep(s: np.ndarray):
nonlocal ring, ring_time, head, ring_fft, y_min_fft, y_max_fft
nonlocal ring_phase, prev_phase_per_bin, phase_offset_per_bin, y_min_phase, y_max_phase
if s is None or s.size == 0 or ring is None:
return
w = ring.shape[1]
@ -1569,7 +1352,7 @@ def run_pyqtgraph(args):
if ring_time is not None:
ring_time[head] = time.time()
head = (head + 1) % ring.shape[0]
# FFT строка (дБ) и фаза
# FFT строка (дБ)
if ring_fft is not None:
bins = ring_fft.shape[1]
take_fft = min(int(s.size), FFT_LEN)
@ -1578,27 +1361,13 @@ def run_pyqtgraph(args):
seg = np.nan_to_num(s[:take_fft], nan=0.0).astype(np.float32, copy=False)
win = np.hanning(take_fft).astype(np.float32)
fft_in[:take_fft] = seg * win
spec = np.fft.rfft(fft_in)
spec = np.fft.ifft(fft_in)
mag = np.abs(spec).astype(np.float32)
fft_row = 20.0 * np.log10(mag + 1e-9)
if fft_row.shape[0] != bins:
fft_row = fft_row[:bins]
# Расчет фазы
phase = np.angle(spec).astype(np.float32)
if phase.shape[0] > bins:
phase = phase[:bins]
# Unwrapping по частоте (внутри свипа)
phase_unwrapped_freq = np.unwrap(phase)
# Unwrapping по времени (между свипами)
phase_unwrapped_time, prev_phase_per_bin, phase_offset_per_bin = apply_temporal_unwrap(
phase_unwrapped_freq, prev_phase_per_bin, phase_offset_per_bin
)
phase_row = phase_unwrapped_time
else:
fft_row = np.full((bins,), np.nan, dtype=np.float32)
phase_row = np.full((bins,), np.nan, dtype=np.float32)
ring_fft[(head - 1) % ring_fft.shape[0], :] = fft_row
fr_min = np.nanmin(fft_row)
fr_max = np.nanmax(fft_row)
@ -1607,17 +1376,6 @@ def run_pyqtgraph(args):
if y_max_fft is None or (not np.isnan(fr_max) and fr_max > y_max_fft):
y_max_fft = float(fr_max)
# Сохраняем фазу в буфер
if ring_phase is not None:
ring_phase[(head - 1) % ring_phase.shape[0], :] = phase_row
# Экстремумы для цветовой шкалы фазы
ph_min = np.nanmin(phase_row)
ph_max = np.nanmax(phase_row)
if y_min_phase is None or (not np.isnan(ph_min) and ph_min < y_min_phase):
y_min_phase = float(ph_min)
if y_max_phase is None or (not np.isnan(ph_max) and ph_max > y_max_phase):
y_max_phase = float(ph_max)
def drain_queue():
nonlocal current_sweep_raw, current_sweep_norm, current_info, last_calib_sweep
drained = 0
@ -1689,33 +1447,16 @@ def run_pyqtgraph(args):
seg = np.nan_to_num(sweep_for_fft[:take_fft], nan=0.0).astype(np.float32, copy=False)
win = np.hanning(take_fft).astype(np.float32)
fft_in[:take_fft] = seg * win
spec = np.fft.rfft(fft_in)
spec = np.fft.ifft(fft_in)
mag = np.abs(spec).astype(np.float32)
fft_vals = 20.0 * np.log10(mag + 1e-9)
xs_fft = freq_shared
if fft_vals.size > xs_fft.size:
fft_vals = fft_vals[: xs_fft.size]
curve_fft.setData(xs_fft[: fft_vals.size], fft_vals)
p_fft.setXRange(0, max(1, xs_fft.size - 1) * 1.5, padding=0)
p_fft.setYRange(float(np.nanmin(fft_vals)), float(np.nanmax(fft_vals)), padding=0)
# Расчет и отображение фазы текущего свипа
phase = np.angle(spec).astype(np.float32)
if phase.size > xs_fft.size:
phase = phase[: xs_fft.size]
# Unwrapping по частоте
phase_unwrapped = np.unwrap(phase)
curve_phase.setData(xs_fft[: phase_unwrapped.size], phase_unwrapped)
phase_min = float(np.nanmin(phase_unwrapped))
phase_max = float(np.nanmax(phase_unwrapped))
p_phase.setYRange(phase_min, phase_max, padding=0)
# Обновляем вторую ось Y с расстоянием
try:
dist_min = phase_to_distance(np.array([phase_min]))[0]
dist_max = phase_to_distance(np.array([phase_max]))[0]
p_phase_dist_axis.setYRange(dist_min, dist_max, padding=0)
except Exception:
pass
if changed and ring is not None:
disp = ring if head == 0 else np.roll(ring, -head, axis=0)
disp = disp.T[:, ::-1] # (width, time with newest at left)
@ -1792,28 +1533,6 @@ def run_pyqtgraph(args):
else:
img_fft.setImage(disp_fft, autoLevels=False)
# Обновление водопада фазы
if changed and ring_phase is not None:
disp_phase = ring_phase if head == 0 else np.roll(ring_phase, -head, axis=0)
disp_phase = disp_phase.T[:, ::-1]
# Автодиапазон для фазы
levels_phase = None
try:
mean_phase = np.nanmean(disp_phase, axis=1)
vmin_p = float(np.nanmin(mean_phase))
vmax_p = float(np.nanmax(mean_phase))
if np.isfinite(vmin_p) and np.isfinite(vmax_p) and vmin_p != vmax_p:
levels_phase = (vmin_p, vmax_p)
except Exception:
levels_phase = None
# Фолбэк к отслеживаемым минимум/максимумам
if levels_phase is None and y_min_phase is not None and y_max_phase is not None and np.isfinite(y_min_phase) and np.isfinite(y_max_phase) and y_min_phase != y_max_phase:
levels_phase = (y_min_phase, y_max_phase)
if levels_phase is not None:
img_phase.setImage(disp_phase, autoLevels=False, levels=levels_phase)
else:
img_phase.setImage(disp_phase, autoLevels=False)
timer = pg.QtCore.QTimer()
timer.timeout.connect(update)
timer.start(interval_ms)