MAVLink (Micro Air Vehicle Link) to protokół komunikacyjny wykorzystywany do sterowania małymi pojazdami bezzałogowymi. Charakteryzuje się bardzo krótkim nagłówkiem. W wersji 1 ma on długość zaledwie 8 bajtów, w wersji 2 jest to 14 bajtów. Dzięki temu dobrze nadaje się do zastosowania w środkach komunikacji o ograniczonej przepustowości. Używa się go między innymi w hobbystycznych projektach wykorzystujących moduły radiowe nRF24L01, ale jest też niezależny od warstwy fizycznej.
W protokole MAVLink dane telemetryczne są przesyłane w taki sposób, aby zużywać jak najmniejszą przepustowość łącza, natomiast wiadomości zawierające ważne polecenia są retransmitowane w przypadku utracenia lub uszkodzenia pakietu. W zależności od rodzaju systemu stosuje się różne „dialekty”. Są one zdefiniowane za pomocą plików xml, a podstawowy zestaw wiadomości określony jest w zbiorze o nazwie common.
Dlaczego warto zastosować protokół MAVLink w swoim projekcie?
Standard ten jest wykorzystywany w wielu projektach autopilotów dla małych bezzałogowych aparatów latających, z których pięć najpopularniejszych to:
Elektronikę potrzebną do skonstruowania autopilota można skompletować samodzielnie lub kupić gotowe urządzenie. MAVLink jest też wykorzystywany w stale rozwijanych aplikacjach dla stacji naziemnych jak QGroundControl czy Mission Planner. Zamiast tworzyć własną aplikację od zera, lepiej wybrać do swojego projektu jedną z nich i zaoszczędzić dużo czasu, który można wykorzystać na budowę pojazdu.
Przykład na podstawie C i Linuksa
Wzorcowa implementacja protokołu MAVLink jest napisana w języku C, jednak istnieją generatory dla wielu innych języków (MAVLink Project Generators/Languages). Sposób generowania i korzystania z bibliotek w języku C jest szczegółowo opisany na stronie Using C MAVLink Libraries (mavgen). Poniżej zamieszczam działający przykład w języku C.
Podobny można znaleźć w repozytorium projektuMAVLink UDP Linux Example, a jego autorem jest Bryan Godbolt. Dzięki dodanym instrukcjom preprocesora, poniższy kod skompiluje się również na systemie Windows. Aby go skompilować, należy pobrać i rozpakować udostępnione przez twórców wygenerowane biblioteki: c_library_v2. Następnie należy wykonać poniższe polecenie, podając właściwą ścieżkę do rozpakowanej biblioteki
- na Linuksie
gcc -I ../c_library_v2/common -o mavlink_udp mavlink_udp.c -lpthread
- na Windowsie
gcc -I ../c_library_v2/common -o mavlink_udp mavlink_udp.c -lwsock32
Program przyjmuje jako parametr uruchomieniowy adres IP stacji naziemnej. W aplikacji QGroundControl należy wejść do menu Application Settings > Comm Links i dodać połączenie typu UDP, podając adres serwera, czyli urządzenia, na którym będzie uruchomiony przykład. Można uruchomić przykład i aplikację stacji naziemnej QGroundControl na tym samym urządzeniu.
Na tym etapie przykładowy program umożliwia tylko nawiązanie połączenia przez stację naziemną. Wysyła co sekundę wiadomość typu HEARTBEAT z kilkoma prostymi informacjami:
- Parametr system_id to identyfikator pojazdu. Wszystkich węzłów w jednej sieci (pojazdów, stacji naziemnych) może być w maksymalnie 255.
- Parametr component_id to identyfikator komponentu pojazdu. W bardziej złożonych konstrukcjach czujnik IMU czy obracana kamera mogą być osobnymi komponentami
).
Sterowanie ręczne
Aplikacja QGroundControl obsługuje popularne kontrolery do gier. Jej twórcy polecają kilka modeli na PC oraz DualShocka 4 znanego z konsol do gier PlayStation. O obsłudze gamepadów przez QGroundControl można przeczytać więcej na stronie Joystick Setup. Telefon lub tablet wraz z kontrolerem mogą razem pełnić rolę modelarskiej aparatury nadawczej. Można ewentualnie wyświetlić wirtualne joysticki na ekranie, ale nie jest to praktyczne rozwiązanie.
Aby pojazd mógł odbierać wiadomości zawierające informacje o wychyleniach joysticków i naciśnięciach przycisków, należy dodać wątek odbierający wiadomości. Są one rozróżniane po identyfikatorze. Wiadomość zawierająca wychylenia joysticków i stan przycisków ma identyfikator 69.
Konstruktorzy z powodzeniem wykorzystują aplikację stacji bazowej do sterowania pojazdami za pośrednictwem sieci komórkowej. Zachęcam do obejrzenia nagrania z lotu quadcoptera sterowanego i przesyłającego obraz z kamery w ten sposób.
Samodzielne napisanie programu, który odczytuje dane przychodzące z urządzenia wejściowego i przekazuje je dalej przez sieć, nie jest bardzo skomplikowane, ale to nie jedyna funkcja stacji bazowej.
Plan lotu
QGroundControl i wiele innych aplikacji stacji bazowych posiada dwa tryby pracy: Fly i Plan. Ten drugi służy do planowania trasy lotu i jest wykorzystywany przez użytkowników aparatów latających o pewnym stopniu autonomii, zdolnych do wykonywania zadań bez ciągłego nadzoru, a nawet poza zasięgiem stacji naziemnej. Obsługę trybu Plan szczegółowo zaprezentowano w tym filmie instruktażowym.
W dalszej części pokażę, jak odebrać ze stacji naziemnej plan lotu, czyli listę punktów trasy do przebycia (waypointów):
Po naniesieniu na mapę w trybie Plan wszystkich punktów trasy, określeniu wysokości przelotu itd., klikamy przycisk Upload. Stacja naziemna (GCS) wysyła do pojazdu wiadomość typu MISSION_COUNT zawierającą informację o ilości punktów trasy. Pojazd odpowiada żądaniem przesłania pierwszego punktu (MISSION_REQUEST_INT). Na każde takie żądanie GCS odpowiada, przesyłając informacje o danym punkcie (MISSION_ITEM_INT). Punkty są numerowane od zera.
Kiedy indeks otrzymanego punktu jest mniejszy o jeden od zadeklarowanej ilości punktów w trasie (count-1), pojazd wysyła potwierdzenie odebrania planu lotu (MISSION_ACK). Należy pamiętać, że przykład korzysta z protokołu UDP, bez gwarancji dostarczenia wiadomości. Dane mogą być również wysyłane kanałem dużo bardziej zawodnym niż domowe połączenie Wi-Fi. Jeśli GCS nie odeśle wiadomości MISSION_ITEM_INT w ciągu 250 milisekund od nadania wiadomość MISSION_REQUEST_INT, to trzeba nadać ją ponownie (maksymalnie 5 razy).
Procedura przesyłania planu lotu jest szczegółowo opisana na stronie Mission Protocol w paragrafie Upload a Mission to the Vehicle, z którego pochodzi poniższy schemat:
Parametry
Aby móc prawidłowo nawiązać połączenie z pojazdem, musi on jeszcze odpowiedzieć na żądania transmisji parametrów przychodzące ze stacji naziemnej. Tutaj sprawa jest prostsza – po otrzymaniu wiadomości PARAM_REQUEST_LIST wystarczy przesłać co najmniej jeden parametr za pomocą wiadomości PARAM_VALUE. W przykładzie jest to parametr o nazwie COMPASS_USE i wartości FALSE. Przesłanie takiej wiadomości oznacza, że pojazd nie wykorzystuje magnetometru. Kolejne parametry powinny być wysyłane w równych odstępach czasu.
Sposób przesyłania parametrów jest szczegółowo opisany na stronie Parameter Protocol w paragrafie Read All Parameters.
Telemetria
Wiadomości typu SYS_STATUS służą do przesyłania ważnych informacji o stanie pojazdu, takich jak jakość połączenia czy poziom naładowania akumulatora. Przykładową wiadomość można wysyłać co sekundę, wstawiając poniższy fragment kodu w funkcji wysyłającej wiadomość HEARTBEAT.
uint16_t voltage_battery = 12600; // millivolts
int16_t current_battery = -1; // centiamperes
int8_t battery_remaining = 100; // percent
mavlink_msg_sys_status_pack(THIS_SYSTEM, THIS_COMPONENT, &out_msg, 0, 0, 0, 0, voltage_battery, current_battery, battery_remaining, 100, 0, 0, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &out_msg);
sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
Do przesyłania aktualnej pozycji pojazdu służy wiadomość GLOBAL_POSITION_INT. Długość geograficzna (longitude), szerokość geograficzna (latitude) i wysokość (nad poziomem morza i względna) są zapisywane jako liczby całkowite. Po odebraniu takiej wiadomości stacja naziemna wyświetla ikonę pojazdu na mapie.
Gdy pojazd przemieszcza się – rysuje przebytą trasę, a nawet umożliwia zapisywanie jej do pliku. Ten kod powinien być wykorzystany za każdym razem, gdy odbiornik GPS zarejestruje istotną zmianę pozycji pojazdu.
int32_t lat = 52.190564 * 1E7; // Latitude
int32_t lon = 21.015880 * 1E7; // Longitude
int32_t alt = 112 * 1000; // above MSL (mm)
int32_t relative_alt = 0; // above ground (mm)
mavlink_msg_global_position_int_pack(THIS_SYSTEM, THIS_COMPONENT, &out_msg, microsSinceEpoch(), lat, lon, alt, relative_alt, 0, 0, 0, UINT16_MAX);
len = mavlink_msg_to_send_buffer(buf, &out_msg);
sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
Informacje o przechyle i pochyle powinny pochodzić z czujnika grawitacji, natomiast kierunek z magnetometru (ewentualnie można je również obliczyć na podstawie współrzędnych GPS z kilku ostatnich sekund). Do ich przesyłania służy wiadomość ATTITUDE. Kąty muszą być podane w radianach. Aktualny kierunek jest przedstawiany w aplikacji przez obrót ikony pojazdu na mapie, natomiast przechył i pochył są prezentowane za pomocą sztucznego horyzontu wzorowanego na urządzeniach spotykanych w samolotach.
float roll = 0 * (M_PI/180.0);
float pitch = 0 * (M_PI/180.0);
float yaw = 0 * (M_PI/180.0);
mavlink_msg_attitude_pack(THIS_SYSTEM, THIS_COMPONENT, &out_msg, microsSinceEpoch(), roll, pitch, yaw, 0.0, 0.0, 0.0);
len = mavlink_msg_to_send_buffer(buf, &out_msg);
sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
Wiadomości GLOBAL_POSITION_INT i ATTITUDE wymagają użycia funkcji microsSinceEpoch(), która zwraca aktualny czas w mikrosekundach.
Przykład na podstawie C, Javy i Androida
Na sam koniec zaprezentuję przykład na podstawie Androida. Poniższa aplikacja nie nadaje stałej pozycji, lecz korzysta z akcelerometru i odbiornika GPS urządzenia, na którym jest zainstalowana. Nawiązanie przez sieć Wi-Fi połączenia z telefonem z uruchomioną aplikacją umożliwia śledzenie jego położenia na mapie.
Podsumowanie
Przedstawiłem tylko niektóre z możliwości protokołu MAVLink i korzystających z niego stacji naziemnych. Ich twórcy stale rozwijają oprogramowanie, by nadążało za potrzebami rozwijającej się branży UAV. Mam nadzieję, że zachęciłem do zgłębienia tematu sterowania lekkimi bezzałogowcami.
Więcej o MAVLink i stacjach bazowych dla miniaturowych aparatów latających można się dowiedzieć tutaj:
- mavlink.io – dokumentacja protokołu MAVLink
- docs.qgroundcontrol.com – dokumentacja aplikacji QGroundControl
- ardupilot.org – dokumentacja aplikacji Mission Planner
Zostaw komentarz