- PVSM.RU - https://www.pvsm.ru -

Разбираемся в MAVLink. Часть 2

В прошлой [1] части мы разобрали основные принципы работы с протоколом MAVLink [2] и научились обмениваться сообщениями типа HEARTBEAT [3]. В этой части мы рассмотрим некоторые другие типы сообщений, которые отвечают за полётные данные и попробуем эти данные визуализировать при помощи Qt.

image

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

include_directories("3dparty/mavlink_v1/ardupilotmega")

В этой части мы рассмотрим некоторые общие [5] сообщения, которые должны быть реализованы в большинстве полётных контроллеров и наземных станциях управления(GCS) и смена диалекта никак не должна отразиться на работоспособности кода. За основу мы возьмём примеры из прошлой части, и добавим обработчики новых типов сообщений, сервис, модель и представление для полётных данных. Сразу оговорюсь, что не буду подробно описывать Qt-представление, это выходит за рамки статьи, но весь исходный код доступен на гитхабе [6]. В качестве модели предметной области будет выступать класс Vehicle, который будет агрегировать полётные данные для каждого из MAV, а сервис VehicleService позволит запросить/создать Vehicle по systemId. Ниже приведена упрощённая диаграмма классов.

image

Сообщение типа ATTITUDE [7] описывает поворотное положение 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 [8], в котором сгруппированы параметры, обычно выводимые на индикаторе на лобовом стекле [9]. К этим параметрам 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 [10] и
GLOBAL_POSITION [11] соответственно. Эти пакеты подразумевают уже обработанные, фильтрованные данные. Для сырых показаний GPS сенсора необходимо обрабатывать пакеты GPS_RAW [12] и GPS_STATUS [13]. Для обработки пакета положения добавим обработчик PositionHandler, а для пакетов данных GPS — GpsHandler. Обработка других общих [5] типов пакетов производиться по тому же принципу. Полётный контроллер отсылает нам пакеты с определённой частотой, которую он определяет сам, на основе настроек, скорости передачи данных или типа канала связи. Тем не менее, частоту отправки каких-либо данных можно запросить вручную, отправив сообщение MESSAGE_INTERVAL [14] с указанием идентификатора нужного сообщения и интервалом в микросекундах.

Метод отправки запроса интервала сообщения

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 [15]. А для того, чтобы эти структуры данных были доступны из представления на Qt Quick(QML), добавим в их описания макрос Q_GADGET [16]. Класс 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

Проверку можно осуществлять на реальном полётном контроллере или же на имитаторе [17] из предыдущей части, отправка новых типов пакетов там уже есть. В следующей статье я постараюсь рассказать про команды и протокол точек(Waypoint Protocol) MAVLink. Благодарю за внимание!

Автор: mrogachev

Источник [18]


Сайт-источник PVSM.RU: https://www.pvsm.ru

Путь до страницы источника: https://www.pvsm.ru/qt-2/213986

Ссылки в тексте:

[1] прошлой: https://habrahabr.ru/post/312300/

[2] MAVLink: http://qgroundcontrol.org/mavlink/start

[3] HEARTBEAT: http://mavlink.org/messages/common#HEARTBEAT

[4] собственные: http://qgroundcontrol.org/mavlink/create_new_mavlink_message

[5] общие: http://mavlink.org/messages/common

[6] гитхабе: https://github.com/MishkaRogachev/JAGCS

[7] ATTITUDE: http://mavlink.org/messages/common#ATTITUDE

[8] VFR_HUD: http://mavlink.org/messages/common#VFR_HUD

[9] индикаторе на лобовом стекле: https://ru.wikipedia.org/wiki/%D0%98%D0%BD%D0%B4%D0%B8%D0%BA%D0%B0%D1%82%D0%BE%D1%80_%D0%BD%D0%B0_%D0%BB%D0%BE%D0%B1%D0%BE%D0%B2%D0%BE%D0%BC_%D1%81%D1%82%D0%B5%D0%BA%D0%BB%D0%B5

[10] LOCAL_POSITION: http://mavlink.org/messages/common#LOCAL_POSITION_NED

[11] GLOBAL_POSITION: http://mavlink.org/messages/common#GLOBAL_POSITION_INT

[12] GPS_RAW: http://mavlink.org/messages/common#GPS_RAW_INT

[13] GPS_STATUS: http://mavlink.org/messages/common#GPS_STATUS

[14] MESSAGE_INTERVAL: http://mavlink.org/messages/common#MESSAGE_INTERVAL

[15] qRegisterMetaType: http://doc.qt.io/qt-5/qmetatype.html#qRegisterMetaType

[16] Q_GADGET: http://doc.qt.io/qt-5/qobject.html#Q_GADGET

[17] имитаторе: https://github.com/MishkaRogachev/mavlink_experiments

[18] Источник: https://habrahabr.ru/post/312592/?utm_source=habrahabr&utm_medium=rss&utm_campaign=best