[Из песочницы] Разбираемся в MAVLink. Часть 111.10.2016 16:03

class AbstractLink: public QObject
{
    Q_OBJECT
    
public:
    explicit AbstractLink(QObject* parent = nullptr);
    
    virtual bool isUp() const = 0;
    
public slots:
    virtual void up() = 0;
    virtual void down() = 0;
    
    virtual void sendData(const QByteArray& data) = 0;
    
signals:
    void upChanged(bool isUp);
    void dataReceived(const QByteArray& data);
};

class MavLinkCommunicator: public QObject
{
    Q_OBJECT
    
public:
    MavLinkCommunicator(QObject* parent = nullptr);
    
public slots:
    void addLink(AbstractLink* link, uint8_t channel);
    void removeLink(AbstractLink* link);
    
    void sendMessage(mavlink_message_t& message, AbstractLink* link);
    void sendMessageOnLastReceivedLink(mavlink_message_t& message);
    void sendMessageOnAllLinks(mavlink_message_t& message);
    
signals:
    void messageReceived(const mavlink_message_t& message);
    
private slots:
    void onDataReceived(const QByteArray& data);
    
private:
    QMap m_linkChannels;
    AbstractLink* m_lastReceivedLink;
};

void MavLinkCommunicator::onDataReceived(const QByteArray& data)
{
    mavlink_message_t message;
    mavlink_status_t status;

    // onDataReceived это слот, который вызываться строго по сигналу от AbstractLink
    m_lastReceivedLink = qobject_cast(this->sender()); 
    if (!m_lastReceivedLink) return;

    // идентификатор канала получаем из словаря
    uint8_t channel = m_linkChannels.value(m_lastReceivedLink); 
    for (int pos = 0; pos < data.length(); ++pos)
    {
        if (!mavlink_parse_char(channel, (uint8_t)data[pos],
                                &message, &status))
            continue;

        emit messageReceived(message); // по этому сигналу происходит обработка принятого пакета
    }
    // Обработка статуса канала связи
}

void HeartbeatHandler::processMessage(const mavlink_message_t& message)
{
    // проверяем, можем ли обработать пакет
    if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT) return;
    
    mavlink_heartbeat_t heartbeat; // создаём сообщение heartbeat
    mavlink_msg_heartbeat_decode(&message, &heartbeat); // наполняем его из полученного пакета
    
    // Здесь должна быть обработка сообщения, но у нас пока будет отладочный вывод
    qDebug() << "Heartbeat received, system type:" << heartbeat.type; 
}

int main(int argc, char* argv[])
{
    QCoreApplication app(argc, argv);

    domain::MavLinkCommunicator communicator;

    domain::HeartbeatHandler heartbeatHandler; // добавляем обработчик сообщений heartbeat
    QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived,
                     &heartbeatHandler, &domain::HeartbeatHandler::processMessage);

    domain::SerialLink link("/dev/ttyUSB0", 57600); // путь к радиомодему и его скорость
    communicator.addLink(&link, MAVLINK_COMM_0);
    link.up();

    return app.exec();
}

void MavLinkCommunicator::sendMessage(mavlink_message_t& message, AbstractLink* link)
{
    if (!link || !link->isUp()) return;

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int lenght = mavlink_msg_to_send_buffer(buffer, &message);

    if (!lenght) return;
    link->sendData(QByteArray((const char*)buffer, lenght));
}

void HeartbeatHandler::timerEvent(QTimerEvent* event)
{
    Q_UNUSED(event)

    mavlink_message_t message;
    mavlink_heartbeat_t heartbeat;
    heartbeat.type = m_type;

    mavlink_msg_heartbeat_encode(m_systemId, m_componentId, &message, &heartbeat);

    emit sendMessage(message);
}

int main(int argc, char* argv[])
{
    QCoreApplication app(argc, argv);

    // Для GCS 255 является стандартным sysid
    domain::MavLinkCommunicator communicator(255, 0);

    // Тип системы - станция наземного управления
    domain::HeartbeatHandler heartbeatHandler(MAV_TYPE_GCS);
    QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived,
                     &heartbeatHandler, &domain::HeartbeatHandler::processMessage);
    // heartbeat отправляем на все доступные каналы связи 
    QObject::connect(&heartbeatHandler, &domain::HeartbeatHandler::sendMessage,
                     &communicator, &domain::MavLinkCommunicator::sendMessageOnAllLinks);

    // Настройки UDP через localhost
    domain::UdpLink link(14550, QString("127.0.0.1"), 14551);
    communicator.addLink(&link, MAVLINK_COMM_0);
    link.up();

    return app.exec();
}

int main(int argc, char* argv[])
{
    QCoreApplication app(argc, argv);

    // Для автопилота по-умолчанию sysid=1
    domain::MavLinkCommunicator communicator(1, 0);

    // Тип системы - самолёт с фиксированным крылом
    domain::HeartbeatHandler heartbeatHandler(MAV_TYPE_FIXED_WING);
    QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived,
                     &heartbeatHandler, &domain::HeartbeatHandler::processMessage);
    // heartbeat отправляем на все доступные каналы связи 
    QObject::connect(&heartbeatHandler, &domain::HeartbeatHandler::sendMessage,
                     &communicator, &domain::MavLinkCommunicator::sendMessageOnAllLinks);

    // Настройки UDP через localhost
    domain::UdpLink link(14551, QString("127.0.0.1"), 14550);
    communicator.addLink(&link, MAVLINK_COMM_0);
    link.up();

    return app.exec();
}