Разбираемся в MAVLink. Часть 226.11.2016 15:50

В прошлой части мы разобрали основные принципы работы с протоколом 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. Благодарю за внимание!

Комментарии (2)

  • 25 ноября 2016 в 18:45

    +1

    ↑

    ↓

    На первой картинке надо было начертать »Hello OSM» :3
    Ну или хотя бы копирайты указать.

    • 26 ноября 2016 в 13:13

      0

      ↑

      ↓

      Qt Location по-умолчанию показывает копирайты OSM, просто скриншоты для статьи я обрезал.
      Картинка
      f9b5826306d24c7ebe5cfe057afd0a7a.png

© Habrahabr.ru