В прошлой части мы разобрали основные принципы работы с протоколом MAVLink и научились обмениваться сообщениями типа HEARTBEAT. В этой части мы рассмотрим некоторые другие типы сообщений, которые отвечают за полётные данные и попробуем эти данные визуализировать при помощи Qt.
В MAVLink существуют различные встроенные типы сообщений, а так же есть возможность добавлять собственные. В действительности, какие данные считать полётными, какие сообщения отправляются периодически, а какие только по запросу решает полётный контроллер. MAVLink не декларирует, какими сообщениями необходимо пользоваться, мы сами при проектировании наших систем решаем какие сообщения наше программное обеспечение будет обрабатывать, и какие — отправлять. Для различных полётных контроллеров предусмотрены диалекты, отличающиеся деталями реализации: составом сообщений или данными, например режимами. В корне header-only С/C++ библиотеки MAVLink есть каталоги, соответствующие этим диалектам: common, ardupilotmega и др. То, какой диалект будет использоваться в наших примерах, можно определить указав путь к нужным заголовочным файлам в CMake.
include_directories("3dparty/mavlink_v1/ardupilotmega") В этой части мы рассмотрим некоторые общие сообщения, которые должны быть реализованы в большинстве полётных контроллеров и наземных станциях управления(GCS) и смена диалекта никак не должна отразиться на работоспособности кода. За основу мы возьмём примеры из прошлой части, и добавим обработчики новых типов сообщений, сервис, модель и представление для полётных данных. Сразу оговорюсь, что не буду подробно описывать Qt-представление, это выходит за рамки статьи, но весь исходный код доступен на гитхабе. В качестве модели предметной области будет выступать класс Vehicle, который будет агрегировать полётные данные для каждого из MAV, а сервис VehicleService позволит запросить/создать Vehicle по systemId. Ниже приведена упрощённая диаграмма классов.
Сообщение типа ATTITUDE описывает поворотное положение MAV(беспилотника) относительно его центра в пространстве — углы тангажа, крена и рыскания. Подобно примеру с сообщением HEARTBEAT из прошлой части наследуемся от абстрактного класса обработки сообщения(AbstractMavLinkHandler), декодируем пакет и получаем наши данные — углы крена, тангажа и рыскания. Из сообщения мы получаем systemId системы, которая прислала наше сообщение, и можем сопоставить, для какого Vehicle необходимо обновить данные.
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).
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, к примеру, будет группировать поворотное положение.
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, результат на картинке внизу.
Проверку можно осуществлять на реальном полётном контроллере или же на имитаторе из предыдущей части, отправка новых типов пакетов там уже есть. В следующей статье я постараюсь рассказать про команды и протокол точек(Waypoint Protocol) MAVLink. Благодарю за внимание!
ссылка на оригинал статьи https://habrahabr.ru/post/312592/
Добавить комментарий