Разбор: rc_channels_examples

Источник: репозиторий pioneer-team/pioneer-sdk2-example (GitFlic). Справочник по классам API — на странице «Pioneer SDK 2».

Что внутри

send_rc_channels.py — постоянная отправка нейтральных значений каналов SDK2.
wasd_rc_channels.py — управление дроном по WASD через RC-каналы с восстановлением параметров автопилота при выходе.
README репозитория также упоминает пример rc_sdk1_to_sdk2.py — формулу конвертации значений SDK1 (центр 1500) в нормированные каналы SDK2.

Перед отправкой каналов оба скрипта переводят источник RC-команд на SDK настройкой параметров автопилота.

Параметры автопилота

Без этого шага каналы до автопилота доходят, но игнорируются им. Пять параметров задают режимы управления, разрешают полёт без аппаратного пульта и переключают источник RC-команд на SDK:

RC_PARAMETERS = {                       # параметры автопилота для управления через RC-каналы
    "Copter_man_rcMode0": 6.0,          # режим управления для положения 0
    "Copter_man_rcMode1": 3.0,          # режим управления для положения 1
    "Copter_man_rcMode2": 3.0,          # режим управления для положения 2
    "Copter_flyWithoutRc": 1.0,         # разрешаем полет без аппаратного пульта
    "SensorMux_rc": 2.0,                # выбираем источник RC-команд от SDK
}


def setup_rc_parameters(drone):         # функция настраивает автопилот для приема RC-каналов от SDK
    for name, value in RC_PARAMETERS.items(): # перебираем параметры ручного управления
        if not drone.set_param(name, value):  # устанавливаем очередной параметр автопилота
            raise RuntimeError(f"Не удалось установить параметр {name}") # прерываем запуск, если параметр не применился

set_param() возвращает True/False: если параметр не применился, скрипт прерывает запуск понятной ошибкой.

Семантика каналов SDK2

Каналы нормированы от −1 до 1. Восемь аргументов send_rc_channels() расшифровываются комментариями в примере:

try:                                    # основной код находится внутри блока try
    setup_rc_parameters(drone)          # настраиваем автопилот перед отправкой RC-каналов
    while True:                         # запускаем бесконечный цикл
        drone.send_rc_channels(         # отправляем значения каналов пульта
            channel_1=0,                # правый стик: влево -1, центр 0, вправо 1
            channel_2=0,                # правый стик: вперед -1, центр 0, назад 1
            channel_3=0,                # левый стик: вверх 1, центр 0, вниз -1
            channel_4=0,                # левый стик: налево 1, центр 0, направо -1
            channel_5=1,                # тумблер SWC: вверх 0, центр 1, вниз 2
            channel_6=0,                # тумблер SWD: вверх 0, вниз 2
            channel_7=1,                # тумблер SWB: вверх 0, центр 1, вниз 2
            channel_8=0                 # тумблер SWA: вверх 0, вниз 2
        )

        time.sleep(0.05)                # ставим паузу на 0.05 секунды

channel_1 (roll): −1 — стик влево, 1 — вправо.
channel_2 (pitch): −1 — вперёд, 1 — назад (знак инвертирован относительно привычного «вперёд = плюс»).
channel_3 (throttle): 1 — вверх, −1 — вниз.
channel_4 (yaw): 1 — влево, −1 — вправо.
channel_5…8 — тумблеры SWC/SWD/SWB/SWA: положения 0/1/2 (SWD и SWA — только 0 и 2).

Каналы нужно отправлять постоянно с паузой около 0,05 с, иначе ручной режим от автопилота «отваливается».

wasd_rc_channels.py — WASD через RC-каналы

Пример добавляет интерактивность: таблица CHANNELS_BY_KEY сопоставляет клавиши и отклонения стиков:

CHANNELS_BY_KEY = {                            # таблица соответствия клавиш и RC-каналов SDK2
    "a": (-ROLL_VALUE, 0.0, 0.0, 0.0),         # channel_1: правый стик влево
    "d": (ROLL_VALUE, 0.0, 0.0, 0.0),          # channel_1: правый стик вправо
    "w": (0.0, -PITCH_VALUE, 0.0, 0.0),        # channel_2: правый стик вперед
    "s": (0.0, PITCH_VALUE, 0.0, 0.0),         # channel_2: правый стик назад
    "z": (0.0, 0.0, -THROTTLE_VALUE, 0.0),     # channel_3: левый стик вниз
    " ": (0.0, 0.0, THROTTLE_VALUE, 0.0),      # channel_3: левый стик вверх
    "q": (0.0, 0.0, 0.0, YAW_VALUE),           # channel_4: левый стик поворот влево
    "e": (0.0, 0.0, 0.0, -YAW_VALUE),          # channel_4: левый стик поворот вправо
}

В SSH-терминале нет события «клавишу отпустили», поэтому стики возвращаются в нейтраль, если автоповтор клавиши давно не приходил (KEY_TIMEOUT = 0.2 с). Отправка каналов идёт строго по таймеру:

        # В SSH-терминале нет события "клавишу отпустили".
        # Если автоповтор клавиши давно не приходил, считаем, что стики вернулись в нейтраль.
        if last_motion_key is not None and now - last_motion_time > KEY_TIMEOUT:
            last_motion_key = None

        channels = CHANNELS_BY_KEY.get(last_motion_key, NEUTRAL_CHANNELS) # движение по клавише или нейтраль
        send_due = time.monotonic() - last_send_time >= SEND_PERIOD       # пора повторить RC-каналы

        if send_due:
            drone.send_rc_channels(              # постоянно отправляем все каналы, даже нейтральные
                channel_1=channels[0],           # roll:  -1 влево, 0 центр, 1 вправо
                channel_2=channels[1],           # pitch: -1 вперед, 0 центр, 1 назад
                channel_3=channels[2],           # throttle: -1 вниз, 0 центр, 1 вверх
                channel_4=channels[3],           # yaw: 1 влево, 0 центр, -1 вправо
                channel_5=1,                     # положение режима управления
                channel_6=0,                     # дополнительный тумблер
                channel_7=1,                     # дополнительный тумблер
                channel_8=0,                     # дополнительный тумблер
            )
            last_send_time = time.monotonic()

Взлёт и посадка выполняются штатными командами arm()/takeoff()/land(), движение после взлёта — через RC-каналы. Приёмы, достойные внимания:

Перед выходом скрипт отправляет нейтральные каналы, сажает дрон (или снимает ARMED) и вызывает restore_rc_parameters() — возвращает автопилоту значения, прочитанные get_param() до запуска.
setup_rc_parameters() заранее сохраняет старые значения — это правило гигиены: меняешь параметры, верни всё обратно.
Скрипт требует интерактивный терминал (sys.stdin.isatty()); из IDE без stdin не запустится.