Розбираємося в MAVLink. Частина 2

минулого частині ми розібрали основні принципи роботи з протоколом MAVLink і навчилися обмінюватися повідомленнями типу HEARTBEAT. У цій частині ми розглянемо деякі інші типи повідомлень, які відповідають за польотні дані і спробуємо ці дані візуалізувати за допомогою Qt.

image


У MAVLink існують різні вбудовані типи повідомлень, а так само є можливість додавати власні. В дійсності, які дані вважати польотні, які повідомлення відправляються періодично, а які тільки за запитом вирішує польотний контролер. MAVLink не декларує, якими повідомленнями необхідно користуватися, ми самі при проектуванні наших систем вирішуємо які повідомлення наше програмне забезпечення буде обробляти, і які — відправляти. Для різних польотних контролерів передбачені діалекти, що відрізняються деталями реалізації: складом повідомлень або даних, наприклад режимами. Докорінно header-only С/C++ бібліотеки MAVLink є каталоги, відповідні цим діалектів: common, ardupilotmega та ін. Те, який діалект буде використовуватися в наших прикладах, можна визначити, вказавши шлях до потрібних заголовочным файлів в CMake.

include_directories("3dparty/mavlink_v1/ardupilotmega")

У цій частині ми розглянемо деякі загальне повідомлення, які повинні бути реалізовані в більшості польотних контролерів і наземних станціях керування(GCS) і зміна діалекту ніяк не повинна позначитися на працездатності коду. За основу ми візьмемо приклади з минулій частині, і додамо обробники нових типів повідомлень, сервіс, модель та подання для польотних даних. Відразу обмовлюся, що не буду детально описувати Qt-подання, це виходить за рамки статті, але весь вихідний код доступний на гітхабі. В якості моделі предметної області буде виступати клас Vehicle, який буде агрегувати польотні дані для кожного з MAV, а сервіс VehicleService дозволить запросити/створити Vehicle systemId. Нижче наведена спрощена діаграма класів.

image

Повідомлення типу ATTITUDE описує поворотне положення MAV(безпілотника) відносно центру в просторі — кути тангажа, крену і рискання. Подібно наприклад з повідомленням HEARTBEAT з минулої частини наследуемся від абстрактного класу обробки повідомлення(AbstractMavLinkHandler), декодируем пакет і отримуємо наші дані — кути крену, тангажа і рискання. З повідомлення ми отримуємо systemId системи, яка надіслала наша повідомлення, і можемо зіставити, для якого Vehicle необхідно оновити дані.

Реалізація методу processMessage класу AttitudeHandler
void AttitudeHandler::processMessage(const mavlink_message_t& message)
{
if (message.msgid != MAVLINK_MSG_ID_ATTITUDE) return;

Vehicle* vehicle = m_vehicleService->requestVehicle(message.sysid);

mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);

vehicle->setAttitude(Attitude(qRadiansToDegrees(attitude.pitch),
qRadiansToDegrees(attitude.roll),
qRadiansToDegrees(attitude.yaw)));
}


Аналогічним чином напишемо обробник пакетів типу VFR_HUD, в якому згруповані параметри, зазвичай виводяться на індикатор на лобовому склі. До цих параметрами MAVLink відносить: повітряна швидкість, шляхова швидкість, висота над рівнем моря, швидкопідйомність, напрямок і газ(throttle).

Реалізація методу processMessage класу VfrHudHandler
void VfrHudHandler::processMessage(const mavlink_message_t& message)
{
if (message.msgid != MAVLINK_MSG_ID_VFR_HUD) return;

Vehicle* vehicle = m_vehicleService->requestVehicle(message.sysid);

mavlink_vfr_hud_t vfrHud;
mavlink_msg_vfr_hud_decode(&message, &vfrHud);

vehicle->setTrueAirSpeed(vfrHud.airspeed);
vehicle->setGroundSpeed(vfrHud.groundspeed);
vehicle->setBarometricAltitude(vfrHud.alt);
vehicle->setBarometricClimb(vfrHud.climb);
vehicle->setHeading(vfrHud.heading);
vehicle->setThrottle(vfrHud.throttle);
}


Положення MAV у просторі може бути визначено за допомогою локальної або глобальної системи позиціонування. Ці дані передаються протоколом у повідомленнях типу LOCAL_POSITION
GLOBAL_POSITION відповідно. Ці пакети мають на увазі вже оброблені, відфільтровані дані. Для сирих показань GPS сенсора необхідно обробляти пакети GPS_RAW і GPS_STATUS. Для обробки пакета положення додамо обробник PositionHandler, а для пакетів даних GPS — GpsHandler. Обробка інших загальних типів пакетів проводитися за тим же принципом. Польотний контролер посилає нам пакети з певною частотою, яку він визначає сам, на основі налаштувань, швидкості передачі даних або типу каналу зв'язку. Тим не менш, частоту відправлення будь-яких даних можна запросити вручну, відправивши повідомлення MESSAGE_INTERVAL з зазначенням ідентифікатора потрібного повідомлення і інтервалом у мікросекундах.

спосіб надсилання запиту інтервалу повідомлення
void IntervalHandler::requestMessageFrequency(int messageId, float frequency)
{
mavlink_message_t message;
mavlink_message_interval_t interval;

interval.message_id = messageId;
interval.interval_us = ::hzToUs(frequency);

mavlink_msg_message_interval_encode(m_communicator->systemId(),
m_communicator->componentId(),
&message, &interval);

m_communicator->sendMessageAllLinks(message);
}


Коли ми напишемо обробники для всіх нас цікавлять типів пакетів, зможемо наповнювати нашу модель (клас Vehicle) даними. При оновленні даних, модель буде сповіщати подання з допомогою системи сигналів-слотів Qt. Для того, щоб оновлення подання (або будь-яка інша дія) не проходила кілька разів при обробці одного і того ж пакету, дані в моделі ми згрупуємо у структуру(клас), дзеркально змістом пакету або за логічним змістом. Так як наші нові типи будуть використані в якості аргументів сигналів і слотів Qt, їх необхідно зареєструвати в мета-об'єктній системі Qt з допомогою функції qRegisterMetaType. А для того, щоб ці структури даних були доступні подання на Qt Quick(QML), додамо в їх опису макрос Q_GADGET. Клас Attitude, приміром, буде групувати поворотне положення.

Заголовковий файл класу Attitude
class Attitude
{
Q_GADGET

Q_PROPERTY(float pitch READ pitch CONSTANT)
Q_PROPERTY(float roll READ roll CONSTANT)
Q_PROPERTY(float yaw READ yaw CONSTANT)

public:
Attitude(float pitch = 0.0, float roll = 0.0, float yaw = 0.0);

float pitch() const;
float roll() const;
float yaw() const;

bool operator ==(const Attitude& other);

private:
float m_pitch;
float m_roll;
float m_yaw;
};


У QML у мене буде два основних вистави(views) — карта і пілотажний прилад. Для карти є готовий компонент з модуля Qt Location, на ньому будемо відображати значки MAV з певним положенням та курсом, а так само їх траєкторії. Пілотажний прилад (FD) доведеться малювати вручну, для цього завдання я вибрав QML Canvas, результат на картинці внизу.

image

Перевірку можна здійснювати на реальному польотному контролері або ж на імітаторі з попередньої частини, відправка нових типів пакетів там вже є. У наступній статті я постараюся розповісти про команди і протокол точок(Waypoint Protocol) MAVLink. Дякую за увагу!
Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.