Управление по WASD через RC-каналы

import select                                  # select нужен для чтения клавиш без блокировки программы
import sys                                     # sys нужен для доступа к вводу с клавиатуры через stdin
import termios                                 # termios нужен для настройки терминала
import time                                    # time нужен для задержек и таймеров
import tty                                     # tty нужен для чтения клавиш без нажатия Enter

from pioneer_sdk2 import Pioneer               # импортируем класс Pioneer из библиотеки pioneer_sdk2


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
}

ROLL_VALUE = 0.25                              # величина отклонения правого стика влево/вправо
PITCH_VALUE = 0.25                             # величина отклонения правого стика вперед/назад
THROTTLE_VALUE = 0.25                          # величина отклонения левого стика вверх/вниз
YAW_VALUE = 0.25                               # величина отклонения левого стика для поворота

SEND_PERIOD = 0.05                             # RC-каналы нужно отправлять постоянно
KEY_TIMEOUT = 0.2                              # через сколько секунд без повтора считать клавишу отпущенной
MAIN_LOOP_DELAY = 0.01                         # небольшая задержка главного цикла

NEUTRAL_CHANNELS = (0.0, 0.0, 0.0, 0.0)        # нейтральные стики: roll, pitch, throttle, yaw

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: левый стик поворот вправо
}


def setup_rc_parameters(drone):                # функция настраивает автопилот для приема RC-каналов от SDK
    old_parameters = {}                         # здесь сохраним значения параметров до изменения

    for name, value in RC_PARAMETERS.items():  # перебираем параметры ручного управления
        old_parameters[name] = drone.get_param(name, update=True) # читаем текущее значение параметра
        if not drone.set_param(name, value):   # устанавливаем очередной параметр автопилота
            restore_rc_parameters(drone, old_parameters) # возвращаем уже измененные параметры
            raise RuntimeError(f"Не удалось установить параметр {name}")

    return old_parameters                       # возвращаем старые значения, чтобы восстановить их при выходе


def restore_rc_parameters(drone, old_parameters): # функция возвращает параметры автопилота к исходным значениям
    for name, value in old_parameters.items():  # перебираем сохраненные параметры
        if value is not None:                  # если параметр удалось прочитать перед запуском
            drone.set_param(name, value)        # возвращаем старое значение


def read_key():                                # функция читает последнюю нажатую клавишу
    key = None                                 # None означает, что новых клавиш нет

    while select.select([sys.stdin], [], [], 0)[0]: # читаем все символы, которые уже пришли в терминал
        key = sys.stdin.read(1)                # берем один символ
        if key != "\x1b":                      # Esc оставляем как есть
            key = key.lower()                  # остальные клавиши приводим к нижнему регистру

    return key                                 # возвращаем последнюю клавишу из очереди


print(
    """
Управление Pioneer Mini 2 через RC-каналы SDK2

1 -- arm        2 -- disarm
3 -- takeoff    4 -- land

↶q  w↑  e↷     Space -- вверх
←a      d→      z     -- вниз
    s↓

x -- нейтральные стики, Esc -- посадка и выход
"""
)

if not sys.stdin.isatty():                     # проверяем, что программа запущена в интерактивном терминале
    raise RuntimeError("Запустите пример в интерактивном SSH-терминале")

drone = Pioneer()                              # создаем объект управления дроном
terminal_settings = termios.tcgetattr(sys.stdin) # сохраняем настройки терминала, чтобы восстановить их при выходе
old_rc_parameters = {}                         # здесь будут значения параметров АП до запуска программы

last_motion_key = None                         # последняя клавиша движения
last_motion_time = 0.0                         # время последнего получения клавиши движения
last_send_time = 0.0                           # время последней отправки RC-каналов

try:
    old_rc_parameters = setup_rc_parameters(drone) # перед управлением переводим RC-источник на SDK
    tty.setcbreak(sys.stdin.fileno())          # включаем чтение клавиш без Enter

    while True:
        now = time.monotonic()                 # текущее время для таймеров
        key = read_key()                       # читаем клавишу, если она была нажата
        state = drone.get_fly_state().name     # состояние дрона: ON_LAND, ARMED или IN_SKY

        if key in CHANNELS_BY_KEY:             # если нажата клавиша движения
            last_motion_key = key              # запоминаем ее
            last_motion_time = now             # обновляем время последнего нажатия

        elif key == "x":                       # X - вернуть стики в нейтраль
            last_motion_key = None

        elif key == "1" and state == "ON_LAND": # 1 - включить двигатели
            drone.arm()

        elif key == "2" and state == "ARMED":  # 2 - выключить двигатели до взлета
            drone.disarm()

        elif key == "3" and state != "IN_SKY": # 3 - взлет
            if state == "ON_LAND":             # если двигатели еще не включены
                drone.arm()                    # включаем их перед взлетом
            if drone.get_fly_state().name == "ARMED": # проверяем, что двигатели включились
                drone.takeoff()                # взлетаем штатной командой SDK

        elif key == "4" and state == "IN_SKY": # 4 - посадка
            drone.send_rc_channels(*NEUTRAL_CHANNELS) # сначала возвращаем стики в нейтраль
            drone.land()                       # затем садимся штатной командой SDK
            last_motion_key = None

        elif key == "\x1b":                    # Esc - выход из программы
            break

        # В 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()

        time.sleep(MAIN_LOOP_DELAY)

finally:
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, terminal_settings) # возвращаем терминал в обычный режим

    drone.send_rc_channels(*NEUTRAL_CHANNELS)   # перед выходом возвращаем стики в нейтраль

    state = drone.get_fly_state().name         # проверяем состояние перед выходом
    if state == "IN_SKY":                      # если программа завершается в полете
        drone.land()                           # сажаем дрон
    elif state == "ARMED":                     # если двигатели включены, но взлета не было
        drone.disarm()                         # выключаем двигатели

    restore_rc_parameters(drone, old_rc_parameters) # возвращаем параметры автопилота к исходным значениям
    drone.close_connection()                   # закрываем соединение с дроном