Програмний код - Куточок саморобкіна

Нотатка присвячена вивченню коду Ardupilot 2.8 (та успадкованих огріхів із попередніх)

  • Вивчення коду для того, щоб описати особливості використання пристрою
  • Виявлення та усунення помилок
  • Очищення від застарілого функціоналу для звільнення простору у пам'яті

Для перевірки зроблених висновків використано

  • XPLANE 9 desktop із моделлю PT60RC
  • ArdupilotSim - для зв'язку симулятора та usb-serial
  • arduino duemilanove - як usb-серіальний адаптер
  • плата ArduPilot-v15 з додатковими з'єднаннями по каналам газу та керма напряму
  • Прошивка Ardupilot 2.8 beta

Виявлені недоліки:

  1. Істотна помилка в навігації, що призводить до руху "по дузі" по маршрутних точках (виправлено в beta 4)
  2. Різке підкидання моделі в режимі landing при досягненні координат точки вказаної в якості точки приземлення (усунено в beta 5 за допомогою плавного збільшення кута)
  3. Помилка відключення газу від каналу ручного керування у заключній стадії автоматичної посадки (усунено у beta 5)
  4. Помилка навігації по висоті порядку -20м (усунуто в beta 5 введено коефіцієнт PID залежно від видалення від точки)
  5. Помилка навігації за курсом порядку +/- 5 градусів (усунуто в beta 5 введено коефіцієнт PID залежно від видалення від точки)
  6. Різкі маневри поблизу маршрутних точок (усунуто в beta 5 введено коефіцієнт PID залежно від видалення точки)
  7. Неточне проходження маршрутних точок (не усунуто)
  8. Визначення пропадання сигналу від передавача для аналогового приймача, не з Failsafe (не усунено)
  9. Розрахунок курсу щодо становища фюзеляжу літака (гіроскоп), не враховується маса моделі, швидкість, тобто. момент інерції (це особливо помітноякщо задати велике посилення в каналі керма напряму без згладжування) (не усунено)
  10. Відсутність обліку зсуву домашньої точки. (не усунено)

Алгоритми коду

Загальні принципи.

Особливості апаратної платформи ардуїно в тому, що процесор має одне ядро, ні середовище розробки ні апаратна платформа не підтримує багатопоточність, натомість використовуються переривання і два основних цикли setup і loop. Мова програмування з ++ при цьому якщо вам незрозуміла мовна конструкція - відповідь слід шукати в посібниках та форумах з цієї мови

Основним модулем вважається pde файл, що має назву, що збігається з назвою папки в яку він вкладений, на початку цього файлу йде декларація бібліотек і глобальних змінних відповідно до вимог С++ потім дві процедури setup і loop, після кожного скидання спочатку один раз виконується програмний код записаний в процедуру setup а потім нескінченну кількість разів (або поки не зависне через помилку в коді) код процедури loop.

Код ардупілота (а також плати сенсорів) зроблений за принципом декількох циклів, що виконуються послідовно, один цикл (fast loop) виконується 50 разів на секунду, другий (medium loop) 10, у гілки цих циклів розставлені функції які повинні виконуватися з необхідною періодичністю

Ручний режим канал керма висоти приєднаний до порту ардуїно № . Зчитування каналу автоматично по перериваннях і доступне в змінній. типу. туди: дописати алгоритми

Усунення недоліків.

ong get_distance(struct Location *loc1, struct Location *loc2) < float pRad = 0.00000000174527777777; float lat2=loc2->lat* pRad; float lon2=loc2-lng* pRad; float lat= loc1->lat* pRad; float lon= loc1-lng* pRad;

float dlx = 6371000 * (lon-lon2); float dly=6371000 log(tan(lat/2+M_PI_4))-6371000 log(tan(lat2/2+M_PI_4)); return sqrt(dlx*dlx+dly*dly)*cos(lat); >

long get_bearing(struct Location *loc1, struct Location *loc2) < float pRad = 0.00000000174527777777; float lat2=loc2->lat* pRad; float lon2=loc2-lng* pRad; float lat= loc1->lat* pRad; float lon= loc1-lng* pRad;

float dlx = 6371000 * (lon-lon2); float dly=6371000 log(tan(lat/2+M_PI_4))-6371000 log(tan(lat2/2+M_PI_4)); long bearing = (atan2(dlx,dly)*180/M_PI+180) * 100.00; return bearing; >

2. Різке підкидання моделі в режимі landing при досягненні координат точки вказаної в якості точки приземлення.

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

4. Помилка навігації по висоті - причина помилки поки що не локалізована, і ймовірно знаходиться в калькуляції PID але вона нівельована функцією в якій мова в пункті 6. Ймовірно однією з причин є те, що піддя дають відхилення на корекцію існуючої помилки але реакція моделі запізнюється і до наступному моменту часу хоч існуюча помилка і усунена, але з'являється нова. Посилення значень під допомогою завдання вбудованих параметрів призводить до коливань моделі при різких маневрах і в маршрутних точках

5. Помилка навігації за курсом - причина помилки поки що не локалізована, і ймовірнознаходиться в калькуляції PID але вона нівельована функцією, в якій мова в пункті 6

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

функція обчислення коеф. корекції помилки висоти

float K_nav_by_wp_distance() < if (wp_distance > 100) < if abs(bearing_error > 60 ) < return 1.0f; >else > else if (wp_distance > 22) < return 1.0f;> else < return 0.1f;>// disable naw near waypoint

7. В ардупілоті маршрутна точка вважається досягнутою якщо відстань до неї становить wp_radius - радіус заданий з утиліти configtoolglobal разом з координатами точок, тобто досягнувши радіуса точки автопілот починає навігацію до наступної, не досягаючи по можливості найближчої дистанції до точки, помилка істотна якщо задати радіус 20м і використовувати його для посадки, логічно змінити код таким чином, що вважати, що точка досягнута в той момент, після проходження радіуса коли дистанція від точки починає зростати.

8. за цим пунктом, поки що сказати, не реалізовано.

9. Немає урахування моменту інерції моделі.