Данная статья включает в себя:
Видео демонстрация
Алгоритм, который мы использовали
Программирование 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)
Предыдущие публикации:
Следующая публикация:
