Скрипт WASD_flight¶
Скрипт WASD_flight является примером, того как можно управлять квадрокоптером Pioneer Mini с клавиатуры ПК. Служит для изучение методов перемещения квадрокоптера и методов библиотеки OpenCV по работе с клавиатурой. Скрипт позволяет управлять квадрокоптером при помощи клавиш «W»,»A»,»S»,»D»,»Q»,»E»,»H»,»L» и будет выполнятся до нажатия на клавишу «Escape».
Разбор скрипта¶
Импортируем необходимые библиотеки и определяем их назначение:
Далее используем конструкцию if __name__ == „__main__“:, которая является точкой входа в программу. Всё, что идёт до этого условия, выполнятся всегда: и при вызове в качестве модуля и при вызове, как исполняемый файл. (Подробное описание данной конструкции)
if __name__ == '__main__':
С помощью функции print выводим в консоль подсказку по управлению.
if __name__ == "__main__": print( """ 1 -- arm 2 -- disarm 3 -- takeoff 4 -- land ↶q w↑ e↷ i-↑ ←a d→ k-↓ s↓""" )
Создаем экземпляры классов 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
Далее используем конструкцию try..except. (Подробное описание данной конструкции)
В блоке 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
В переменную 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)
Создаем переменную 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-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)
При нажатии клавиш «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
При нажатии клавиш «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
Затем реализуем отправку сообщения с помощью метода .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)
Опишем блок finally для завершения работы.
finally: time.sleep(1) pioneer_mini.land() # Выполнить посадку pioneer_mini.close_connection() # Закрыть соединение del pioneer_mini # Удалить экземпляр класса pioneer_mini