Данная статья включает в себя:
Видео демонстрация
Алгоритм, который мы использовали
Программирование Geoscan Pioneer max (небольшой туториал, будет интересен не всем)
Видео демонстрация
Конструкция из Система автоматической разгрузки и загрузки дрона (Часть 1 — конструкция) / Хабр (habr.com)
Для проверки работы в условиях приближенных к реальным, мы прикрепили наши посадочные места к имитации окна для определения необходимого расстояния от поверхности, к которой закреплено посадочное место, при использование автопилота (ветер ни кто не отменял; дрон должен иметь возможность для экстренного манёвра). Ниже представлено видео после множественных тестовых полётов, которые помогли нам отладить систему и доработать конструкцию посадочного места (задержки перед каждой командой по 16 секунд (для того чтобы контролировать весь полёт), поэтому мы ускорили видео):
Результат тестовых полётов:
Автопилоту сложно контролировать себя при залёте на горизонтальную площадку (решение-сократить площадь поверхности)
Вынести платформу ещё на 15 сантиметров от стены (выравниваясь, очень близко подлетает к вертикальной поверхности, и малейший ветерок прижмёт его к стене/окну)
Алгоритм, который мы использовали
Так как нам требовалось доработать конструкцию в условиях будущего, мы решили разработать алгоритм для автоматического полёта и провести тестирование нашей конструкции для "тупой машины". К счастью нам не понадобилось писать с нуля автопилот, так как у нашего дрона Geoscan Pioneer max, он встроенный, несмотря на это нам всё равно понадобилось разработать алгоритм построения траектории движения, по двум вводимым точкам посадки (третья - стартовая точка).
Алгоритм (упрощённо):
Программирование Geoscan Pioneer max, подключение сервопривода
Для начала мы подключили сервопривод, для этого нам понадобится Rapberry (так как встроенная система не умеет общаться с сервоприводом), подключаем Raspberry по инструкции в интернете и теперь встаёт вопрос куда подключать серво и что делать. Некоторые пины используются по умолчанию (но информацию об этом можно не найти на сайте геоскана) использование таких пинов может повредить сервопривод, поэтому мы рекомендуем использовать, например, пин GPIO25. Ну и конечно Ground и Power.
Необходимые методы API для программирования на Lua
Uart.new(num, rate, parity, stopBits) - создать Uart на порте с настройками.
Параметры: | num – номер UART;rate – скорость;parity – Uart.PARITY_NONE, Uart.PARITY_EVEN, Uart.PARITY_ODD, необязательный параметр, по умолчанию Uart.PARITY_NONE;stopBits – Uart.ONE_STOP, Uart.TWO_STOP, необязательный параметр, по умолчанию Uart.ONE_STOP. |
|---|
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, программирование автоматического полёта в локальной системе координат
Для начала разберёмся с тем, какие команды дрон может принять:
Название | Название |
|---|---|
MCE_PREFLIGHT | Запустить двигатели и провести подготовку |
ENGINES_DISARM | Отключить двигатели |
MCE_LANDING | Отправить на посадку |
MCE_TAKEOFF | Отправить на взлет |
Для того чтобы отправить команду, мы будем использовать метод
ap.push(Event)
Параметры: | Event – номер события или название (например, |
|---|
Но так же дрон нам будет присылать свои выполненные действия в функцию
function callback(event) - Вызывается, когда приходят события от автопилота.
Доступны следующие события, приходящие от автопилота:
Название | Описание |
|---|---|
ENGINES_STARTED | Двигатели запущены |
COPTER_LANDED | Коптер совершил посадку |
TAKEOFF_COMPLETE | Коптер достиг высоты взлета |
POINT_REACHED | Коптер достиг точки |
POINT_DECELERATION | Коптер начал тормозить при подлёте к точке |
LOW_VOLTAGE1 | Низкое напряжение аккумулятора, для возвращения домой |
LOW_VOLTAGE2 | Низкое напряжение аккумулятора, переходит в режим посадки |
SYNC_START | Получен сигнал синхронного старта от системы навигации |
SHOCK | Столкновение или слишком сильные вибрации |
CONTROL_FAIL | Угол наклона коптера превысил допустимый |
ENGINE_FAIL | Отказ двигателя |
Из этого обширного круга событий нам понадобятся:
TAKEOFF_COMPLETE
POINT_REACHED
Ну и только теперь перейдём к методу, который ключевой для написания автопилота:
ap.goToLocalPoint(x, y, z, time) - для полёта с использованием локальной системы координат.
Параметры: | x – задается координата точки по оси x, в метрах;y – задается координата точки по оси y, в метрах;z – задается координата точки по оси 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)
Предыдущие публикации:
Следующая публикация:
