""" Визуализатор декодирования CRSF-кадра для урока по проекту ERLS_Rover. Задача стенда — показать НА ЭКРАНЕ то, что прошивка ESP32 делает невидимо внутри `CrsfParser.hpp`: как непрерывный поток байт из UART превращается в 16 чисел-каналов (1000..2000 мкс), а те — в позиции стиков/тумблеров RadioMaster Boxer. Педагогический смысл: ученик своими руками "двигает" виртуальные стики и тумблеры пульта (ползунки внизу окна), а программа в реальном времени: 1. упаковывает 16 каналов в 22 байта по 11 бит (как это делает передатчик Boxer); 2. добавляет заголовок 0xC8, длину, тип 0x16 и CRC8 — собирает НАСТОЯЩИЙ кадр; 3. распаковывает кадр ТЕМ ЖЕ алгоритмом, что и `CrsfParser::parseCrsfChannels()`; 4. прогоняет декодированные каналы через миксер `MotorDriver::drive()` и рисует, как поедет ровер. Ключевая учебная мысль: функции pack/unpack здесь — побитовая копия C++-кода из прошивки. Если поменять раскладку каналов в `enum Channel` (main.cpp), сломается и симуляция — ровно так же, как сломается реальный ровер при неверном Channel Order на пульте. Стенд делает эту причинно-следственную связь наглядной. Запуск (зависимости ставит uv автоматически): uv run crsf_visualizer.py Без uv: pip install numpy matplotlib python crsf_visualizer.py """ # /// script # requires-python = ">=3.11" # dependencies = ["numpy>=1.26", "matplotlib>=3.8"] # /// import numpy as np import matplotlib.pyplot as plt from matplotlib.widgets import Slider from matplotlib.patches import Rectangle, FancyArrow # --- Границы протокола CRSF, ОДИН-В-ОДИН с CrsfParser.hpp --- # Пульт шлёт каналы не в микросекундах, а в "попугаях" протокола: 172..1811. # Прошивка переводит их в стандартные RC-микросекунды 1000..2000 функцией map(). CRSF_MIN = 172 CRSF_MAX = 1811 US_MIN = 1000 US_MAX = 2000 # Карта каналов — копия enum Channel из main.cpp. Индексация с 1, как у оператора. CH_THROTTLE = 3 CH_YAW = 4 CH_ARM = 5 CH_GIMBAL_PAN = 6 CH_GIMBAL_TILT = 7 CH_GEAR = 8 ARM_THRESHOLD_US = 1700 # SA вверх (>1700) = ARM, см. main.cpp # ===================================================================== # ЧАСТЬ 1. Кодер: 16 каналов -> байтовый CRSF-кадр (роль передатчика Boxer) # ===================================================================== def us_to_crsf(us): """Обратный map(): микросекунды 1000..2000 -> "попугаи" 172..1811.""" val = np.interp(us, [US_MIN, US_MAX], [CRSF_MIN, CRSF_MAX]) return int(round(val)) def pack_channels(channels_us): """ Упаковывает 16 каналов (в мкс) в 22 байта по 11 бит подряд — тот самый плотный битовый поток без разделителей, что описан в rf.md. Это ровно обратная операция к parseCrsfChannels() в прошивке. """ values = [us_to_crsf(us) & 0x7FF for us in channels_us] # 11 бит на канал bits = 0 nbits = 0 payload = bytearray() for v in values: bits |= v << nbits # доклеиваем 11 бит канала к "хвосту" nbits += 11 while nbits >= 8: # как только набрался байт — выкладываем его payload.append(bits & 0xFF) bits >>= 8 nbits -= 8 if nbits > 0: payload.append(bits & 0xFF) return bytes(payload[:22]) # 11 бит * 16 = 176 бит = ровно 22 байта def crc8_dvb_s2(data): """CRC8 с полиномом 0xD5 — тот же алгоритм, что calculate_crc8() в прошивке.""" crc = 0 for b in data: crc ^= b for _ in range(8): if crc & 0x80: crc = ((crc << 1) ^ 0xD5) & 0xFF else: crc = (crc << 1) & 0xFF return crc def build_frame(channels_us): """ Собирает полный CRSF-кадр так, как его шлёт приёмник в UART ESP32: [0xC8][LEN][0x16][22 байта каналов][CRC8] LEN считается по протоколу: тип(1) + payload(22) + crc(1) = 24. """ payload = pack_channels(channels_us) frame_type = 0x16 body = bytes([frame_type]) + payload length = len(body) + 1 # +1 байт CRC crc = crc8_dvb_s2(body) return bytes([0xC8, length]) + body + bytes([crc]) # ===================================================================== # ЧАСТЬ 2. Декодер: байтовый кадр -> 16 каналов (КОПИЯ прошивки CrsfParser) # ===================================================================== def parse_crsf_channels(payload): """ Побитовая распаковка 16 каналов из 22 байт. Формулы сдвигов — построчная копия parseCrsfChannels() из CrsfParser.hpp. Если здесь ошибиться хоть в одном сдвиге, "поедет" номер канала — та же ошибка, что неверный Channel Order на Boxer. """ p = payload ch = [0] * 16 ch[0] = ( p[0] | (p[1] << 8)) & 0x7FF ch[1] = ((p[1] >> 3) | (p[2] << 5)) & 0x7FF ch[2] = ((p[2] >> 6) | (p[3] << 2) | (p[4] << 10)) & 0x7FF ch[3] = ((p[4] >> 1) | (p[5] << 7)) & 0x7FF ch[4] = ((p[5] >> 4) | (p[6] << 4)) & 0x7FF ch[5] = ((p[6] >> 7) | (p[7] << 1) | (p[8] << 9)) & 0x7FF ch[6] = ((p[8] >> 2) | (p[9] << 6)) & 0x7FF ch[7] = ((p[9] >> 5) | (p[10] << 3)) & 0x7FF ch[8] = ( p[11] | (p[12] << 8)) & 0x7FF ch[9] = ((p[12] >> 3) | (p[13] << 5)) & 0x7FF ch[10] = ((p[13] >> 6) | (p[14] << 2) | (p[15] << 10)) & 0x7FF ch[11] = ((p[15] >> 1) | (p[16] << 7)) & 0x7FF ch[12] = ((p[16] >> 4) | (p[17] << 4)) & 0x7FF ch[13] = ((p[17] >> 7) | (p[18] << 1) | (p[19] << 9)) & 0x7FF ch[14] = ((p[19] >> 2) | (p[20] << 6)) & 0x7FF ch[15] = ((p[20] >> 5) | (p[21] << 3)) & 0x7FF # "Попугаи" -> микросекунды, с зажимом в границы — как в прошивке. out = [] for v in ch: us = int(round(np.interp(v, [CRSF_MIN, CRSF_MAX], [US_MIN, US_MAX]))) out.append(int(np.clip(us, US_MIN, US_MAX))) return out def decode_frame(frame): """ Повторяет State-Machine из CrsfParser::update(): находит 0xC8, читает длину, проверяет тип 0x16 и CRC8, и только валидный кадр отдаёт на распаковку. Возвращает (каналы|None, crc_ok, len_ok). """ if len(frame) < 4 or frame[0] != 0xC8: return None, False, False length = frame[1] len_ok = 2 <= length <= 62 and len(frame) >= length + 2 if not len_ok: return None, False, False body = frame[2:2 + length - 1] # тип + payload (без CRC) crc_recv = frame[2 + length - 1] crc_ok = crc8_dvb_s2(body) == crc_recv if not crc_ok or body[0] != 0x16: return None, crc_ok, len_ok return parse_crsf_channels(body[1:]), True, True # ===================================================================== # ЧАСТЬ 3. Миксер ровера — копия MotorDriver::drive() из прошивки # ===================================================================== def rover_mixer(throttle_us, yaw_us, gear_us, armed): """Танковый (skid-steer) микшер: возвращает (left, right, mode) в диапазоне -255..255.""" if not armed: return 0, 0, 'N' raw_speed = int(np.interp(throttle_us, [1000, 2000], [0, 255])) if raw_speed < 15: raw_speed = 0 steering = int(np.interp(yaw_us, [1000, 2000], [-255, 255])) if abs(steering) < 15: steering = 0 if gear_us < 1300: throttle, mode = raw_speed, 'D' elif gear_us > 1700: throttle, mode = -raw_speed, 'R' else: throttle, mode = 0, 'N' left = int(np.clip(throttle + steering, -255, 255)) right = int(np.clip(throttle - steering, -255, 255)) return left, right, mode # ===================================================================== # ЧАСТЬ 4. Интерактивная визуализация (matplotlib) # ===================================================================== def main(): # Стартовое состояние всех 16 каналов: газ в 0 (безопасность), остальное в центр. channels = [1500] * 16 channels[CH_THROTTLE - 1] = 1000 channels[CH_ARM - 1] = 1000 # ARM выключен по умолчанию (SA внизу) channels[CH_GEAR - 1] = 1500 # нейтраль fig = plt.figure(figsize=(13, 8)) fig.suptitle("ERLS_Rover: путь сигнала Boxer → CRSF-кадр → ровер", fontsize=14, fontweight="bold") ax_frame = fig.add_axes([0.06, 0.72, 0.88, 0.16]) # байты кадра ax_chan = fig.add_axes([0.06, 0.40, 0.55, 0.26]) # бары каналов ax_rover = fig.add_axes([0.66, 0.40, 0.28, 0.26]) # вид ровера сверху def redraw(_=None): frame = build_frame(channels) decoded, crc_ok, len_ok = decode_frame(frame) # --- Панель 1: байты кадра --- ax_frame.clear() ax_frame.set_xlim(0, len(frame)) ax_frame.set_ylim(0, 1) ax_frame.axis("off") # Раскраска по смыслу байта: заголовок / длина / тип / данные / CRC. for i, b in enumerate(frame): if i == 0: color, tag = "#d1495b", "SYNC" # 0xC8 elif i == 1: color, tag = "#edae49", "LEN" elif i == 2: color, tag = "#66a182", "TYPE" # 0x16 elif i == len(frame) - 1: color, tag = "#8d5a97", "CRC8" else: color, tag = "#2e86ab", "" # 22 байта каналов ax_frame.add_patch(Rectangle((i, 0.25), 0.92, 0.5, facecolor=color, edgecolor="white")) ax_frame.text(i + 0.46, 0.5, f"{b:02X}", ha="center", va="center", color="white", fontsize=7, fontweight="bold") if tag: ax_frame.text(i + 0.46, 0.05, tag, ha="center", va="bottom", fontsize=6, color=color, fontweight="bold") status = "CRC OK — кадр принят" if crc_ok else "CRC FAIL — кадр ОТБРОШЕН" ax_frame.set_title(f"Сырой CRSF-кадр ({len(frame)} байт). {status}", fontsize=10, loc="left") # --- Панель 2: декодированные каналы --- ax_chan.clear() vals = decoded if decoded else [1500] * 16 xs = np.arange(1, 17) highlight = {CH_THROTTLE, CH_YAW, CH_ARM, CH_GIMBAL_PAN, CH_GIMBAL_TILT, CH_GEAR} colors = ["#2e86ab" if c in highlight else "#c9ccd1" for c in range(1, 17)] ax_chan.bar(xs, vals, color=colors) ax_chan.axhline(1500, color="#999", lw=0.8, ls="--") ax_chan.set_ylim(1000, 2000) ax_chan.set_xticks(xs) ax_chan.set_ylabel("мкс") ax_chan.set_title("Декодировано прошивкой (getChannel 1..16). " "Синие — используются кодом", fontsize=9) labels = {CH_THROTTLE: "Газ", CH_YAW: "Руль", CH_ARM: "ARM", CH_GIMBAL_PAN: "Pan", CH_GIMBAL_TILT: "Tilt", CH_GEAR: "D/N/R"} for c, name in labels.items(): ax_chan.text(c, vals[c - 1] + 15, name, ha="center", fontsize=7) # --- Панель 3: вид ровера сверху --- ax_rover.clear() ax_rover.set_xlim(-1.6, 1.6) ax_rover.set_ylim(-1.6, 1.6) ax_rover.set_aspect("equal") ax_rover.axis("off") armed = decoded and decoded[CH_ARM - 1] > ARM_THRESHOLD_US left, right, mode = rover_mixer( decoded[CH_THROTTLE - 1] if decoded else 1000, decoded[CH_YAW - 1] if decoded else 1500, decoded[CH_GEAR - 1] if decoded else 1500, bool(armed), ) body_color = "#66a182" if armed else "#c9ccd1" ax_rover.add_patch(Rectangle((-0.6, -0.9), 1.2, 1.8, facecolor=body_color, edgecolor="#333")) # Колёса: длина стрелки = мощность борта, знак = направление. for x, power in [(-0.75, left), (0.75, right)]: ax_rover.add_patch(Rectangle((x - 0.18, -0.7), 0.36, 1.4, facecolor="#333")) dy = power / 255 * 0.9 if abs(dy) > 0.02: ax_rover.add_patch(FancyArrow(x, 0, 0, dy, width=0.06, color="#d1495b", length_includes_head=True)) arm_txt = "ARM: ON" if armed else "ARM: OFF" ax_rover.set_title(f"Ровер сверху\n{arm_txt} | Режим {mode}\n" f"L={left} R={right}", fontsize=9) fig.canvas.draw_idle() # --- Ползунки = виртуальные органы управления Boxer --- slider_specs = [ ("Газ (CH3)", CH_THROTTLE, 1000), ("Руль (CH4)", CH_YAW, 1500), ("ARM SA (CH5)", CH_ARM, 1000), ("Pan S1 (CH6)", CH_GIMBAL_PAN, 1500), ("Tilt S2 (CH7)", CH_GIMBAL_TILT, 1500), ("Коробка SB (CH8)", CH_GEAR, 1500), ] sliders = [] for i, (label, ch, init) in enumerate(slider_specs): ax_s = fig.add_axes([0.12, 0.30 - i * 0.045, 0.76, 0.025]) s = Slider(ax_s, label, 1000, 2000, valinit=init, valstep=1) def make_cb(channel_index): def cb(val): channels[channel_index - 1] = int(val) redraw() return cb s.on_changed(make_cb(ch)) sliders.append(s) redraw() plt.show() if __name__ == "__main__": main()