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