МІНІСТЕРСТВО ВНУТРІШНІХ СПРАВ УКРАЇНИ

ЛЬВІВСЬКИЙ ДЕРЖАВНИЙ УНІВЕРСИТЕТ ВНУТРІШНІХ СПРАВ

Навчально-науковий інститут управління, психології та безпеки

Кафедра інформаційних технологій

Розроблення апаратного і програмного забезпечення польотного контролера

безпілотного літального апарату

кваліфікаційна робота

здобувача вищої освіти

4 курсу денної форми навчання

Діани РОМАНЮК

Науковий керівник:

доцент, кандидат технічних наук

Олег ЗАЧЕК

Рецензент:

____________________________

вчене звання, науковий ступінь

____________________________

(Ім’я ПРІЗВИЩЕ рецензента)

Кваліфікаційна робота допущена до захисту

«___» _______________ 2025 р., протокол № ____

Завідувач кафедри інформаційних технологій

_________________Ігор ФЕДЧАК_

(підпис)

Львів

2025

Анотація

Романюк Д., Зачек О. (керівник). Розроблення апаратного і програмного забезпечення польотного контролера безпілотного літального апарату. Бакалаврська кваліфікаційна робота. – Львівський державний університет внутрішніх справ, Львів, 2025.

У дипломній роботі розроблено апаратне та програмне забезпечення польотного контролера БпЛА (квадрокоптера). Контролер забезпечує стабілізацію БпЛА в повітрі, автоматичне втримання заданої висоти БпЛА, управління польотом БпЛА: газом (throttle), креном (roll), тангажом (pitch) і рисканням (yaw) відповідно до отриманих команд.

Контролер польоту БпЛА розроблено на платформі Arduino Nano з мікроконтролером ATmega328P, вимірювальному модулі GY-87 (IMU) з 10-ма ступенями свободи, який містить цифровий 3-х осьовий акселерометр і 3-х осьовий гіроскоп на чіпі MPU-6050, цифровий 3-х осьовий магнітометр HMC5883L і цифровий давач атмосферного тиску (барометр) BMP180.

Розроблено схему контролера польоту БпЛА (квадрокоптера). Створено підпрограми калібрування гіроскопа, визначення швидкостей крену, тангажу і рискання з допомогою гіроскопа MPU-6050. Розроблено підпрограму опрацювання PPM-сигналу команд від радіоприймача і генерування PWM-сигналів управління електронними регуляторами швидкості (ESC) двигунів. Розроблено схему захисту від короткого замикання контролера польоту, підпрограму моніторингу напруги, спожитого струму, визначення початкової при старті і залишкової під час польоту ємності акумулятора.

Створено підпрограми калібрування акселерометра MPU-6050 і вимірювання прискорень. Розроблено програмно-алгоритмічне забезпечення отримання абсолютних кутів крену і тангажу методом інтегрування кутових швидкостей виміряних гіроскопом і з використанням методу тригонометрії до значень прискорення виміряних акселерометром. Розроблено і програмно реалізовано фільтр Калмана для прогнозування кута крену і тангажу.

Зібрано макет контролера польоту БпЛА на безпаєчній макетній платі і проведено його тестування. Результати тестування апаратного і програмного забезпечення контролера польоту БпЛА дозволяють в подальшому провести дослідження контролера польоту на фізичній моделі квадрокоптера (БпЛА).

Ключові слова: БпЛА, квадрокоптер, контролер польоту, плата Arduino Nano, МК ATmega328P, GY-87, MPU-6050, HMC5883L, BMP180, C/C++, вбудоване програмне забезпечення, Arduino IDE.

Abstract

Romanyuk D., Zachek O. (supervisor). Development of hardware and software for UAV flight controller. Bachelor’s thesis. – Lviv State University of Internal Affairs, Lviv, 2025.

In the bachelor’s thesis, the hardware and software of the UAV flight controller (quadcopter) are developed. The controller provides air stabilization of the UAV, automatic holding the UAV at a constant altitude, flight control of the UAV (quadcopter): throttle, roll, pitch and yaw according to the received commands.

The UAV (quadcopter) flight controller is developed on the Arduino Nano board with the ATmega328 microcontroller, the GY-87 IMU module with 10 DOF, which contains a digital 3-axis accelerometer and a 3-axis gyroscope on the MPU-6050 chip, digital 3-axis magnetometer HMC5883L and digital atmospheric pressure sensor (barometer) BMP180.

The UAV flight controller is developed on the Arduino Nano platform with an ATmega328P microcontroller, GY-87 sensor module (IMU) with 10 –DOF, which contains a 3-axis accelerometer and 3-axis gyroscope on the MPU-6050 chip, the digital 3-axis HMC5883L magnetometer and BMP180 digital atmospheric pressure sensor (barometer).

The UAV (quadcopter) flight controller circuit is developed. The subroutines for gyroscope calibration, determination of roll, pitch and yaw rates using the MPU-6050 gyroscope are created. The subroutine for processing the PPM signal of commands from the radioreceiver and generating PWM signals for controlling the electronic speed controllers (ESC) of the engines is developed. The short-circuit protection circuit of the flight controller, a subroutine for monitoring the voltage, current consumption, determination of the initial battery capacity at start-up and remaining during the flight are developed.

The subroutines for calibrating the MPU-6050 accelerometer and measuring accelerations are created. Software and algorithm support for obtaining absolute angles of roll and pitch by the method of integration of angular velocities measured by the gyroscope and using the trigonometric method to the acceleration values measured by the accelerometer are developed. The Kalman filter for predicting roll angle and pitch is developed and implemented in software.

The model of the UAV flight controller is assembled on a solderless breadboard and tested. The results of testing the hardware and software of the UAV flight controller make it possible to perform the further research of the flight controller on the physical model of the quadcopter (UAV).

Keywords: UAV, quadcopter, flight controller, Arduino Nano, ATmega328P, IMU GY-87, MPU-6050, HMC5883L, BMP180, C/C++, embedded software, Arduino IDE.

Зміст

Вступ	8

Розділ 1 дослідження та аналіз предметної області	11

1.1. Системи управління польотом БпЛА	11

1.2. Огляд контролерів польоту БпЛА, доступних до купівлі	12

Розділ 2 РОЗРОБКА апаратнОГО забезпечення ПОЛЬОТНОГО контролера ДРОНА	16

2.1. Опис комплектуючих для проєктування польотного контролера дрона	16

2.2. Проєктування апаратного забезпечення польотного контролера дрона	20

Розділ 3 Програмне забезпечення польотного контролера в режимі керування швидкістю	25

3.1. Використання гіроскопа MPU-6050 для визначення поворотів дрона	25

3.2. Калібрування і програмування електронного регулятора швидкості	32

3.3. Розробка схеми і програмного забезпечення моніторингу параметрів акумуляторної батареї	38

3.4. Опис динаміки та перетворення швидкості повороту у дані управління двигунами дрона	43

Розділ 4 Програмне забезпечення польотного контролера в режимі стабілізації польотУ	46

4.1. Визначення абсолютних кутів крену і тангажу	46

4.2. Розробка фільтра Калмана для визначення кутів крену і тангажу	52

ВИСНОВКИ	55

Список використаних літературних джерел	56

Додатки	59

Додаток 1.	60

Додаток 2.	61

Додаток 3.	62

Додаток 4.	64

Додаток 5.	66

Додаток 6.	67

Додаток 7.	68

Додаток 8.	69

Додаток 9.	72

Список умовних скорочень

ПК – персональний комп’ютер.

АЗ – апаратне забезпечення.

МК – мікроконтролер.

ПЗ – програмне забезпечення.

БпЛА – безпілотний літальний апарат.

Arduino Nano – апаратно-програмна платформа на МК AVR ATmega328.

Arduino IDE – інтегроване середовище для написання і прошивки програмного коду в МК плати Arduino.

AVR – сімейство 8 - розрядних мікроконтролерів фірми Atmel.

FLASH – різновид напівпровідникової технології пам’яті, яка перепрограмовується електричним методом.

EEPROM – (англ. Electrically Erasable Programmable Read-Only Memory) – постійна пам’ять, яка стирається і перепрограмовується електричним методом.

ESC – (Electronic Speed Controller) електронний регулятор швидкості

ОЗП (RAM) – оперативна пам’ять або оперативний запам’ятовуючий пристрій (англ. Random Access Memory, RAM).

UART – (Universal Asynchronous Receiver/Transmitter) універсальний асинхронний приймач та передавач, УАПП.

SPI (Serial Peripheral Interface, SPI bus) – послідовний периферійний інтерфейс, шина SPI) – послідовний синхронний повнодуплексний стандарт передачі даних, розроблений фірмою Motorola для забезпечення простого підключення мікроконтролера та периферії.

I2C – послідовна шина даних для зв’язку інтегральних схем, що використовує дві двонаправлені лінії зв’язку (SDA і SCL).

IMU (Inertial Measurement Unit) – інерційний вимірювальний блок/пристрій.

PPM (Pulse Position Modulation) – імпульсно-позиційна модуляція.

PWM (Pulse Width Modulation), ШІМ – широтно-імпульсна модуляція.

Вступ

Квадрокоптери є одним з найбільш корисних винаходів людства і на даний час вони є дуже популярними. Їх можна використовувати в багатьох галузях народного господарства. Широке застосування квадрокоптери знайшли в бойових діях під час російсько-української війни. Багато людей цікавляться придбанням та використанням квадрокоптерів. І є певне число людей, які займаються самостійним створенням квадрокоптерів. Частини, з яких виготовляється квадрокоптер, є доступними у продажі. Актуальним є створення БпЛА для допомоги силам оборони України. Тому тема розробки квадрокоптера актуальна.

Аналіз останніх досліджень і публікацій. Дослідженню розробки контролерів польоту БпЛА з використанням мікроконтролерів та Arduino присвячено чимало публікацій українських та закордонних вчених. Прикладами можуть слугувати роботи: Akarsh Agarwal [1], Montvydas [2], Ткачука А.Г., Безвесільної О.М., Добржанського О.О., Гуменюка А.А., Янчука В.М. [3]. Однак, проблема розробки таких контролерів залишається актуальною і надалі.

Мета роботи – розробити апаратну частину і програмне забезпечення польотного контролера БпЛА. Контролер польоту базуватиметься на платформі Arduino, яка буде підключена до 3-х осьового гіроскопа та 3-х осьового акселерометра. Буде створена програма контролера польоту. Апаратне забезпечення має базуватися на платі Arduino Nano та МК ATmega328.

Для досягнення поставленої мети слід виконати наступні завдання:

Необхідно використати наступні компоненнти апаратного забезпечення:

Плата Arduino Nano на мікроконтролері ATmega328.

Інерційний вимірювальний модуль GY-87, що має десять ступенів свободи та включає в собі наступні давачі – 3-х осьовий гіроскоп і 3-х осьовий акселерометр на мікросхемі MPU-6050, 3-х осьовий магнітометр HMC5883L і барометр BMP180.

Спроектувати апаратне забезпечення контролера польоту БпЛА (коптера) в Proteus ISIS.

Розробити алгоритм калібрування і роботи польотного контролера БпЛА (коптера).

Розробити програмний модуль (бібліотеку) для роботи з акселерометром модуля MPU-6050.

Розробити програмний модуль (бібліотеку) для роботи з гіроскопом модуля MPU-6050.

Розробити програмний модуль (бібліотеку) для роботи з магнітометром HMC5883L модуля MPU-6050.

Розробити програмний модуль (бібліотеку) для роботи з давачем тиску (барометром) BMP180.

Розробити основне програмне забезпечення збору та обробки даних з давачів для МК плати Arduino Nano на мові С в середовищі Arduino IDE.

Провести дослідження макету контролера польоту БпЛА (коптера).

Об’єкт дослідження – процес забезпечення управління безпілотним літальним апаратом (квадрокоптером).

Предмет дослідження – методи і засоби управління польотом безпілотного літального апарату (квадрокоптера).

Методи досліджень. Під час виконання роботи застосовано низку загальновідомих методів досліджень, які дозволили досягти мети роботи та вирішити завдання дослідження. Бібліометричний метод використано під час опрацювання публікацій, що дало змогу побачити динаміку зростання проблеми. Філософські методи пізнання (діалектичний, феноменологічний, синергетичний, аксіологічний) дозволили здійснити аналіз наукових позицій щодо предмета дослідження. Загальнонаукові методи, такі, як індуктивний, дедуктивний, моделювання, синтезу, дали змогу виконати завдання дослідження.

Практичне значення одержаних результатів полягає у:

розробці контролера польоту з дешевих та доступних деталей;

розробленні програми для визначення положення квадрокоптера, ШІМ - управління електронного регулятора швидкості двигунів, перевірки параметрів акумулятора на основі бібліотек, що є у загальному доступі.

Структура роботи. Кваліфікаційна робота містить вступ, чотири розділи, висновки, список використаних джерел, додатки. Обсяг основного тексту роботи складає 49 сторінок, 41 рисунок, 3 таблиць, 9 додатків і 19 бібліографічних джерел. Загальний обсяг роботи – 77 сторінок.

Розділ 1
дослідження та аналіз предметної області

1.1. Системи управління польотом БпЛА

Контролер польоту є мозком БпЛА. Системи управління польотом БпЛА є різноманітними. Це може бути система на основі GPS, яка використовує телеметрію, або проста система стабілізації на звичайному радіоуправлінні.

Спочатку були розроблені радіокеровані літаки та гелікоптери. Система управління гелікоптером, на відміну від літака, має особливість – наявність хвостового гвинта, який використовується для протидії обертанню корпусу гелікоптера під час обертання несучого гвинта. Коли гелікоптер висить нерухомо, все добре. Але коли пілот збільшує тягу, крутний момент несучого гвинта зростає і спричиняє рискання гелікоптера з боку в бік, поки момент хвостового гвинта не скомпенсує момент несучого гвинта.

Для стабілізації гелікоптера в такій ситуації почали використовувати гіроскопи, які намагаються утримати своє положення в просторі під час рискання гелікоптера, створюючи момент нахилу, що виявляється датчиком Холла і призводить до збільшення тяги хвостового гвинта для протидії обертанню корпуса. Також гіроскопи використовували для контролю курсу гелікоптера.

На сьогоднішній день для управління польотом використовують GPS, давачі швидкості повітря, давачі тиску. Також використовують акселерометри для вимірюваня прискорення від різних сил: навантаження у повороті, гальмівної сили, сили тяжіння. Використовуються і гіроскопи для уникнення рискання, виміру крену та тангажу БпЛА.

1.2. Огляд контролерів польоту БпЛА, доступних до купівлі

У мережі Інтернет та різноманітній літературі є досить багато відомостей про контролери польоту БпЛА [4]. На рис. 1.1 – 1.7 зображено деякі контролери польоту БпЛА.

Рис. 1.1 Політний контролер Radiolink Pixhawk.

Політний контролер Radiolink Pixhawk базується на МК STM32F427VIT6 з використанням давачів положення. Він не містить GPS-модуля. Але він містить трьохосьові барометр MEAS MS5611, акселерометри MPU – 6000 та IST8310, магнітометр QMC5883L.

Рис. 1.2 Контролер польоту БПЛА APM 2.8.

Контролер польоту APM 2.8 є наступним поколінням контролера польоту APM 2.6. Цей контролер за допомогою акселерометра, гіроскопа, барометра керує польотом шляхом керування контролерами двигунів та допоміжних приводів. Він використовує контролери ATMEGA2560 та ATMEGA32U-2. Цей контролер має автопілот для польоту по заданим точкам. Також значною перевагою цього контролера є відкритий вихідний код. Але цей контролер польоту не має на борту компаса, щоб він не піддавався впливам електромагнітного поля бортової проводки та електроніки. Первинне налаштування та програмування плати здійснюється за допомогою вільно розповсюджуваного ПЗ Mission Planner. Цей контролер може підключатися до комп’ютера через USB-порт. Розміри 67х41 мм.

Контролер польоту Cube Orange+ містить приймач ADS-B 1090 МГц, який дозволяє одержувати інформацію від пілотованих літальних апаратів, які знаходяться поблизу. Він є придатним для створення складних БпЛА.

Рис. 1.3 Польотний контролер Cube Orange+ фірми RFDesign.

Цей контролер польоту використовує потужний МК STM32H757 на базі ARM Cortex-M7. Він дозволяє керувати дистанційно керованим літальним апаратом будь-якого типу за допомогою ПЗ Cube Yellow. Цей контролер має резервовані датчики акселерометра та гіроскопа, інтерфейси для підключення GPS та компасів, а також посилений захист від статичної електрики та електромагнітних імпульсів.

Рис.1.4 Польотний контролер польоту БпЛА Cube+ Orange

Cube Orange – є новою версією польотного контролера Cube Black для несучої плати Pixhawk 2.1. Має 80-контактний роз’єм інтерфейсів.

Рис. 1.5 Блок-схема польотного контролера БПЛА Cube+ Orange (HS 9014.20.00)

Польотний контролер Mini APM V3.1 базується на платі Ardupilot без вбудованого компаса. Він має давач струму і напруги, компас, GPS та телеметрію. Також він містить дванадцятиканальний інтерфейс. Він важить всього 16 грам та має розміри 35х35 мм.

Рис. 1.6 Польотний контролер Mini APM V3.1.

Польотний контролер Ardupilot APM 2.8 є модернізованим контролером APM 2.6. Він дає змогу літати по заданому маршруту. Також його перевагою є використання відкритого коду. Він дозволяє створити дрон на базі будь-якого транспортного засобу. Він забезпечує передачу інформації пілоту через телеметрію, автоматичний політ по точкам з використанням GPS-координат та стабілізацію в польоті. Також забезпечує аварійне повернення в точку вильоту.

Рис.1.7 Польотний контролер Ardupilot APM 2.8.

Також цей контролер забезпечує політ за оператором на кшталт Hexo+ чи AirDog.

Розділ 2
РОЗРОБКА апаратнОГО забезпечення ПОЛЬОТНОГО контролера ДРОНА

2.1. Опис комплектуючих для проєктування польотного контролера дрона

Для проєктування апаратної частини польотного контролера дрона було вибрано наступні комплектуючі:

Блок GY-87 10-DOF, що містить гіроскоп на мікросхемі MPU-6050, барометр BMP180 та магнітометр HMC5883L.

Плата Arduino Nano з МК ATmega328P-PU.

Плата Arduino Nano з МК ATmega328P-PU(рис. 2.1-2.3) має роз’єми живлення та програмування, кнопку скидання, 14 входів для цифрових сигналів та 6 для аналогових [5].

Рис. 2.1 Плата Arduino Nano

Рис. 2.2 Компоненти плати

Рис. 2.3 Виводи плати

Мікроконтролер ATmega328P

Основою Arduino Nano є МК AVR – Atmega328P з тактовою частотою 16 МГц, який містить флеш-пам’ять ємністю 32 кБ, оперативну пам’ять ємністю 2 кБ і 1 КБ енергонезалежної пам’яті.

Табл.2.1

Індикаторні світлодіоди

ICSP – роз’єм для заливання прошивки в МК та з’єднання з платами розширення. Регулятор напруги LM1117MPX-5.0 з вихідною напругою 5 вольт і струмом 800 мА для живлення МК. Для приєднання USB-порта використовується FTDI FT232R.

Контакти живлення:

VIN: Контакт для підключення джерела живлення (U= 7-12 В).

GND: земляний контакт.

3.3V: Вихідний контакт від стабілізатора напруги мікросхеми FT232R з стабільною напругою 3,3 В і струмом до 50 мА, він не призначений для живлення плати.

5V: Вихідний контакт LM1117MPX-5.0 з стабільною напругою 5 В і струмом 800 мА. Через нього також не можна подавати живлення на плату.

AREF: Контакт опорної напруги аналогово-цифрового перетворювача.

Порти вводу/виводу

Контакти 0-13 – це входи/виходи цифрового сигналу, розраховані на максимальний струм 40 мА, рівень логічної одиниці 5 В, рівень логічного нуля 0 В.

Контакти 3, 5, 6, 9, 10 і 11 для виводу ШІМ-сигналу.

Контакти A0-A7 є вхідними контактами аналогово-цифрового перетворювача, на них не можна подавати більше 5 вольт.

Контакти A4 і A5 призначені для з’єднання з периферійними пристроями.

Контакти 10-13 призначені для з'єднання з периферійними пристроями через SPI.

Контакти 0 та 1 призначені для з'єднання з комп'ютером через послідовний інтерфейс.

Блок GY-87 10-DOF, містить гіроскоп на мікросхемі MPU-6050, барометр BMP180 та магнітометр HMC5883L (рис. 2.4) [6-10]. Його напруга живлення від 3 до 5 вольт. Інтерфейс – I2C.

Рис. 2.4 Блок GY-87

MPU-6050 – це модуль трьохосьових гіроскопа та акселерометра (протокол I2C) (рис. 2.5) [10, 11].

Рис. 2.5 Модуль MPU-6050

2.2. Проєктування апаратного забезпечення польотного контролера дрона

Польотний контролер допомагає підтримувати стабільний політ дрона, регулюючи швидкість двигунів залежно від орієнтації та положення. Він приймає сигнали керування від передавача і подає сигнал на ESC для регулювання швидкості двигунів.

Польотний контролер містить гіроскопи та акселерометри. Також може містити компас та давач тиску.

Рис. 2.6. Блок-схема польотного контролера дрона

Рис. 2.7 Будова комерційного польотного контролера

Польотний контролер збирає дані з давачів та подає на них живлення. Його потрібно розміщати в центрі БпЛА.

Ми будемо застосовувати гіроскоп та акселерометр MPU-6050.

Керувати БпЛА без польотного контролера неможиво. Блок гіроскопа виявляє рух у всіх напрямах та видає результат у º/сек. Це дозволяє розрахувати кут нахилу БпЛА і за допомогою зміни потужності двигунів стабілізувати політ.

Будемо будуваати квадрокоптер. Його потрібно намалювати з обов’язковим позначенням передньої сторони. Тобто є два двигуни спереду і два ззаду (рис. 2.8.).

Рис. 2.8 Схема квадрокоптера

На рис. 2.8 позначено напрям осей Х та Y, який ми прийняли для квадрокоптера. Польотний контролер перевіряє відхилення від осей та дає команди на зміну оборотів котрогось з двигунів для стабілізації польоту. Для адресного керування двигунами ці двигуни пронумеровані відповідно до номерів контактів плати Arduino. Напрям обертання двигунів показано на рисунку.

Тому польотний контролер повинен мати по чотири виводи на кожен двигун і повинен отримувати команди по шести радіоканалам (рис. 2.9).

Рис. 2.9 Польотний контролер з виводами керування.

Польотний контролер живиться від LiPo батареї квадрокоптера по проводам VCC та GND. Напруга поступає на вбудовані стабілізатори 3.3 В та 5 В. Електронний регулятор швидкості ESC кожного двигуна підключається до GND та свого контакта D3, чи D9, чи D10, чи D11. Також потрібно з’єднати контакти SCL та SDA MPU-6050 з відповідними контактами А5 та А4 Arduino Nano. Блок MPU-6050 потрібно розміщати з врахуванням осей Х та Y.

На рис. 2.10 показано повністю підключений польотний контролер. Всі комплектуючі повинні бути з’єднані по земляним виводам і повинні отримувати живлення від акумулятора. Блок MPU-6050 повинен бути розташований по центру дрона.

В додатку 1 показано макет контролера польоту БпЛА (квадрокоптера).

Рис. 2.10 Польотний контролер у зборі.

Розділ 3
Програмне забезпечення польотного контролера в режимі керування швидкістю

3.1. Використання гіроскопа MPU-6050 для визначення поворотів дрона

Гіроскоп блоку орієнтації MPU-6050 виміряє кутові швидкості під час крену, рискання та тагнажу у градусах за секунду (°/с). Вони потрібні для стабілізації дрона у режимі керування швидкістю [10,11]. Щоб квадрокоптер підтримував поточне положення, він повинен мати кутову швидкість 0 °/с.

Крен – це обертання відносно осі Х гіроскопа по годинниковій стрілці. Тангаж – це обертання відносно осі Y по годинниковій стрілці. І рискання – це обертання відносно осі Z проти годинникової стрілки. Ці обертання ми бачимо на рис. 3.1.

Рис. 3.1 Обертання по крену, тангажу та рисканню

Червона вісь на рис. 3.1 є віссю, навколо якої здійснюється обертання дрона. Осі Х та Y позначені на MPU-6050 і під час збирання БпЛА потрібно їх орієнтувати на відповідні осі дрона.

Для програмної реалізації визначення крену, рискання та тангажу потрібно встановити бібліотеку Wire.h в Arduino IDE за допомогою команд Sketch → Include Library →Manage Libraries [12,13].

У програмі задаємо глобальні змінні, які позначають швидкість рискання, тангажу та крену. Як ці змінні виступають результати вимірів гіроскопа, отримані з використанням функції gyro_signals(). I2C-адреса давача блока MPU-6050 становить 0x68, вона використовується для зв’язку функції Wire.beginTransmission з давачем.

Для зв’язку блоку MPU-6050 та мікроконтролера на платі Arduino Nano за протоколом I2C використовуються лінія зв’язку SDA для побітової передачі даних та лінія синхронізації SCL. Щоб підключити блок MPU-6050 до Arduino Nano потрібно підключити SDA до А4, а SCL до А5, також треба підключити GND та подати живлення на Vcc.

Програма для підключення:

Рис. 3.2 З’єднання блока MPU-6050 з Arduino Nano

Також необхідно підключити фільтр низьких частот з використанням регістру за адресою 0x1A, встановивши значення бітів регістра згідно таблиці 3.1.

Використовуючи регістр за адресою 0x1B потрібно встановити коефіцієнт чутливості давача 65,5 LSB/(°/с), встановивши значення бітів регістра згідно таблиці 3.2.

Для отримання результатів вимірювання гіроскопа потрібно звертатися до регістрівв з адресами від 0x43 до 0x48. Запит для отримання даних з шести регістрів давача буде містити шість байт.

Включення фільтра низьких частот [14-16]:

Встановлення масштабного коефіцієнта (коефіцієнт чутливості) [14-16]:

Виконання запиту до регістрів для отримання результатів вимірювання гіроскопа [14-16]:

Для запису швидкості обертання навколо осі Х оголошуємо змінну GyroХ, яка має бути цілим числом без знаку розмірністю 16 біт. Wire.read() потрібно викликати два рази, оскільки результати виміру швидкості обертання навколо осі Х містяться у двох регістрах 0x43 і 0x44 розмірністю по 8 біт.

Для запису швидкості обертання навколо осі Y створюємо такий же код для запису в регістри 0x45 і 0x46. А для осі Z має бути такий же код для запису в регістри 0x47 і 0x48.

Для отримання результатів вимірювання в градусах за секунду ділимо значення в LSB на 65,6 LSB/(°/s) за умови встановлення коефіцієнту чутливості 65,5 LSB/(°/s).

Тактова частота блока MPU-6050 має бути 400 кГц для зв’язку за протокоом I2C. Також, щоб блок запустився, потрібно ввести затримку 250 мс.

Щоб увімкнути живлення MPU-6050 потрібно обнулити регістр за адресою 0x6B.

Щоб вивести швидкості крену, рискання та тангажу на монітор потрібно викликати функцію gyro_signals() у розділі loop() програми [14-16].

У додатку 2 показано програмний код підключення гіроскопа MPU-6050 та отримання даних з нього.

Після запуску програми МК можемо на моніторі помітити, що швидкості крену, тангажу та рискання не є нульовими, хоча MPU-6050 є нерухомим:

Далі вирішимо цю проблему з допомогою калібрування.

Для внесення змін в регістр потрібно застосовувати функцію Wire.write(address) та адресу регістра, яку можна знайти в технічній документації МК. А для читання з регістра потрібно застосовувати функцію Wire.read().

Коли працюють двигуни дрона, вони спричиняють виникнення вібрацій, які впливають на гіроскоп. І навіть якщо дрон ще не стартував, стоїть на землі, дані з гіроскопа постійно змінюються. Це дуже добре видно на рис. 3.3, коли невідфільтровані покази з гіроскопа нарощують коливання, і це призводить до неможливості стабілізації дрона.

Рис. 3.3 Коливання показів гіроскопа внаслідок вібрацій від обертання двигунів.

Щоб відфільтрувати такі вібрації, потрібно застосовувати фільтр, який гасить покази гіроскопа з частотами вище 10 Гц. Це значення було отримане внаслідок досвіду експлуатації дронів.

Для того, щоб покази гіроскопа відповідали реальності, його необхідно калібрувати. Для цього в програмі оголошуємо ще чотири змінні, а саме калібрувальні дані для швидкостей обертання по рискання, тангажу та крену, а також треба ввести змінну для обрахування кількості калібрувальних даних.

Калібрування означає, що треба надати давачу його фізичну точку відліку, щоб результати роботи давача відповідали дійсним значенням.

Кутова швидкість за умови відсутності руху давача повинна бути нульовою. Щоб врахувати вібрації беруть середнє значення результатів вимірювань нерухомого гіроскопа, які нескоректовані, та віднімають це середнє значення від результатів наступних вимірювань [14-16,18].

Беремо у циклі функції setup() значення 2000 результатів вимірів давача через 1 мс кожен, додаємо їх як змінні RollCalibration, PitchCalibration, YawCalibration та RateCalibration. Під час цих вимірів гіроскоп має бути нерухомим. Для отримання середнього значення обраховуємо суму вимірів та ділимо на 2000. Потім ці середні значення віднімаємо від результатів наступних вимірів. Виводимо калібровані значення на монітор.

У додатку 3 наведено програмний код калібрування гіроскопа.

У результаті калібрування значення швидкостей обертання мають дорівнювати практично нулю при нерухомому гіроскопі.

Спробуємо нахиляти чи обертати гіроскоп як на рис. 3.4 і подивимось на результати. Якщо нахилити давач відносно осі Y на 45°, так потримати і повернути назад в початкове положення, то швидкість тангажу має збільшитись, потім стати нульовою, а потім від’ємною.

Рис. 3.4 Калібрування гіроскопа блока MPU-6050

Калібрування потрібно робити під час кожного запуску. При цьому рухати дрон і запускати двигуни не можна. Це займає кілька секунд під час кожного пуску.

3.2. Калібрування і програмування електронного регулятора швидкості

Швидкість двигуна регулюється за допомогою електронного регулятора швидкості з використанням ШІМ. Робота ШІМ полягає у зміні тривалості імпульсів напруги. Чим ширші імпульси, тим більше швидкість обертання двигуна. Скважність імпульсів для більшості регуляторів швидкості становить 4 мс, тобто частота слідування імпульсів становить 250 Гц. Це показано на рис. 3.5 [15].

Рис. 3.5 Управління швидкістю двигуна з допомогою ШІМ

ШІМ дозволяє передавати по одному кабелю сигнали лише з одного радіоканалу, а коли потрібно мати кілька каналів, треба мати стільки сигнальних кабелів. Тут може допомогти імпульсно-позиційна модуляція PPM, коли ширина імпульсів є постійною, а змінюється позиція імпульсу в часі. На рис. 3.6 показано порівняння ШІМ (PWM) та PPM.

Нехай потрібно передавати інформацію про значення газу та крок. Для її передачі потрібні два сигнальні кабелі за умови застосування ШІМ. Обробка кожного з цих сигналів у МК потребує своєї команди і одночасно обробляти обидва сигнали неможливо, це здійснюється по черзі.

Рис. 3.6 Порівняння керування за допомогою ШІМ (PWM) та PPM

Імпульсно-позиційна модуляція дозволяє по одному кабелю декілька сигналів, якщо вони слідують один за одним (рис. 3.7).

Оскільки у приймачі канал 1 позначено як PPM, по ньому можна передавати сигнали на МК з використанням PPM. На рис. 3.8. показано з’єднання приймача з Arduino Nano. Зібравши схему, можна писати код.

Рис. 3.7 Передача кількох сигналів по одному радіоканалу з використанням PPM

Рис. 3.8 Підключення радіомодуля, плати Arduino Nano і ESC - двигуна

Є готова бібліотека PulsePosition.h, її потрібно попередньо оголосити.

У програмі використовуються 2 змінні – масив для збереження 8 значень каналів та ціле число для збереження кількості каналів. Для багаторазового читання даних приймача, потрібно написати функцію, яка перевіряє число доступних каналів, зчитує значення з кожного і зберігає в масиві значень.

Дані по крену передаються по 1 каналу, дані по тангажу передаються п 2 каналу, дані по рисканню передаються по 3 каналу, дані по газу – по 4 каналу. Враховуючи те, що масиви в Arduino IDE нумеруються не з 1, а з 0, каналу 1 відповідає команда ReceiverValue[0]. Також ми вводимо затримку 50 мс для перегляду значень на моніторі [14-17].

В додатку 4 показаний програмний код читання з приймача і відображення на моніторі сигналу управління з використанням імпульсно-позиційної модуляції (PPM).

Щоб побачити інформацію з передавача потрібно підключити плату до ПК через USB, завантажити програму та відкрити монітор. При виключеному передавачі на приймачі буде блимати світлодіод, а значення на моніторі дорівнюватимуть нулю, лише кількість каналів покаже -1. Після ввімкнення передавача світлодіод вже не блимає і команди з передавача відображаються на моніторі. Покрутивши ручки керування, побачимо зміну даних на моніторі. Це свідчить про встановлений зв’язок.

ESC призначений для керування швидкістю двигуна. Підключення елементів схеми показано на рис. 3.9. Потрібно створити програму, яка буде через МК керувати ESC та двигуном [15].

Електронний регулятор швидкості призначений для зміни напруги живлення двигуна з метою регулювання його швидкості. Для регулювання швидкості використовується ШІМ, коли середня напруга, яка подається на двигун, залежить від співвідношення тривалості імпульсів напруги та ділянок без напруги. Схема з’єднання приймача, Arduino Nano і ESC показана на рис. 3.10.

Рис. 3.9 З’єднання приймача, Arduino Nano і ESC

Рис. 3.10 Схема з’єднання приймача, Arduino Nano і ESC

Для тестування потрібно зафіксувати дрон, щоб він не піднімався, зібрати схему. Живлення подаємо тільки тоді, коли завантажимо програму і від'єднаємо USB-шнур від ПК. Треба перевірити чи обертання двигуна 1 відбувається проти годинникової стрілки і якщо це не так, поміняти полярність підключення.

Керування швидкістю обертання двигуна відбувається по 3 каналу. Значення ШІМ змінюються в межах 1000мкс (двигун стоїть) – 2000 мкс (максимальні оберти).

Ось програмний код для налаштування ШІМ-сигналу:

Для більш точного керування швидкістю двигуна потрібно змінити роздільну здатність ШІМ сигналів, які поступають на ESC, з 8 до 12 біт. За умови використання частоти ШІМ 250 Гц потрібно щоб значенню ШІМ 4000 мкс відповідало значення 4095 мкс, для цього обраховуємо коефіцієнт 4095:4000=1.024 і далі всі керуючі сигнали, що ідуть на ESC, домножаємо на нього.

3.3. Розробка схеми і програмного забезпечення моніторингу параметрів акумуляторної батареї

Якщо замкнути акумулятор накоротко, то значний струм, який при цьому виділяється, може завдати багато шкоди. Можуть згоріти кабелі живлення, доріжки плати і навіть може вибухнути сам акумулятор. Тому обов’язково має передбачатися захист акумуляторної батареї від короткого замикання. Також повинен бути вимикач, який дозволяє вимикати живлення двигунів навіть при повному навантаження, а це не просто, оскільки стум може досягати 20 А. Необхідно також виміряти струм споживання для контролю заряду акумуляторної батареї.

Як сильнострумовий вимикач можна застосувати транзистор Infineon BTS50055-1TMB (максимальний струм 70 А) або BTS50080-1TMB (максимальний струм 37,5 А). Такий транзистор дозволяє вимикати живлення дрона при повних обертах двигунів за допомогою низькострумового вимикача, що керує цим транзистором.

Контроль напруги акумулятора не дозволяє перевіряти рівень заряду під час роботи, бо напруга під дією навантаження просідає. Тому потрібно заміряти саме струм споживання. Транзистор Infineon має спеціальний вивід 5 для заміру струму. На цьому виводі є струм у 14000 разів менший за струм, який протікає через силове коло. Для того, щоб Arduino Nano могла виміряти струм, його потрібно перетворити у падіння напруги на резисторі. Вибираємо резистор опором 510 Ом, бо струм 14 А, який протікатиме через нього, викличе падіння напруги 0.1 В. Також потрібно врахувати можливість короткого замикання, при якому струм може досягти 180 А до виключення транзистора. При такому струмі напруга на резисторі досягне 6.5 В. А оскільки напруга з резистора подається на вхід плати Arduino з максимально допустимою вхідною напругою 3.3 В, це може спричинити ушкодження МК. Для уникнення цього паралельно з входом підключаємо стабілітрон на 2.4 В. А для уникнення переполюсування можна послідовно включити діод [18].

Схема включення для контролю і захисту акумуляторної батареї показана на рис. 3.11.

Але навіть вимикач у силовому колі не завжди може запобігти короткому замиканню, тому перед включення живлення потрібно перевірити відсутність коротких замикань у схемі за допомогою тестера.

На рис. 3.12. показано залежність напруги від струму на вимірювальному резисторі у випадку короткого замикання.

Перевірити як виміряється струм на платі Arduino не можна, бо доріжки плати можуть не витримати струмів. Для контролю стану акумулятора створено програму, яка наводиться далі.

Рис. 3.11 Схема підключення для контролю і захисту акумуляторної батареї

Рис. 3.12 Залежність падіння напруги на вимірювальному резисторі від струму

В програмі контролю стану акумулятора потрібно використовувати змінні, які позначають напругу, струм, залишкову ємність батареї, ємність батареї під час запуску, струм споживання в режимі польоту, ємність батареї по замовчуванню. Ми використовуємо акумулятор ємністю 1300 mAh, тому остання змінна використовує саме таке початкове значення.

Ми використовуємо функцію analogRead для зчитування сигналу на вході плати Arduino Nano. Напрузі 0 В відповідає число 0, а максимально допустимій напрузі 3.3В відповідає число 1023 як результат дії функції. При використанні вимірювального резистора 510 Ом кожному амперу через нього відповідає напруга 0.036 В. Отже, потрібно множити отриманий код на коефіцієнт 0.089.

Для контролю ємності акумулятора треба спочатку ввести реальну ємність батареї, а вже потім контролювати її зміну. Ми можемо досить просто і точно заміряти напругу акумулятора на холостому ходу. Тому під час налаштування перед польотом виміряємо напругу і визначаєм заряд акумулятора. Є залежність між напругою на акумуляторі та рівнем заряду, одержана експериментально. Вона наведена на рис. 3.13. Цю залежність можна описати формулою

Рис. 3.13 Залежність між напругою та рівнем заряду акумулятора

Ємність акумулятора розраховуємо, віднімаючи витрату ємності, яку отримуємо на основі спожитого струму, від початкової ємності акумуляторної батареї. Для індикації падіння ємності нижче за 30 % є червоний світлодіод.

Підпрограма для моніторингу акумулятора:

Оскільки транзистор Infineon є досить дефіцитною деталлю, досить часто дрони роблять без нього, підключаючи акумулятор напряму, без вимикача. Але тоді відсутній захист від короткого замикання і включати або виключати живлення можна лише підключаючи чи відключаючи батарею. Крім того ми не зможемо міряти струм і контролювати рівень заряду акумулятора можна буде лише по напрузі. Підпрограма для контролю рівня заряду за напругою є наведена в додатку 5. Індикатором падіння напруги до рівня менше 7.5 В є червоний світлодіод.

3.4. Опис динаміки та перетворення швидкості повороту у дані управління двигунами дрона

Для зміни напряму руху квадрокоптера потрібно змінювати оберти окремих двигунів. Наприклад, якщо хочемо здійснити поворот квадрокоптера навколо осі Х направо (крен направо), потрібно щоб обороти двигунів лівої сторони були вищими за обороти двигунів правої сторони. Тому потрібно подавати сигнал крену, щоб збільшити обороти двигунів зліва і зменшити обороти двигунів справа. Так само можемо змінювати тангаж або рискання. Ілюстрацію цих дій показано на рис. 3.14 [19].

Таким чином можна розрахувати обороти двигунів як комбінацію потрібних рухів:

Рис. 3.14 Вигляд динаміки дрона

Для керування дроном у необхідному напрямі ми змінюжмо обороти всіх чотирьох двигунів. Для висіння в одній позиції всі двигуни мають віддавати 50 % потужності (рис. 3.15). Якщо хочемо щоб дрон летів вище, просто потрібно збільшити обороти двигунів.

Рис. 3.15 Динаміка квадрокоптера

Керування двигунами ми здійснюємо не за процентами потужності, а змінюючи ШІМ від 1000 мкс (0 % потужності) до 2000 мкс (100 % потужності). Але у програмі ми задаємо ШІМ максимально не більше 1800 мкс, щоб був запас потужності для крену, рискання, тангажу.

Для джойстика газу на пульті керування дроном мінімальне положення відповідає 0 % потужності (1000 мкс), а максимальне – 100 % ппотужності (2000 мкс). Положення джойстиків крену, рискання та тангажу за замовчуванням відповідає значенню ШІМ 1500 мкс. Змінюючи положення цих джойстиків, ми або збільшуємо швидкість обертання дрону, або зменшуємо. Більшим значенням відповідає більша маневреність, але при цьому зростає важкість керування БпЛА. Залежність швидкості повороту від значення ШІМ показано на рис. 3.16.

Рис. 3.16. Залежність швидкості повороту від значення ШІМ

Налаштування дій дрона при втраті зв’язку

Мусимо передбачити ситуацію, коли пропаде зв’язок з дроном. Можливим варіантом є подача команди на повернення додому. Але простіше у такому випадку посадити дрон на землю. Для цього потрібно, щоб подалася команда ШІМ 1000 мкс по керуванню газом. При цьому двигуни вимкнуться і дрон впаде.

Для такого налаштування потрібно на пульті радіопередавача дві секунди утримувати натиснутою кнопку ОК, далі System→ RX setup → Failsafe → канал 3→ UP або DOWN і вибираємо ON. Перемістивши джойстик газу в найменше значення натискаємо і тримаємо Cancel. Після повернення до попереднього екрану маємо бачити на каналі 3 100 %.

Налаштувавши дії при втраті зв’язку, можна почати літати. Після увімкнення засвічується червоний світлодіод. Кілька секунд чекаємо проходження калібрування, не рухаючи дрон. Після засвічування зеленого світлодіода трохи збільшуємо газ і маємо почути 4 звукові сигнали від двигунів. Тепер проведемо перевірку дій дрона при пропаданні зв’язку. Для цього збільшуємо обороти до 30 %, щоб дрон залишався на місці. Після цього вимикаємо передавач. Двигуни повинні вимкнутись.

Розділ 4
Програмне забезпечення польотного контролера в режимі стабілізації польотУ

В режимі керування швидкістю не дуже просто літати, бо потрібно постійно контролювати положеня дрона. Але є можливість зробити польотний контролер досконалішим, щоб він автоматично вирівнювався.

4.1. Визначення абсолютних кутів крену і тангажу

Для польоту в режимі стабілізації потрібно знати абсолютні кути крену і тангажу дрона. Є два методи їх вимірювання [14-19].

1. Інтегрування кутових швидкостей виміряних гіроскопом

Перший метод – це інтегрування кутових швидкостей, які ми виміряємо гіроскопом. Наступне рівняння дозволяє зробити це для тангажу:

(4.1.)

(4.2.)

Кут тангажу змінюється при зміні напряму осі Y під час повороту навколо осі Z навіть при відсутньому обертанні відносно осі Y. Також збільшуватиметься кут тангажу, бо напрям осі Х теж змінюється. Тобто навіть при нульових швидкостях обертання по тангажу чи крену, кути по тангажу чи крену змінюються. Це видно на рис. 4.1.

Рис. 4.1 Визначення кутів крену, тангажу і рискання

2. Використання акселерометра блока MPU-6050

MPU-6050 має в своєму складі акселерометр, який виміряє прискорення вздовж осей координат, а не лише гіроскоп. Прискорення сили гравітації становить 9.81 м/с² (1g). Коли MPU-6050 просто лежить, то він показує на виході акселерометра прискорення по осі Z 1g. Прискорення по іншим осям буде нульовим. Повернувши акселерометр так, щоб інша вісь була під кутом 90º до землі, побачимо що прискорення по цій осі буде 1g (рис. 4.2).

Рис. 4.2 Вимірювання прискорення земного тяжіння вздовж осей Z, Y, X

Якщо ж розмістити акселерометр не перпендикулярно до землі по якійсь осі, то прискорення для трьох осей не буде дорівнювати нулю. Це дозволяє вирахувати кути тангажу та крену дрона. Це проілюстровано на рис. 4.3.

Рис. 4.3 Визначення кута крену (roll) з допомогою акселерометра

Рис.4.4 Визначення кута тангажу (pitch) з допомогою акселерометра

Наведемо рівняння які дозволяють вивести рівняня розрахунку кута крену:

(4.3)

(4.4)

Подібним способом на основі рис. 4.4 можна вивести рівняння для розрахунку кута тангажу:

(4.5)

Використовуючи ці рівняння можна розраховувати кути тангажу та крену на основі показів акселерометра. Для тестування MPU-6050 повинен бути підключений до Arduino Nano.

Рис. 4.5 Макет для тестування розрахунку кутів на основі показів акселерометра

Програму для отримання показів акселерометра MPU-6050 можна створити на основі програми для гіроскопа.

Спочатку потрібно описати змінні, які є значеннями прискорення по трьом осям координат та кутами крену та тангажу. Потім потрібно налаштувати зчитування з акселерометра. Програмний код для зчитування показів акселерометра MPU-6050 наведено в додатку 6.

Коли ми запустимо програму для зчитування показів акселерометра MPU-6050, в той момент коли він просто лежить на столі, то на моніторі буде видно, що прискорення по осях Х та Y не дорівнюють нулю, а по осі Z не дорівнює 1g:

Отже знову потріно здійснювати калібрування. Спочатку потрібно вирівняти MPU-6050 таким чином, щоб прискорення по осі Z було 1g. Так само, поставивши вісь Х вертикально, добиваємося значення 1g. Так само робимо для осі Y.

Програмний код для калібрування акселерометра MPU-6050 наведено в додатку 7. Результати калібрування показано на рис. 4.6.

Рис.4.6. Результати калібрування акселерометра

Потім ще раз запускаємо програму для перевірки правильності виміру прискорення. Також потрібно перевірити правильність обчислення кутів тангажу та крену. Коли акселерометр рівно лежить, то ці кути повинні наближатися до 0.

Ми розглянули два методи визначення кутів крену та тангажу. Метод інтегрування кутових швидкостей, які ми виміряємо гіроскопом має більше недоліків.

Під час польоту обчислення кутів методом інтегрування не дає вірних значень кутів через не врахування зміни кутів тангажу та крену при рисканні. Крім того під час інтегрування є накопичення похибки у часі, що добре видно на рис. 4.7. Якщо стартова поверхня нерівна, то інтегрування не дасть точних результатів, бо інтегрування має починатися з нульового значення кута.

Рис.4.7. Результати визначення кутів двома методами

Код для корекції значення акселерометра після калібрування:

Рис.4.8. Виміряні кути крену і тангажу після корекції

4.2. Розробка фільтра Калмана для визначення кутів крену і тангажу

(4.6.)

Використовуючи фільтр Калмана, ми можемо розрахувати точне значення кутів крену та тангажу, поєднавши два методи.

Спочатку розглянемо рівняння, яке перетворює кутову швидкість в кут:

(4.7.)

Нехай цей розрахунок дає не сам кут, а його прогноз. Розрахуємо невизначеність прогнозу кута:

(4.8.)

Далі потрібно визначити коефіцієнт підсилення Калмана. Він порівнює передбачення кута, отримане згідно рівняння 4.7. з кутом, який виміряється акселерометром, для нового прогнозу:

(4.9.)

Коефіцієнт підсилення Калмана розраховується як результат розрахунку в рівнянні 4.8., поділений на невизначеність кута, який виміряється акселерометром:

(4.10.)

Цей коефіцієнт підсилення Калмана розрахований для кута крену або для тангажу. Якщо ж потрібно здійснювати розрахунки для багатьох вимірів, то потрібно використовувати матрично-векторну форму представлення.

Для перевірки фільтра Калмана використовуємо блок MPU-6050, підключений до плати Arduino. Визначаємо кути за допомогою фільтра Калмана. Фільтр Калмана дозволяє врахувати похибку при розрахунку кутів для зльоту з не дуже рівної поверхні на основі значень акселерометра. Виходом фільтра Калмана є прогнозований кут та невизначеність для цього прогнозу.

Програмний код для фільтра Калмана наведено в додатку 8.

Далі пишемо функцію для фільтра Калмана, яка використовує 4 змінні: прогнозований кут; невизначеність прогнозу для кута; кутові швидкості з гіроскопа; кути, які виміряє акселерометр. Ці змінні ми використовуємо для розв’язку розглянутих вище рівнянь.

На рис. 4.9 бачимо порівняння результатів вимірювання кутів акселерометром і кутів на виході фільтра Калмана для дрона, який нерухомо стояв. Як бачимо, результат на виході фільтра Калмана є набагато стабільніший.

Рис. 4.9 Порівняння результатів вимірювання кутів акселерометром і кутів на виході фільтра Калмана

Головний програмний модуль польотного контролера квадрокоптера наведено в додатку 9.

ВИСНОВКИ

В бакалаврській кваліфікаційній роботі розроблені апаратна частина та програмне забезпечення польотного контролера квадрокоптера. Контролер забезпечує стабілізацію дрона в повітрі, зависання на заданій висоті, політ, управляє газом, поворотами по крену, рисканню та тангажу.

Польотний контролер квадрокоптера базується на платі Arduino Nano з МК ATmega328P, він використовує IMU GY-87, у складі якого є акселерометр та гіроскоп у блоці MPU-6050, датчик тиску BMP180 та компас HMC5883L.

Розроблено схему польотного контролера дрона. Написано програмний код для калібрування гіроскопа, для визначення швидкості крену, швидкості тангажу та швидкості рискання з використанням блока MPU-6050. Написано програмний код для опрацювання сигналу з імпульсно-позиційною модуляцією від приймача та створення сигналів з широтно-імпульсною модуляцією для управління електронними регуляторами швидкості двигунів. Розроблено схему для захисту польотного контролера від короткого замикання. Написано програмний код для контролю напруги на акумуляторі, струму споживання та рівня заряду акумулятора перед стартом та під час поьоту. Також написано програмний код для калібрування акселерометра та вимірювання прискорень по осям координат. Розглянуто два методи для визначення абсолютних кутів тангажу та крену, показано сильні та слабкі сторони кожного методу. Розроблено програму для розрахунку абсолютних кутів тангажу та крену за методом інтегрування кутових швидкостей, які ми виміряємо гіроскопом, та за методом тригонометричного обчислення прискорень, виміряних акселерометром. Розглянуто фільтр Калмана та програму, яка його реалізує, для прогнозування кутів тангажу та крену. В майбутньому можна використати результати роботи для дослідження фізичної моделі дрона.

Список використаних літературних джерел

Akarsh Agarwal. Flight Controller Tutorial | Arduino Based Quadcopter Drone. URL: https://www.hackster.io/akarsh98/flight-controller-tutorial-arduino-based-quadcopter-drone-8d752e

Montvydas. Arduino Nano Quadcopter. URL: https://www.instructables.com/Arduino-micro-Quadcopter/

Ткачук А.Г., Безвесільна О.М., Добржанський О.О., Гуменюк А.А., Янчук В.М. Налаштування польотного контролера та системи стабілізації безпілотного літального апарата із системою моніторингу наявності пожеж та витоків теплової енергії на борту. URL: https://eztuir.ztu.edu.ua/jspui/bitstream/123456789/8117/1/8.%20%D0%A2%D0%BA%D0%B0%D1%87%D1%83%D0%BA.pdf

Польотні контролери. URL: https://arduino.ua/prod4624-poletnii-kontroller-apm-2-8-v-korpyse-s-bokovimi-razemami?srsltid=AfmBOoq3KjdmNSKY3S6CFGA9T52GE7FWYaDL0eaR2hR-XAO0Utc47zRx, https://www.amazon.com/Drone-Flight-Controller/s?k=­Drone+­Flight+Controller.

Електронний ресурс по Arduino Nano. URL: https://docs.arduino.cc/hardware/nano, https://store.arduino.cc/products/arduino-nano.

Електронний ресурс з datasheet на модуль GY-87. URL: https://5.imimg.com/data5/LF/FE/MY-1833510/gy-87-10dof-mpu6050-hmc5883l-bmp180-sensor-module.pdf.

Електронний ресурс з технічною документацією на модуль 6DOF гіроскоп+акселерометр MPU-6050. URL: https://datasheetspdf.­com/pdf-file/735134/InvenSense/MPU6050/1, https://github.com/ElectronicCats/­CatSat/blob/master/PDF's/4-%20Datasheet%20%20GY-87.pdf.

Електронний ресурс з datasheet на 3-х осьовий цифровий компас Honeywell HMC5883L. URL: https://github.com/ElectronicCats/CatSat­/blob/master/PDF's/5-%20­Datasheet%20%20GY-87%20HMC5883L.pdf, https://www.alldatasheet.com/datasheet-pdf/pdf/428790/HONEYWELL­/HMC5883L.html, https://www.farnell.com/datasheets/1683374.pdf.

Електронний ресурс з datasheet на цифровий барометр BMP180. URL: https://www.alldatasheet.com/datasheet-pdf/pdf/1132068/­BOSCH/­BMP180.html, https://cdn-shop.adafruit.com/­datasheets/BST-BMP180-DS000-09.pdf, https://www.digikey.com/html­data­sheets/production/­856385/­0/0/1/­bmp180-datasheet.html.

Елекронний ресурс по підключенню MPU-6050 до Arduino. URL: https://robu.in/mpu6050-with-arduino/, https://howtomechatronics.com­/tutorials/­arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/, https://www.hackster.io/shilleh/how-to-connect-mpu6050-to-arduino-uno-301814, https://www.instructables.com/MPU-6050-Tutorial-How-to-Program-MPU-6050-With-Ard/, https://lastminuteengineers.com/mpu6050-accel-gyro-arduino-tutorial/#google_vignette.

Jonathan Oxer and Hugh Blemings. Practical Arduino: Cool Projects for Open Source Hardware. - Springer-Verlag New York , 2009 - P. 445.

Emily Gertz & Patrick Di Justo. Environmental Monitoring with Arduino. Building Simple Devices to Collect Data About the World Around Us. – Published by O’Really Media Inc., 2012 – p. 96.

Jack Purdum. Beginning C for Arduino, Second Edition: Learn C Programming for the Arduino, Ecosoft, Inc. 2015, p. 381.

Massimo Banzi, Michael Shiloh. Getting Started with Arduino, 3rd Edition. Published by Maker Media, Inc., 2015, p. 262.

Ryan Turner. Arduino Programming: The Ultimate Intermediate Guide to Learn Arduino Programming Step by Step. Publisher: nelly B.L. International Consulting LTD. (December 5, 2019), p. 164.

Електронні ресурси по Arduino. URL: https://www.arduino.cc/, https://www.arduino.cc/en/Tutorial/HomePage, https://www.tutorialspoint.­com/arduino/, https://learn.adafruit.com/­category/learn-arduino, https://www.­hackster.io/­arduino/projects https://playground.arduino.cc/Projects/Ideas.

Середовище розробки Arduino IDE. URL: https://www.arduino.cc/en/Main/Software.

Arduino Drone. URL: https://projecthub.arduino.cc/robocircuits/arduino-drone-9a7c7c

CarbonAeronautics/Manual-Quadcopter-Drone. URL: https://github.com/CarbonAeronautics/Manual-Quadcopter-Drone

Додатки

Додаток 1.

Макет контролера польоту БпЛА (квадрокоптера)

Додаток 2.

Програмний код підключення гіроскопа MPU-6050 та отримання даних з нього.

int16_t GyroX=Wire.read()<<8 | Wire.read(); // читаємо вимірювання гіроскопа навколо осі X

int16_t GyroY=Wire.read()<<8 | Wire.read();

int16_t GyroZ=Wire.read()<<8 | Wire.read();

RateRoll=(float)GyroX/65.5; // перетворення вимірювань у значення °/с

RatePitch=(float)GyroY/65.5;

RateYaw=(float)GyroZ/65.5;

}

void setup() {

Serial.begin(57600);

pinMode(13, OUTPUT);

digitalWrite(13, HIGH);

Wire.setClock(400000); // вст. швидкості комунікації I2C

Wire.begin();

delay(250);

Wire.beginTransmission(0x68); // Start the gyro in pow­er mode

Wire.write(0x6B);

Wire.write(0x00);

Wire.endTransmission();

}

void loop() {

gyro_signals(); // виклик функції для зчитування вимірювань гіроскопа

Serial.print("Roll rate [°/s]= ");

Serial.print(RateRoll);

Serial.print(" Pitch Rate [°/s]= ");

Serial.print(RatePitch);

Додаток 3.

Програмний код калібрування гіроскопа MPU-6050

#include <Wire.h>

float RateRoll, RatePitch, RateYaw;

float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw; // Оголошення змінних калібрування

int RateCalibrationNumber;

void gyro_signals(void) {

Wire.beginTransmission(0x68);

Wire.write(0x1A);

Wire.write(0x05);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x1B);

Wire.write(0x08);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x43);

Wire.endTransmission();

Wire.requestFrom(0x68,6);

int16_t GyroX=Wire.read()<<8 | Wire.read();

int16_t GyroY=Wire.read()<<8 | Wire.read();

int16_t GyroZ=Wire.read()<<8 | Wire.read();

RateRoll=(float)GyroX/65.5;

RatePitch=(float)GyroY/65.5;

RateYaw=(float)GyroZ/65.5;

}

void setup() {

Serial.begin(57600);

pinMode(13, OUTPUT);

digitalWrite(13, HIGH);

Wire.setClock(400000);

Wire.begin();

delay(250);

Wire.beginTransmission(0x68);

Wire.write(0x6B);

Wire.write(0x00);

Wire.endTransmission();

for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) { // виконуємо калібрувальні вимірювання

gyro_signals();

RateCalibrationRoll+=RateRoll;

RateCalibrationPitch+=RatePitch;

RateCalibrationYaw+=RateYaw;

delay(1);

}

RateCalibrationRoll/=2000; // обчислення калібрувальних значень

RateCalibrationPitch/=2000;

RateCalibrationYaw/=2000;

}

void loop() { // коректування виміряних значень

gyro_signals();

RateRoll-=RateCalibrationRoll;

RatePitch-=RateCalibrationPitch;

RateYaw-=RateCalibrationYaw;

Serial.print("Roll rate [°/s]= ");

Serial.print(RateRoll);

Serial.print(" Pitch Rate [°/s]= ");

Serial.print(RatePitch);

Serial.print(" Yaw Rate [°/s]= ");

Serial.println(RateYaw);

delay(50);

}

Додаток 4.

Програмний код читання з приймача і відображення на моніторі сигналу управління з використанням імпульсно-позиційної модуляції (PPM).

#include <PulsePosition.h>

PulsePositionInput ReceiverInput(RISING); // Use the PulsePosition library

float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0}; // Declare the variables to store channel info

int ChannelNumber=0;

void read_receiver(void){ // Define a function to read the receiver data

ChannelNumber = ReceiverInput.available();

if (ChannelNumber > 0) {

for (int i=1; i<=ChannelNumber;i++){

ReceiverValue[i-1]=ReceiverInput.read(i);

}

}

}

void setup() {

Serial.begin(57600);

pinMode(13, OUTPUT);

digitalWrite(13, HIGH);

ReceiverInput.begin(14);

}

void loop() { // Read and display the PPM information on the serial monitor

read_receiver();

Serial.print("Number of channels: ");

Serial.print(ChannelNumber);

Serial.print(" Roll [μs]: ");

Serial.print(ReceiverValue[0]);

Serial.print(" Pitch [μs]: ");

Serial.print(ReceiverValue[1]);

Serial.print(" Throttle [μs]: ");

Serial.print(ReceiverValue[2]);

Serial.print(" Yaw [μs]: ");

Serial.println(ReceiverValue[3]);

delay(50);

}

Додаток 5.

Програмний код контролю залишку енергії акумулятора

float Voltage;

void battery_voltage(void) {

Voltage=(float)analogRead(15)/62;

}

void setup() {

if (Voltage > 7.5) digitalWrite(5, LOW);

}

void loop() {

battery_voltage();

if (Voltage < 7.5) digitalWrite(5, HIGH);

else digitalWrite(5, LOW);

}

void loop() { // обчислення ємності акумулятора під час польоту

battery_voltage();

CurrentConsumed=Current*1000*0.004/3600+

CurrentConsumed;

BatteryRemaining=(BatteryAtStart-

CurrentConsumed)/BatteryDefault*100;

if (BatteryRemaining<=30) digitalWrite(5, HIGH);

else digitalWrite(5, LOW);

}

Додаток 6.

Програмний код для зчитування показів акселерометра MPU-6050.

#include <Wire.h>

float RateRoll, RatePitch, RateYaw;

// Define the accelerometer variables

float AccX, AccY, AccZ;

float AngleRoll, AnglePitch;

float LoopTimer;

void gyro_signals(void) {

// Включення низькочастотного фільтра (низьких частот)

Wire.beginTransmission(0x68);

Wire.write(0x1A);

Wire.write(0x05);

Wire.endTransmission();

// Налаштування виходу акселерометра

Wire.beginTransmission(0x68);

Wire.write(0x1C);

Wire.write(0x10);

Wire.endTransmission();

// Зчитуємо значення вимірювань з акселерометра

Wire.beginTransmission(0x68);

Wire.write(0x3B);

Wire.endTransmission();

Wire.requestFrom(0x68,6);

int16_t AccXLSB = Wire.read() << 8 | Wire.read();

int16_t AccYLSB = Wire.read() << 8 | Wire.read();

int16_t AccZLSB = Wire.read() << 8 |

Wire.read();

// Налаштовуємо вихід гіроскопа та отримуємо вимірювання кутових швидкостей

Wire.beginTransmission(0x68);

Wire.write(0x1B);

Wire.write(0x8);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x43);

Wire.endTransmission();

Додаток 7.

Програмний код для калібрування акселерометра MPU-6050.

Wire.requestFrom(0x68,6);

int16_t GyroX=Wire.read()<<8 | Wire.read();

int16_t GyroY=Wire.read()<<8 | Wire.read();

int16_t GyroZ=Wire.read()<<8 | Wire.read();

RateRoll=(float)GyroX/65.5;

RatePitch=(float)GyroY/65.5;

RateYaw=(float)GyroZ/65.5;

// Перетворюємо вимірювання у фізичні значення

AccX=(float)AccXLSB/4096;

AccY=(float)AccYLSB/4096;

AccZ=(float)AccZLSB/4096;

// Обчислюємо абсолютні кути

AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ* AccZ))*1/(3.142/180);

AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ* AccZ))*1/(3.142/180);

}

// Комунікація з гіроскопом і калібрування

void setup() {

Serial.begin(57600);

pinMode(13, OUTPUT);

digitalWrite(13, HIGH);

Wire.setClock(400000);

Wire.begin();

delay(250);

Wire.beginTransmission(0x68);

Wire.write(0x6B);

Wire.write(0x00);

Wire.endTransmission();

}

// Вивід значень акселерометра

void loop() {

gyro_signals();

Serial.print("Acceleration X [g]= ");

Serial.print(AccX);

Serial.print(" Acceleration Y [g]= ");

Serial.print(AccY);

Serial.print(" Acceleration Z [g]= ");

Serial.println(AccZ);

delay(50);

}

Додаток 8.

Програмний код для фільтра Калмана.

#include <Wire.h>

// Оголошення змінних гіроскопа і акселерометра

float RateRoll, RatePitch, RateYaw;

float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;

int RateCalibrationNumber;

float AccX, AccY, AccZ;

float AngleRoll, AnglePitch;

uint32_t LoopTimer;

// Визначаємо прогнозовані кути та невизначеності

float KalmanAngleRoll=0,

KalmanUncertaintyAngleRoll=2*2;

float KalmanAnglePitch=0,

KalmanUncertaintyAnglePitch=2*2;

// Ініціалізуємо вихід фільтра

float Kalman1DOutput[]={0,0};

// Створюємо функцію, яка обчислює прогнозований кут і невизначеність за допомогою рівнянь Калмана

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {

KalmanState=KalmanState+0.004*KalmanInput;

KalmanUncertainty=KalmanUncertainty + 0.004 * 0.004 * 4 * 4;

float KalmanGain=KalmanUncertainty * 1/ (1*KalmanUncertainty + 3 * 3);

KalmanState=KalmanState+KalmanGain * ( KalmanMeasurement-KalmanState);

KalmanUncertainty=(1-KalmanGain) * KalmanUncertainty;

// Вихід фільтра Калмана

Kalman1DOutput[0]=KalmanState;

Kalman1DOutput[1]=KalmanUncertainty;

}

// Зчитуємо кутові швидкості (швидкості обертання), прискорення та кути з MPU-6050

void gyro_signals(void) {

Wire.beginTransmission(0x68);

Wire.write(0x1A);

Wire.write(0x05);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x1C);

Wire.write(0x10);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x3B);

Wire.endTransmission();

Wire.requestFrom(0x68,6);

int16_t AccXLSB = Wire.read() << 8 |

Wire.read();

int16_t AccYLSB = Wire.read() << 8 | Wire.read();

int16_t AccZLSB = Wire.read() << 8 | Wire.read();

Wire.beginTransmission(0x68);

Wire.write(0x1B);

Wire.write(0x8);

Wire.endTransmission();

// Вихідні дані функції фільтра Калмана складаються з передбачення стану (кута) та відповідної невизначеності

Wire.beginTransmission(0x68);

Wire.write(0x43);

Wire.endTransmission();

Wire.requestFrom(0x68,6);

int16_t GyroX=Wire.read()<<8 | Wire.read();

int16_t GyroY=Wire.read()<<8 | Wire.read();

int16_t GyroZ=Wire.read()<<8 | Wire.read();

RateRoll=(float)GyroX/65.5;

RatePitch=(float)GyroY/65.5;

RateYaw=(float)GyroZ/65.5;

// Сюди поміщаємо власні значення калібрування акселерометра

AccX=(float)AccXLSB/4096-0.04;

AccY=(float)AccYLSB/4096+0.03;

AccZ=(float)AccZLSB/4096+0.01;

AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ* AccZ))*1/(3.142/180);

AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ* AccZ))*1/(3.142/180);

}

void setup() {

Serial.begin(57600);

pinMode(13, OUTPUT);

digitalWrite(13, HIGH);

Wire.setClock(400000);

Wire.begin();

delay(250);

// Комунікація з гіроскопом і калібрування

Wire.beginTransmission(0x68);

Wire.write(0x6B);

Wire.write(0x00);

Wire.endTransmission();

for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) {

gyro_signals();

RateCalibrationRoll+=RateRoll; RateCalibrationPitch+=RatePitch;

// Коли кутова швидкість (швидкість обертання) від гіроскопа та кути від акселерометра виміряні, запускаємо ітерацію для фільтра Калмана

RateCalibrationYaw+=RateYaw;

delay(1);

}

RateCalibrationRoll/=2000;

RateCalibrationPitch/=2000;

RateCalibrationYaw/=2000;

LoopTimer=micros();

}

void loop() {

gyro_signals();

// Обчислюємо (визначаємо) значення кутових швидкостей (швидкостей обертання)

RateRoll-=RateCalibrationRoll;

RatePitch-=RateCalibrationPitch;

RateYaw-=RateCalibrationYaw;

kalman_1d(KalmanAngleRoll,

KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);

KalmanAngleRoll=Kalman1DOutput[0];

KalmanUncertaintyAngleRoll=Kalman1DOutput[1];

kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);

KalmanAnglePitch=Kalman1DOutput[0];

KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

Serial.print("Roll Angle [°] ");

Serial.print(KalmanAngleRoll);

Serial.print("Pitch Angle [°] ");

Serial.println(KalmanAnglePitch);

while (micros() - LoopTimer < 4000);

LoopTimer=micros();

}

Додаток 9.

Головний програмний модуль контролера польоту безпілотного літального апарату (квадрокоптера)

/* Контролер польоту безпілотного літального апарату (квадрокоптера)

UAV (quadcopter) flight controller

Розробила: студентка групи ІТ-41, Діана Романюк

*/

#include <Wire.h>

// Ініціалізуємо ті самі змінні, які вже потрібні для режиму швидкості

float RateRoll, RatePitch, RateYaw;

float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;

int RateCalibrationNumber;

#include <PulsePosition.h>

PulsePositionInput ReceiverInput(RISING);

float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0};

int ChannelNumber=0;

float Voltage, Current, BatteryRemaining, BatteryAtStart;

float CurrentConsumed=0;

float BatteryDefault=1300;

uint32_t LoopTimer;

float DesiredRateRoll, DesiredRatePitch, DesiredRateYaw;

float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;

float InputRoll, InputThrottle, InputPitch, InputYaw;

float PrevErrorRateRoll, PrevErrorRatePitch, PrevErrorRateYaw;

float PrevItermRateRoll, PrevItermRatePitch, PrevItermRateYaw;

float PIDReturn[]={0, 0, 0};

float PRateRoll=0.6; float PRatePitch=PRateRoll;

float PRateYaw=2;

float IRateRoll=3.5; float IRatePitch=IRateRoll;

float IRateYaw=12;

float DRateRoll=0.03; float DRatePitch=DRateRoll; float DRateYaw=0;

float MotorInput1, MotorInput2, MotorInput3, MotorInput4;

// Ініціалізуємо змінні акселерометра

float AccX, AccY, AccZ;

float AngleRoll, AnglePitch;

// Визначаємо бажані кути крену і тангажу та відповідні похибки для PDI – контролера зовнішнього контуру

float DesiredAngleRoll, DesiredAnglePitch;

float ErrorAngleRoll, ErrorAnglePitch;

// Визначаємо значень, необхідних для PID – контролера зовнішнього контуру, включаючи параметри P, I та D

float PrevErrorAngleRoll, PrevErrorAnglePitch;

float PrevItermAngleRoll, PrevItermAnglePitch;

float PAngleRoll=2; float PAnglePitch=PAngleRoll;

float IAngleRoll=0; float IAnglePitch=IAngleRoll;

float DAngleRoll=0; float DAnglePitch=DAngleRoll;

// Функція фільтра Калмана

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {

KalmanState=KalmanState+0.004*KalmanInput;

KalmanUncertainty=KalmanUncertainty + 0.004 * 0.004 * 4 * 4;

float KalmanGain=KalmanUncertainty * 1/ (1*KalmanUncertainty + 3 * 3);

KalmanState=KalmanState+KalmanGain * ( KalmanMeasurement-KalmanState);

KalmanUncertainty=(1-KalmanGain) * KalmanUncertainty;

Kalman1DOutput[0]=KalmanState;

Kalman1DOutput[1]=KalmanUncertainty;

}

// Функція напруги акумулятора

void battery_voltage(void) {

Voltage=(float)analogRead(15)/62;

Current=(float)analogRead(21)*0.089;

}

// Функція приймача

void read_receiver(void){

ChannelNumber = ReceiverInput.available();

if (ChannelNumber > 0) {

for (int i=1; i<=ChannelNumber;i++){

ReceiverValue[i-1]=ReceiverInput.read(i);

}

}

}

// Функція гіроскопа та акселерометра

void gyro_signals(void) {

Wire.beginTransmission(0x68);

Wire.write(0x1A);

Wire.write(0x05);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x1C);

Wire.write(0x10);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x3B);

Wire.endTransmission();

Wire.requestFrom(0x68,6);

int16_t AccXLSB = Wire.read() << 8 | Wire.read();

int16_t AccYLSB = Wire.read() << 8 | Wire.read();

int16_t AccZLSB = Wire.read() << 8 | Wire.read();

Wire.beginTransmission(0x68);

Wire.write(0x1B);

Wire.write(0x8);

Wire.endTransmission();

Wire.beginTransmission(0x68);

Wire.write(0x43);

Wire.endTransmission();

Wire.requestFrom(0x68,6);

int16_t GyroX=Wire.read()<<8 | Wire.read();

int16_t GyroY=Wire.read()<<8 | Wire.read();

int16_t GyroZ=Wire.read()<<8 | Wire.read();

RateRoll=(float)GyroX/65.5;

RatePitch=(float)GyroY/65.5;

RateYaw=(float)GyroZ/65.5;

// Тут потрібно розмістити власні значения калібрування акселерометра

AccX=(float)AccXLSB/4096-0.05;

AccY=(float)AccYLSB/4096+0.01;

AccZ=(float)AccZLSB/4096-0.11;

AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ* AccZ))*1/(3.142/180);

AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180);

}

// Функція PID - контролера

void pid_equation(float Error, float P , float I, float D, float PrevError, float PrevIterm) {

float Pterm=P*Error;

float Iterm=PrevIterm+I*(Error+PrevError)*0.004/2;

if (Iterm > 400) Iterm=400;

else if (Iterm <-400) Iterm=-400;

float Dterm=D*(Error-PrevError)/0.004;

float PIDOutput= Pterm+Iterm+Dterm;

if (PIDOutput>400) PIDOutput=400;

else if (PIDOutput <-400) PIDOutput=-400;

PIDReturn[0]=PIDOutput;

PIDReturn[1]=Error;

PIDReturn[2]=Iterm;

}

// Функція скидання PID - контролера

void reset_pid(void) {

PrevErrorRateRoll=0; PrevErrorRatePitch=0;

PrevErrorRateYaw=0;

PrevItermRateRoll=0;

PrevItermRatePitch=0;

PrevItermRateYaw=0;

// Скидання похибки PID - контролера та інтегральні значення для зовнішнього контуру PID - контролера

PrevErrorAngleRoll=0; PrevErrorAnglePitch=0;

PrevItermAngleRoll=0; PrevItermAnglePitch=0;

}

// Візуалізація фази налаштування за допомогою червоного світлодіода

void setup() {

pinMode(5, OUTPUT);

digitalWrite(5, HIGH);

pinMode(13, OUTPUT);

digitalWrite(13, HIGH);

// Communication with the gyroscope and calibration (project 4 and 5)

// Комунікація з гіроскопом і його калібрування

Wire.setClock(400000);

Wire.begin();

delay(250);

Wire.beginTransmission(0x68);

Wire.write(0x6B);

Wire.write(0x00);

Wire.endTransmission();

//

for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) {

gyro_signals();

RateCalibrationRoll+=RateRoll;

RateCalibrationPitch+=RatePitch;

RateCalibrationYaw+=RateYaw;

delay(1);

}

RateCalibrationRoll/=2000;

RateCalibrationPitch/=2000;

RateCalibrationYaw/=2000;

// Встановлення PWM - частоти 250 Гц і роздільної здатності 12 біт для всіх двигунів

analogWriteFrequency(1, 250);

analogWriteFrequency(2, 250);

analogWriteFrequency(3, 250);

analogWriteFrequency(4, 250);

analogWriteResolution(12);

// Показати закінчення процесу налаштування та визначити відсоток початкової напруги акумулятора

pinMode(6, OUTPUT);

digitalWrite(6, HIGH);

battery_voltage();

if (Voltage > 8.3) { digitalWrite(5, LOW);

BatteryAtStart=BatteryDefault; }

else if (Voltage < 7.5) {

BatteryAtStart=30/100*BatteryDefault ;}

else { digitalWrite(5, LOW);

BatteryAtStart=(82*Voltage-580)/100* BatteryDefault; }

// Рядки коду повязані з безпекою квадрокоптера: запобігання випадковому (взльоту) підйому квадрокоптера після процесу налаштування

ReceiverInput.begin(14);

while (ReceiverValue[2] < 1020 || ReceiverValue[2] > 1050) {

read_receiver();

delay(4);

}

LoopTimer=micros();

}

// Вимірювання кутових швидкостей (швидкостей обертання) та віднімання калібрувальних значень

void loop() {

gyro_signals();

RateRoll-=RateCalibrationRoll;

RatePitch-=RateCalibrationPitch;

RateYaw-=RateCalibrationYaw;

// Обчислення кутів крену (roll angle) і тангажу (pitch angle) через фільтр Калмана

kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);

KalmanAngleRoll=Kalman1DOutput[0];

KalmanUncertaintyAngleRoll=Kalman1DOutput[1];

kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);

KalmanAnglePitch=Kalman1DOutput[0];

KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

read_receiver();

// Обчислення бажаних кутів зі значень отриманих від приймача

DesiredAngleRoll=0.10*(ReceiverValue[0]-1500);

DesiredAnglePitch=0.10*(ReceiverValue[1]-1500);

InputThrottle=ReceiverValue[2];

DesiredRateYaw=0.15*(ReceiverValue[3]-1500);

// Обчислення різниці між бажаним і фактичним кутами крену та тангажу

ErrorAngleRoll=DesiredAngleRoll - KalmanAngleRoll;

ErrorAnglePitch=DesiredAnglePitch - KalmanAnglePitch;

// Обчислення бажаних кутів крену (roll angle) і тангажу (pitch angle) з допомогою PID – контролера зовнішнього контуру

pid_equation(ErrorAngleRoll, PAngleRoll, IAngleRoll, DAngleRoll, PrevErrorAngleRoll, PrevItermAngleRoll);

DesiredRateRoll=PIDReturn[0];

PrevErrorAngleRoll=PIDReturn[1];

PrevItermAngleRoll=PIDReturn[2];

pid_equation(ErrorAnglePitch, PAnglePitch, IAnglePitch, DAnglePitch, PrevErrorAnglePitch, PrevItermAnglePitch);

DesiredRatePitch=PIDReturn[0];

PrevErrorAnglePitch=PIDReturn[1];

PrevItermAnglePitch=PIDReturn[2];

// Обчислюємо різницю між бажаними і фактичними значеннями кутових швидкосей крену (roll), тангажу (pitch) і рискання (yaw). Ці значення використовуються для PID – контролера внутрішнього контуру

ErrorRateRoll=DesiredRateRoll-RateRoll;

ErrorRatePitch=DesiredRatePitch-RatePitch;

ErrorRateYaw=DesiredRateYaw-RateYaw;

pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, DRateRoll, PrevErrorRateRoll, PrevItermRateRoll);

InputRoll=PIDReturn[0];

PrevErrorRateRoll=PIDReturn[1];

PrevItermRateRoll=PIDReturn[2];

pid_equation(ErrorRatePitch, PRatePitch, IRatePitch, DRatePitch, PrevErrorRatePitch, PrevItermRatePitch);

InputPitch=PIDReturn[0];

PrevErrorRatePitch=PIDReturn[1];

PrevItermRatePitch=PIDReturn[2];

pid_equation(ErrorRateYaw, PRateYaw, IRateYaw, DRateYaw, PrevErrorRateYaw, PrevItermRateYaw);

InputYaw=PIDReturn[0];

PrevErrorRateYaw=PIDReturn[1];

PrevItermRateYaw=PIDReturn[2];

// Обмеження значення газу до 80%

if (InputThrottle > 1800) InputThrottle = 1800;

// Використання рівнянь динаміки квадрокоптера

MotorInput1= 1.024*(InputThrottle - InputRoll – InputPitch - InputYaw);

MotorInput2= 1.024*(InputThrottle-InputRoll +InputPitch+InputYaw);

MotorInput3= 1.024*(InputThrottle+InputRoll + InputPitch - InputYaw);

MotorInput4= 1.024*(InputThrottle+InputRoll – InputPitch + InputYaw);

// Обмежуємо команди максимальної потужності, що надсилаються двигунам

if (MotorInput1 > 2000)MotorInput1 = 1999;

if (MotorInput2 > 2000)MotorInput2 = 1999;

if (MotorInput3 > 2000)MotorInput3 = 1999;

if (MotorInput4 > 2000)MotorInput4 = 1999;

// Тримаємо двигуни квадрокоптера під час польоту на рівні не менше 18%.

int ThrottleIdle = 1180;

if (MotorInput1 < ThrottleIdle) MotorInput1 = ThrottleIdle;

if (MotorInput2 < ThrottleIdle) MotorInput2 = ThrottleIdle;

if (MotorInput3 < ThrottleIdle) MotorInput3 = ThrottleIdle;

if (MotorInput4 < ThrottleIdle) MotorInput4 = ThrottleIdle;

// рядки пов’язані з безпекою: потрібно переконатися у взмозі вимкнути двигуни

int ThrottleCutOff=1000;

if (ReceiverValue[2]<1050) {

MotorInput1=ThrottleCutOff;

MotorInput2=ThrottleCutOff;

MotorInput3=ThrottleCutOff;

MotorInput4=ThrottleCutOff;

reset_pid();

}

// Відсилання команд до двигунів

analogWrite(1,MotorInput1);

analogWrite(2,MotorInput2);

analogWrite(3,MotorInput3);

analogWrite(4,MotorInput4);

// Моніторинг рівня заряду акумулятора

battery_voltage();

CurrentConsumed=Current*1000*0.004/3600 + CurrentConsumed;

BatteryRemaining=(BatteryAtStart-

CurrentConsumed)/BatteryDefault*100;

if (BatteryRemaining<=30) digitalWrite(5, HIGH);

else digitalWrite(5, LOW);

// Завершення циклу управління 250 Гц

while (micros() - LoopTimer < 4000);

LoopTimer=micros();

}