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

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 - замок)