Скрипт WASD_flight

Скрипт WASD_flight является примером, того как можно управлять квадрокоптером Pioneer Mini с клавиатуры ПК. Служит для изучение методов перемещения квадрокоптера и методов библиотеки OpenCV по работе с клавиатурой. Скрипт позволяет управлять квадрокоптером при помощи клавиш «W»,»A»,»S»,»D»,»Q»,»E»,»H»,»L» и будет выполнятся до нажатия на клавишу «Escape».

Разбор скрипта

  1. Импортируем необходимые библиотеки и определяем их назначение:
from pioneer_sdk import Pioneer
import cv2
import math
import numpy as np
  1. Далее используем конструкцию if __name__ == „__main__“:, которая является точкой входа в программу. Всё, что идёт до этого условия, выполнятся всегда: и при вызове в качестве модуля и при вызове, как исполняемый файл. (Подробное описание данной конструкции)
 if __name__ == '__main__':
  1. С помощью функции print выводим в консоль подсказку по управлению.
 if __name__ == "__main__":
   print(
         """
     1 -- arm
     2 -- disarm
     3 -- takeoff
     4 -- land
     ↶q  w↑  e↷    i-↑
     ←a      d→     k-↓
         s↓"""
   )
  1. Создаем экземпляры классов Pioneer и Camera (Подробнее о них можно узнать в разделах: Описание класса Pioneer, Описание класса Camera). Далее объявляем переменные min_v = 1300 и max_v = 1700. Переменные min_v и max_v принимают значения длины PPM сигнала с пульта РУ в микросекундах. Минимальное значение длины сигнала 1000 мкс, максимальное 2000 мкс, что соответствует крайнему верхнему/нижнему или левому/правому положению стика на пульте РУ. Значения min_v и max_v подобраны так, чтобы имитировать отклонения стиков на небольшое расстояние, относительно центрального положения стика (центральное положение-значение 1500мкс).
 pioneer_mini = Pioneer()
 camera = Camera()
 min_v = 1300
 max_v = 1700
  1. Далее используем конструкцию try..except. (Подробное описание данной конструкции)
  2. В блоке try входим в цикл с помощью while True. В теле цикла, будем задавать перемещение коптеру, получать изображение с камеры и выводить его на экран. Создаем переменные ch_1 - ch_4, в которых будем хранить значение длины PPM сигнала, соответствующее среднему положению стиков (1500мкс). Переменная ch_5 отвечает за положение тумблера SwС, значение 2000 переводит его в крайнее нижнее положение, что имитирует режим «Navigation».
 ch_1 = 1500 # Имитация центрального положения стиков и режима "Navigation"
 ch_2 = 1500
 ch_3 = 1500
 ch_4 = 1500
 ch_5 = 2000
  1. В переменную frame c помощью метода .get_frame() помещаем массив байтов в успешном случае и None - в противном случае. Выполняем проверку значения. Если значение внутри frame не равно None(есть изображение), создаем переменную camera_frame, внутрь которой передаем изображение от квадрокоптера.
Метод cv2.imdecode(buf, flag) - читает изображения из указного массива, где:
  • buf – читаемый массив, принимает значение np.frombuffer(frame, dtype=np.uint8);
  • flag – тип изображения, принимает значение cv2.IMREAD_COLOR.
Метод np.frombuffer(buffer, dtype) - интерпретирует буфер как одномерный массив, где:
  • buffer - буфер-подобный объект, в нашем случае это экземпляр класса Camera – frame;
  • dtype – тип данных, которым будут интерпретироваться элементы массива.
С помощью метода cv2.imshow(name, image) выводим изображение на экран, где:
  • name - имя создаваемого окна, в нашем случае это «pioneer_camera_stream»;
  • image – выводимое изображение – camera_frame.
 frame = camera.get_frame()
   if frame is not None:
     camera_frame = cv2.imdecode(
         np.frombuffer(frame, dtype=np.uint8), cv2.IMREAD_COLOR
     )
     cv2.imshow("pioneer_camera_stream", camera_frame)
  1. Создаем переменную key, в которой будем хранить информацию о нажатой клавише. Затем, с помощью конструкции if…elif проверяем key на соответсвие определённому в условии значению. Функция ord() возвращает число из таблицы символов Unicode, представляющее его позицию.
 key = cv2.waitKey(1) # Создаем переменную key
 if key == 27:  # Если key = 27(что соответсвует клавише ESC)
   print("esc pressed") # Вывести сообщение
   cv2.destroyAllWindows() # Закрыть все окна
   pioneer_mini.land() # Выполнить посадку
   break # Прервать выполнение
  1. При нажатии клавиш 1-2 производятся запуск и остановка двигателей с помощью методов .arm() и .disarm(). При нажатии клавиш 3-4 происходит вызов методов .takeoff() и.land() которые отвечают за выполнение взлёта и посадки.
 elif key == ord("1"):
   pioneer_mini.arm()
 elif key == ord("2"):
   pioneer_mini.disarm()
 elif key == ord("3"):
   time.sleep(2)
   pioneer_mini.arm()
   time.sleep(1)
   pioneer_mini.takeoff()
   time.sleep(2)
 elif key == ord("4"):
   time.sleep(2)
   pioneer_mini.land()
   time.sleep(2)
  1. При нажатии клавиш «W, S, A, D» происходит присваивание 3-у и 4-у каналу значений из переменных min_v, max_v. 3-й канал отвечает за управление по оси тангажа а 4-й - по оси крена.
 elif key == ord("w"):
     ch_3 = min_v
 elif key == ord("s"):
     ch_3 = max_v
 elif key == ord("a"):
     ch_4 = min_v
 elif key == ord("d"):
     ch_4 = max_v
  1. При нажатии клавиш «Q, E, I, K» происходит присваивание 2-у и 1-у каналу значений 1000, 2000, что соответсвует значению длины PPM сигнала при крайних положениях стика рысканьягаза (2-ой и 1-ый каналы).
 elif key == ord("q"):
     ch_2 = 2000
 elif key == ord("e"):
     ch_2 = 1000
 elif key == ord("i"):
     ch_1 = 2000
 elif key == ord("k"):
     ch_1 = 1000
  1. Затем реализуем отправку сообщения с помощью метода .send_rc_channels(). В качестве аргументов channel_1 - channel_5 передаем значения переменных ch_1 - ch_5. Добавляем задержку 0.02с внутри всего цикла while True.
 pioneer_mini.send_rc_channels(
   channel_1=ch_1,
   channel_2=ch_2,
   channel_3=ch_3,
   channel_4=ch_4,
   channel_5=ch_5,
 )
 time.sleep(0.02)
  1. Опишем блок finally для завершения работы.
finally:
 time.sleep(1)
 pioneer_mini.land() # Выполнить посадку

 pioneer_mini.close_connection() # Закрыть соединение
 del pioneer_mini # Удалить экземпляр класса pioneer_mini

Вопросы для самостоятельного разбора.

1) Дописать алгоритм, чтобы вместе с поворотом коптера по углу рысканья поворачивались оси X и Y.
2) Добавить чтобы при нажатии клавиши коптер не только менял свое направление движения, но и производил индикацию светодиодами.
3) Добавьте в данный скрипт возможность сохранения фотографии.