Разбираемся в 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. Благодарю за внимание!
    Метки:
    • +10
    • 6,4k
    • 4
    Поделиться публикацией
    Комментарии 4
    • +1
      На первой картинке надо было начертать "Hello OSM" :3
      Ну или хотя бы копирайты указать.
      • +1
        Qt Location по-умолчанию показывает копирайты OSM, просто скриншоты для статьи я обрезал.
        Картинка


      • 0
        А почему имитатор через cmake а не через pro файл? Это не удобно, например для тех кто пишет на Qt. QMake ничем не хуже cmak-a и идет в комплекте с Qt.
        • 0
          Я использую cmake, потому что большинство библиотек и инструментов, которыми я пользуюсь используют именно эту систему сборки. А касательно qmake, я не до конца понимаю чем он может быть удобнее. Qt Creator так же может открывать cmake проекты.

        Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.