Система автоматической разгрузки и загрузки дрона (Часть 2 — автопилот)

Данная статья включает в себя:

1. Видео демонстрация

2. Алгоритм, который мы использовали

3. Программирование Geoscan Pioneer max (небольшой туториал, будет интересен не всем)

Видео демонстрация

Конструкция из Система автоматической разгрузки и загрузки дрона (Часть 1 — конструкция) / Хабр (habr.com)

Для проверки работы в условиях приближенных к реальным, мы прикрепили наши посадочные места к имитации окна для определения необходимого расстояния от поверхности, к которой закреплено посадочное место, при использование автопилота (ветер ни кто не отменял; дрон должен иметь возможность для экстренного манёвра). Ниже представлено видео после множественных тестовых полётов, которые помогли нам отладить систему и доработать конструкцию посадочного места (задержки перед каждой командой по 16 секунд (для того чтобы контролировать весь полёт), поэтому мы ускорили видео):

Результат тестовых полётов:

1. Автопилоту сложно контролировать себя при залёте на горизонтальную площадку (решение-сократить площадь поверхности)

2. Вынести платформу ещё на 15 сантиметров от стены (выравниваясь, очень близко подлетает к вертикальной поверхности, и малейший ветерок прижмёт его к стене/окну)

Алгоритм, который мы использовали

Так как нам требовалось доработать конструкцию в условиях будущего, мы решили разработать алгоритм для автоматического полёта и провести тестирование нашей конструкции для «тупой машины». К счастью нам не понадобилось писать с нуля автопилот, так как у нашего дрона Geoscan Pioneer max, он встроенный, несмотря на это нам всё равно понадобилось разработать алгоритм построения траектории движения, по двум вводимым точкам посадки (третья — стартовая точка).

Алгоритм (упрощённо):

Программирование Geoscan Pioneer max, подключение сервопривода

Для начала мы подключили сервопривод, для этого нам понадобится Rapberry (так как встроенная система не умеет общаться с сервоприводом), подключаем Raspberry по инструкции в интернете и теперь встаёт вопрос куда подключать серво и что делать. Некоторые пины используются по умолчанию (но информацию об этом можно не найти на сайте геоскана) использование таких пинов может повредить сервопривод, поэтому мы рекомендуем использовать, например, пин GPIO25. Ну и конечно Ground и Power.

Необходимые методы API для программирования на Lua

Uart.new(num, rate, parity, stopBits) — создать Uart на порте с настройками.

Uart.read(self, size) — прочитать size байт.

Uart.write(self, data, size) — записать данные (data) длиной (size).

Uart.bytesToRead(self) — количество данных доступных для чтения.

Ниже представлены программы для работы сервопривода, так как вряд ли вы будете использовать только сервопривод в одностороннем порядке, то в предложенных программах уже реализована двухсторонняя связь, НО ограничение на передачу 31 символ, всё составлялось на основе документации (Программирование на Lua — Документация Pioneer December update 2022 (geoscan.aero)), НО информация имеет особенности применения.

-----------------Lua код, загружается на дрон через Pioneer station----------- local uart = Uart.new(4, 57600) -- объявляем uart для общения с pyhon кодом local servo_stat = 'o'    -- статус сервопривода(например 'o'(открыто),'c'(закрыто)) local rc = Sensors.rc     -- подключаем пульт local inp = ''            -- переменная для хранения информации для отправки на Python local rec = ''            -- переменная для хранения ответа(для будущего применения)  local function rotate_servo_open() -- функция открытия сервопривода   servo_stat='o' end  local function rotate_servo_close() -- функция закрытия сервопривода   servo_stat='c' end  local function main()               -- цикл   rc_chans = table.pack(rc())     -- получаем иинформацию с пульта   if rc_chans[8] < -0.8 then      --открытие (канал посмотри на пульте)     rotate_servo_open()   elseif rc_chans[8] > 0.8 then   --закрытие (канал посмотри на пульте)     rotate_servo_close()   end   inp = servo_stat..'\n' -- пусть разделитель '\n'   uart:write(inp, #inp)  -- отправляем на Python   rec = uart:read(uart:bytesToRead())    -- принимаем данные с Python end  t = Timer.new(0.08, main) --устанавливаем частоту цикла ---!ВРЕМЯ СИНХРОНИЗАЦИИ ДОЛЖНО БЫТЬ ОДИНАКОВО НА LUA и PYTHON! t:start() -- начинаем цикл ----------!!!КОВЫЧКИ ОДИНАРНЫЕ ОБЯЗАТЕЛЬНО, ИНАЧЕ РАБОТАТЬ НЕ БУДЕТ !!!---------------

##########################Python код, загружается на Raspberry######################## import serial             # библиотека для общения from time import sleep    # библиотека для синхронизации import RPi.GPIO as GPIO   # библиотека для общения ser = serial.Serial("/dev/ttyS0", 57600, timeout=5) # открываем порт, бездумно не меняй GPIO.setmode(GPIO.BCM)    # объявляем для общения с сервоприводом GPIO.setup(25, GPIO.OUT)  # объявляем для общения с сервоприводом sg = GPIO.PWM(25, 50)     # объявляем сервопривод sg.start(8.06)            # объявляем задаём начальный угол servo_opened = True       # по умолчанию замок открыт  def uart_read():          # функция чтения с Lua   data = ser.readline().decode().replace('\n', "") # читаем, разделяем, убираем мусор   if ser.in_waiting > 20:      # чтобы не зависало, бездумно не меняй       ser.reset_input_buffer() # чтобы не зависало, бездумно не меняй    print('Read data: ')   print(data)             return data  def uart_write(answer): # функция ответа для Lua   ser.write(answer.encode()) # кодируем, пишем   print('UART writed')  def servo_control(event): # функция управления сервоприводом   global servo_opened   # подключаем глобальные переменные   global sg             # подключаем глобальные переменные   if event == "c" and servo_opened: # закрываем серво если получили 'c'       sg.ChangeDutyCycle(2.5)       servo_opened = False       print('closed')      elif event == "o" and not (servo_opened): # открываем серво если получили 'o'       sg.ChangeDutyCycle(8.06)       servo_opened = True       print('opened')  def auto_p_control(data):   servo_event = data[0]   servo_control(servo_event) # вызов функции для поворота сервопривода    print("ANS : " + str(ans))   uart_write(ans)  # отправка данных на Lua   sleep(0.08)  # синхронизация  while True: # бесконечный цикл   uart_data = uart_read() # вызываем функцию чтения   if uart_data != ['']:  #если прочитанные данные не пустые       auto_p_control(uart_data) # вызываем функцию ответа  

Программирование Geoscan Pioneer max, программирование автоматического полёта в локальной системе координат

Для начала разберёмся с тем, какие команды дрон может принять:

Для того чтобы отправить команду, мы будем использовать метод

ap.push(Event)

Но так же дрон нам будет присылать свои выполненные действия в функцию

function callback(event) - Вызывается, когда приходят события от автопилота.

Доступны следующие события, приходящие от автопилота:

Из этого обширного круга событий нам понадобятся:

• TAKEOFF_COMPLETE

• POINT_REACHED

Ну и только теперь перейдём к методу, который ключевой для написания автопилота:

ap.goToLocalPoint(x, y, z, time) — для полёта с использованием локальной системы координат.

Теперь можем приступить к написанию автопилота.

Возьмём за шаблон данный кусок кода:

-----------------Lua код, загружается на дрон через Pioneer station----------- local rc = Sensors.rc     -- подключаем пульт local lpsPosition = Sensors.lpsPosition -- подключаем steamVR станцию (если есть, иначе удалить строчки связанные с данной переменной)  local points = {         {-0.6, 0.3, 0.2},         {0.6, 0.3,  0.2},         {0, 0, 0.5},         {0.6, -0.3, 0.2} }  local curr_point = 1  local function nextPoint()     if(#points >= curr_point) then         ap.goToLocalPoint(points[curr_point][1], points[curr_point][2], points[curr_point][3]         curr_point = curr_point + 1     else         ap.push(Ev.MCE_LANDING)     end end  function callback(event)     if (event == Ev.TAKEOFF_COMPLETE) then         nextPoint()     end     if (event == Ev.POINT_REACHED) then         nextPoint()     end end local function main()               -- цикл   rc_chans = table.pack(rc())     -- получаем иинформацию с пульта   lpsX, lpsY, lpsZ = lpsPosition() -- получаем координаты относительно steamVR станции end  t = Timer.new(0.05, main) --устанавливаем частоту цикла (например, 0.05 секунды) t:start() -- начинаем цикл  ap.push(Ev.MCE_PREFLIGHT) -- запускаем двигатели при старте Timer.callLater(1, function() ap.push(Ev.MCE_TAKEOFF) end)  -- поднимаем в воздух при старте

Далее, если хотим сами записывать точки (две) и использовать сервопривод, то берём этот код за основу:

uart = Uart.new(4, 57600) servo_stat = 'o' send_auto = '0'  inp = '' rc = Sensors.rc autopilot = False auto_pilot_ret = False flight_loaded = False point = [             [0, 0, 0],             [0, 0, 0],             [0, 0, 0],             [''],             [0, 0, 0],             [0, 0, 0],             [0, 0, 0],             [''],             [0, 0, 0],             [0, 0, 0],             [0, 0, 0],             ['']         ]  point_upploaded = [0,0,0],[0,0,0],[0,0,0]] point_dx = 1 lock = False point_now = 1 rec = ''   ENGINES_STARTED=False TAKEOFF_COMPLETE=False POINT_REACHED=False LOW_VOLTAGE1=False SHOCK=False  lpsPosition = Sensors.lpsPosition  leds = Ledbar.new(4) for i = 0, 3, 1 do     leds:set(i, 0, 0, 0)   def indication(r,g,b)     for i = 0, 4, 1 do         leds:set(i, r, g, b)    def info_light()     for i=0,1,0.1 do         indication(i,i,i)         indication(1,1,1)      indication(0,0,0)    def round(num, step)     return num - num % step   def rotate_servo_open()     indication(0,1,0)     indication(0,0,0)     servo_stat='o'   def rotate_servo_close()     #демонстрационная индикация[     indication(1,0,0)     indication(0,0,0)     #демонстрационная индикация]      servo_stat='c'   def autopilot_on_to_off()     autopilot = False     indication(1, 1, 1)   def autopilot_off_to_on()  # включить автопилот     if point_dx == 3 :  # не даст включить автопилот, если не записали две точки         point_upploaded[3][1] = round(posX_now, 0.01)         point_upploaded[3][2] = round(posY_now, 0.01)         point_upploaded[3][3] = round(posZ_now, 0.01)                  point = [[point_upploaded[3][1], point_upploaded[3][2], point_upploaded[3][3]+0.5],                  [point_upploaded[1][1], point_upploaded[1][2], point_upploaded[1][3]+0.5],                  [point_upploaded[1][1], point_upploaded[1][2], point_upploaded[1][3]+0.3],                  ['land'],                  [point_upploaded[1][1], point_upploaded[1][2], point_upploaded[1][3]+0.5],                  [point_upploaded[2][1], point_upploaded[2][2], point_upploaded[2][3]+0.5],                  [point_upploaded[2][1], point_upploaded[2][2], point_upploaded[2][3]+0.3],                  ['land'],                  [point_upploaded[2][1], point_upploaded[2][2], point_upploaded[2][3]+0.5],                  [point_upploaded[3][1], point_upploaded[3][2], point_upploaded[3][3]+0.5],                  [point_upploaded[3][1], point_upploaded[3][2], point_upploaded[3][3]+0.1],                  ['land']                 ]          autopilot = True         flight_loaded = True          auto_pilot_ret = False         point_now = 1  def autopilot_point_add()     if point_dx < 3 :         point_upploaded[point_dx][1] = round(posX_now, 0.01)         point_upploaded[point_dx][2] = round(posY_now, 0.01)         point_upploaded[point_dx][3] = round(posZ_now, 0.01)         point_dx = point_dx + 1         indication(1, 0, 0)         send_auto = 'a'         lock = True  def autopilot_point_clear()     point_upploaded[1] = [0,0,0]     point_upploaded[2] = [0,0,0]     point_upploaded[3] = [0,0,0]     point_dx = 1     flight_loaded = False     indication(0, 1, 0)     lock = True          send_auto = 'c'  def next_targer()     if point_now == 1 :         Timer.callLater(2, ap.push(Ev.MCE_TAKEOFF))      elif point_now == 4 :         Timer.callLater(2, ap.push(Ev.MCE_LANDING))         Timer.callLater(4, rotate_servo_open())         Timer.callLater(6, ap.push(Ev.MCE_PREFLIGHT))         Timer.callLater(8, ap.push(Ev.MCE_TAKEOFF))      elif point_now == 8 :         Timer.callLater(2, ap.push(Ev.MCE_LANDING))         Timer.callLater(4, rotate_servo_close())         Timer.callLater(6, ap.push(Ev.MCE_PREFLIGHT))         Timer.callLater(8, ap.push(Ev.MCE_TAKEOFF))      elif point_now == 12 :         ap.push(Ev.MCE_LANDING)         Timer.callLater(2, rotate_servo_open())         Timer.callLater(4, ap.push(Ev.ENGINES_DISARM))         autopilot = False         auto_pilot_ret = False    # Завершил, а почему 0?       Timer.callLater(14, function()         if auto_pilot_ret and not(point[point_now][1]=='land') :             indication(1, 0, 1)             ap.goToLocalPoint(point[point_now][1], point[point_now][2], point[point_now][3])             indication(0, 0, 1)          point_now = point_now + 1    )  Приём сообщения от автопилота[ def callback(event)        #Вызывается, когда приходят события от автопилота.     if(event == Ev.ENGINES_STARTED) :         ENGINES_STARTED=True    #Двигатели запущены      if(event == Ev.TAKEOFF_COMPLETE) :         TAKEOFF_COMPLETE=True  #Коптер достиг высоты взлета         # Timer.callLater(6, ap.goToLocalPoint(point[3][1], point[3][2], point[3][3]))      if(event == Ev.POINT_REACHED) :         POINT_REACHED=True      #Коптер достиг точки         next_targer()         #  Timer.callLater(6, ap.push(Ev.MCE_LANDING))      if(event == Ev.LOW_VOLTAGE1) :         LOW_VOLTAGE1=True      if(event == Ev.SHOCK) :         SHOCK=True         autopilot = False  def float_cut(x, num)     str = string.sub(tostring(x), 1, string.find(tostring(x), '.') + num)     return str    def main()     rc_chans = table.pack(rc())      if rc_chans[8] < -0.8 :         rotate_servo_open()     elif rc_chans[8] > 0.8 :                 rotate_servo_close()      if not autopilot :         if (rc_chans[1] < -0.8) and (rc_chans[2] < -0.8) and (rc_chans[3] > 0.8) and (rc_chans[4] < -0.8) and not lock :             indication(0,0,4)             indication(0,0,0)             autopilot_point_add()         elif (rc_chans[1] > 0.8) and (rc_chans[2] < -0.8) and (rc_chans[3] > 0.8) and (rc_chans[4] > 0.8) and not lock :             indication(0,0,4)             indication(0,0,0)             autopilot_point_clear()         elif (rc_chans[1] > -0.8) and (rc_chans[2] > -0.8) :             lock = False             send_auto = '0'       if rc_chans[6] <= 0 and not autopilot :         autopilot_off_to_on()     elif rc_chans[6] > 0.8 and autopilot :         autopilot_on_to_off()       posX_now, posY_now, posZ_now = lpsPosition()     inp = servo_stat+','+point_upploaded[3][1]+','+tostring(point_now)+','+Ev.POINT_REACHED+'\n'      uart:write(inp, len(inp))  # записываем          if autopilot and flight_loaded and not auto_pilot_ret :         auto_pilot_ret = True         indication(1, 0, 0)         indication(0, 0, 0)                  ap.push(Ev.MCE_PREFLIGHT)         next_targer()  t = Timer.new(0.08, main) t:start()

После загрузки, вы можете сделать так:

В ответ вы получите ответную индикацию, если запись прошла успешно, то моргнёт красный, если успешно очистили план полёта, то моргнёт зелёный, если ошибка, то индикации не будет.

Более подробная информация о каждой строке вы сможете найти в документации геоскана:

Описание методов API — Документация Pioneer December update 2022 (geoscan.aero)

---

Предыдущие публикации:

• Система автоматической разгрузки и загрузки дрона (Часть 1 — конструкция) / Хабр (habr.com)

Следующая публикация:

• Система автоматической разгрузки и загрузки дрона (Часть 3 — замок) / Хабр (habr.com)