GQC的HIL启动流程

最新推荐文章于 2024-10-11 07:04:50 发布

wangrui_cn

最新推荐文章于 2024-10-11 07:04:50 发布

阅读量862

点赞数

分类专栏： QGC 文章标签： qt

本文链接：https://blog.csdn.net/wangrui_cn/article/details/119938177

版权

QGC 专栏收录该内容

1 篇文章

订阅专栏

本文详细解析了QGC 3.5版本中HILConfig dock widget的创建过程，涉及MainWindow的初始化、MultiVehicleDockWidget的管理，以及如何通过QGCHilConfiguration连接和控制XPlane仿真。重点展示了HIL启动、状态切换和模拟器配置的实现细节。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

QGC的HIL启动流程（V3.5）

main.cc打开QGCApplication：

    QGCApplication* app = new QGCApplication(argc, argv, runUnitTests);

QGCApplication.cc打开MainWindow.cc:

MainWindow* mainWindow = MainWindow::_create();

MainWindow.cc:

static const char *rgDockWidgetNames[] = {
    "MAVLink Inspector",
    "Custom Command",
    "Onboard Files",
    "HIL Config",
    "Analyze"
};

/// Creates the specified inner dock widget and adds to the QDockWidget
bool MainWindow::_createInnerDockWidget(const QString& widgetName)
{
    QGCDockWidget* widget = nullptr;
    QAction *action = _mapName2Action[widgetName];
    if(action) {
        switch(action->data().toInt()) {
            case MAVLINK_INSPECTOR:
                widget = new QGCMAVLinkInspector(widgetName, action, qgcApp()->toolbox()->mavlinkProtocol(),this);
                break;
            case CUSTOM_COMMAND:
                widget = new CustomCommandWidget(widgetName, action, this);
                break;
            case ONBOARD_FILES:
                widget = new QGCUASFileViewMulti(widgetName, action, this);
                break;
            case HIL_CONFIG:
                widget = new HILDockWidget(widgetName, action, this);
                break;
            case ANALYZE:
                widget = new Linecharts(widgetName, action, _mavLinkDecoderInstance(), this);
                break;
        }
        if(widget) {
            _mapName2DockWidget[widgetName] = widget;
        }
    }
    return widget != nullptr;
}

MainWindow.ui定义的菜单如下：
在这里插入图片描述

其中的“Widgets"的二级菜单如下：
在这里插入图片描述

点击”HIL Config“之后，HILDockWidget生成一个新的widget。它用到了MultiVehicleDockWidget和QGCHilConfiguration。

HILDockWidget.cc文件如下：

#include "HILDockWidget.h"
#include "QGCHilConfiguration.h"

HILDockWidget::HILDockWidget(const QString& title, QAction* action, QWidget *parent)
    : MultiVehicleDockWidget(title, action, parent)
{
    init();
    
    loadSettings();
}

HILDockWidget::~HILDockWidget()
{

}

QWidget* HILDockWidget::_newVehicleWidget(Vehicle* vehicle, QWidget* parent)
{
    return new QGCHilConfiguration(vehicle, parent);
}

MultiVehicleDockWidget中的init()为：

void MultiVehicleDockWidget::init(void)
{
    QmlObjectListModel* vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles();

    for (int i=0; i<vehicles->count(); i++) {
        _vehicleAdded(qobject_cast<Vehicle*>(vehicles->get(i)));
    }

    if (qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) {
        _activeVehicleChanged(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle());
    }
}

其中的_activeVehicleChanged是个槽函数：
（这里好像是把multiVehicleManager和UI关联起来的一个关键地方？）

connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &MultiVehicleDockWidget::_activeVehicleChanged);

void MultiVehicleDockWidget::_activeVehicleChanged(Vehicle* vehicle)
{
    if (vehicle) {
        int id = vehicle->id();
        
        if (!_vehicleWidgets.contains(id)) {
            _vehicleAdded(vehicle);
        }
        
        QWidget* vehicleWidget = _vehicleWidgets[id];
        _ui->stackedWidget->setCurrentWidget(vehicleWidget);
    }
}

其中的_vehicleAdded(vehicle)，调用HILDockWidget的虚函数_newVehicleWidget，在其中再调用QGCHilConfiguration，生成一个新的QWidget

void MultiVehicleDockWidget::_vehicleAdded(Vehicle* vehicle)
{
    int id = vehicle->id();
    
    if (!_vehicleWidgets.contains(id)) {
        QWidget* vehicleWidget = _newVehicleWidget(vehicle, _ui->stackedWidget);
        _vehicleWidgets[id] = vehicleWidget;
        _ui->stackedWidget->addWidget(vehicleWidget);
    }
}

QWidget* HILDockWidget::_newVehicleWidget(Vehicle* vehicle, QWidget* parent)
{
    return new QGCHilConfiguration(vehicle, parent);
}

QGCHilConfiguration.ui为：
在这里插入图片描述

如果选中”XPlane9或XPlane10，就把对应的vehicle->uas-> enablehilxplane(false),然后生成一个新的QGCHilXPlaneConfiguration对象，并生成新的UI窗口显示它：ui->simulatorConfigurationLayout->addWidget(hxpconf);

void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index)
{
    //clean up
    QLayoutItem *child;
    while ((child = ui->simulatorConfigurationLayout->takeAt(0)) != 0)
    {
        delete child->widget();
        delete child;
    }

    if(1 == index)
    {
        // Ensure the sim exists and is disabled
        _vehicle->uas()->enableHilFlightGear(false, "", true, this);
        QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(_vehicle, this);
        hfgconf->show();
        ui->simulatorConfigurationLayout->addWidget(hfgconf);
        QGCFlightGearLink* fg = dynamic_cast<QGCFlightGearLink*>(_vehicle->uas()->getHILSimulation());
        if (fg)
        {
            connect(fg, &QGCFlightGearLink::statusMessage, ui->statusLabel, &QLabel::setText);
        }

    }
    else if (2 == index || 3 == index)
    {
        // Ensure the sim exists and is disabled
        _vehicle->uas()->enableHilXPlane(false);
        QGCHilXPlaneConfiguration* hxpconf = new QGCHilXPlaneConfiguration(_vehicle->uas()->getHILSimulation(), this);
        hxpconf->show();
        ui->simulatorConfigurationLayout->addWidget(hxpconf);

        // Select correct version of XPlane
        QGCXPlaneLink* xplane = dynamic_cast<QGCXPlaneLink*>(_vehicle->uas()->getHILSimulation());
        if (xplane)
        {
            xplane->setVersion((index == 2) ? 10 : 9);
            connect(xplane, &QGCXPlaneLink::statusMessage, ui->statusLabel, &QLabel::setText);
        }
    }
// Disabling JSB Sim since its not well maintained,
// but as refactoring is pending we're not ditching the code yet
//    else if (4)
//    {
//        // Ensure the sim exists and is disabled
//        _vehicle->uas()->enableHilJSBSim(false, "");
//        QGCHilJSBSimConfiguration* hfgconf = new QGCHilJSBSimConfiguration(_vehicle, this);
//        hfgconf->show();
//        ui->simulatorConfigurationLayout->addWidget(hfgconf);
//        QGCJSBSimLink* jsb = dynamic_cast<QGCJSBSimLink*>(_vehicle->uas()->getHILSimulation());
//        if (jsb)
//        {
//            connect(jsb, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString)));
//        }
//    }
}

这里虽然enableHilXPlane(false)，但是最主要的功能是生成了一个QGCXPlaneLink对象待用：

void UAS::enableHilXPlane(bool enable)
{
    QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
    if (!link) {
        if (simulation) {
            stopHil();
            delete simulation;
        }
        simulation = new QGCXPlaneLink(_vehicle);

//        qDebug() <<"Entering enableHilXPlane."<<simulation->isConnected(); // WR revised

        float noise_scaler = 0.0001f;

        // WR revised
        noise_scaler = noise_scaler * 0.01f;

        xacc_var = noise_scaler * 0.2914f;
        yacc_var = noise_scaler * 0.2914f;
        zacc_var = noise_scaler * 0.9577f;
        rollspeed_var = noise_scaler * 0.8126f;
        pitchspeed_var = noise_scaler * 0.6145f;
        yawspeed_var = noise_scaler * 0.5852f;
        xmag_var = noise_scaler * 0.0786f;
        ymag_var = noise_scaler * 0.0566f;
        zmag_var = noise_scaler * 0.0333f;
        abs_pressure_var = noise_scaler * 0.5604f;
        diff_pressure_var = noise_scaler * 0.2604f;
        pressure_alt_var = noise_scaler * 0.5604f;
        temperature_var = noise_scaler * 0.7290f;
    }
    // Connect X-Plane Link
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}

QGCHilXPlaneConfiguration.ui为：

它的构造函数为：

QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilConfiguration *parent) :
    QWidget(parent),
    ui(new Ui::QGCHilXPlaneConfiguration)
{
    ui->setupUi(this);
    this->link = link;

    connect(ui->startButton, &QPushButton::clicked, this, &QGCHilXPlaneConfiguration::toggleSimulation);

    connect(ui->hostComboBox, static_cast<void (QComboBox::*)(const QString&)>(&QComboBox::activated),
            link, &QGCHilLink::setRemoteHost);

    connect(link, &QGCHilLink::remoteChanged, ui->hostComboBox, &QComboBox::setEditText);
    connect(link, &QGCHilLink::statusMessage, parent, &QGCHilConfiguration::receiveStatusMessage);

//    connect(mav->getHILSimulation(), SIGNAL(statusMessage(QString)), this, SLOT(receiveStatusMessage(QString)));
//    connect(ui->simComboBox, SIGNAL(activated(QString)), mav->getHILSimulation(), SLOT(setVersion(QString)));

    ui->startButton->setText(tr("Connect"));

    QGCXPlaneLink* xplane = dynamic_cast<QGCXPlaneLink*>(link);

    if (xplane)
    {
//        connect(ui->randomAttitudeButton, SIGNAL(clicked()), link, SLOT(setRandomAttitude()));
//        connect(ui->randomPositionButton, SIGNAL(clicked()), link, SLOT(setRandomPosition()));

        //ui->airframeComboBox->setCurrentIndex(link->getAirFrameIndex());
        //connect(ui->airframeComboBox, SIGNAL(activated(QString)), link, SLOT(selectAirframe(QString)));
        // XXX not implemented yet
        //ui->airframeComboBox->hide();
        ui->sensorHilCheckBox->setChecked(xplane->sensorHilEnabled());
        ui->useHilActuatorControlsCheckBox->setChecked(true);
        connect(xplane, &QGCXPlaneLink::sensorHilChanged, ui->sensorHilCheckBox, &QCheckBox::setChecked);
        connect(ui->sensorHilCheckBox, &QCheckBox::clicked, xplane, &QGCXPlaneLink::enableSensorHIL);
        connect(xplane, &QGCXPlaneLink::useHilActuatorControlsChanged, ui->useHilActuatorControlsCheckBox, &QCheckBox::setChecked);
        connect(ui->useHilActuatorControlsCheckBox, &QCheckBox::clicked, xplane, &QGCXPlaneLink::enableHilActuatorControls);

        connect(link, static_cast<void (QGCHilLink::*)(int)>(&QGCHilLink::versionChanged),
                this, &QGCHilXPlaneConfiguration::setVersion);
    }

    ui->hostComboBox->clear();
    ui->hostComboBox->addItem(link->getRemoteHost());


}

点击“Start”将调用以下代码：

void QGCHilXPlaneConfiguration::toggleSimulation(bool connect)
{
    if (!link) {
        return;
    }

    Q_UNUSED(connect);
    if (!link->isConnected())
    {
        link->setRemoteHost(ui->hostComboBox->currentText());
        link->connectSimulation();
        ui->startButton->setText(tr("Disconnect"));
    }
    else
    {
        link->disconnectSimulation();
        ui->startButton->setText(tr("Connect"));
    }
}

在构造这个对象时，定义了link=_vehicle->uas()->getHILSimulation()，它返回的是：QGCHilLink* simulation;
toggleSimulation最重要的代码是：link->connectSimulation();
它在QGCHilLink里定义，但是QGCXPlaneLink.h进行了重载：

bool QGCXPlaneLink::connectSimulation()
{
    if (connectState) {
        qDebug() << "Simulation already active";
    } else {
        qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
        // XXX Hack
        storeSettings();

        start(HighPriority);
    }

    return true;
}

- start(HighPriority);自动调用自己所在类的run()，在这里打开udp，进行数据的收发

void QGCXPlaneLink::run()
{
    if (!_vehicle) {
        emit statusMessage("No MAV present");
        return;
    }

    if (connectState) {
        emit statusMessage("Already connected");
        return;
    }

    socket = new QUdpSocket(this);
    socket->moveToThread(this);
    connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
    if (!connectState) {

        emit statusMessage("Binding socket failed!");

        socket->deleteLater();
        socket = NULL;
        return;
    }

    emit statusMessage(tr("Waiting for XPlane.."));

    QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);

    connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection);
    connect(_vehicle, &Vehicle::hilActuatorControlsChanged, this, &QGCXPlaneLink::updateActuatorControls, Qt::QueuedConnection);

    connect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth, Qt::QueuedConnection);
    connect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState, Qt::QueuedConnection);
    connect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps, Qt::QueuedConnection);
    connect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors, Qt::QueuedConnection);

    _vehicle->uas()->startHil();

#pragma pack(push, 1)
    struct iset_struct
    {
        char b[5];
        int index; // (0->20 in the list below)
        char str_ipad_them[16];
        char str_port_them[6];
        char padding[2];
        int use_ip;
    } ip; // to use this option, 0 not to.
#pragma pack(pop)

    ip.b[0] = 'I';
    ip.b[1] = 'S';
    ip.b[2] = 'E';
    ip.b[3] = 'T';
    ip.b[4] = '0';

    QList<QHostAddress> hostAddresses = QNetworkInterface::allAddresses();

    QString localAddrStr;
    QString localPortStr = QString("%1").arg(localPort);

    for (int i = 0; i < hostAddresses.size(); i++)
    {
        // Exclude loopback IPv4 and all IPv6 addresses
        if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":"))
        {
            localAddrStr = hostAddresses.at(i).toString();
            break;
        }
    }

    ip.index = 0;
    strncpy(ip.str_ipad_them, localAddrStr.toLatin1(), qMin((int)sizeof(ip.str_ipad_them), 16));
    strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6));
    ip.use_ip = 1;

    writeBytesSafe((const char*)&ip, sizeof(ip));

    /* Call function which makes sure individual control override is enabled/disabled */
    enableHilActuatorControls(_useHilActuatorControls);

    _should_exit = false;

    while(!_should_exit) {
        QCoreApplication::processEvents();
        QGC::SLEEP::msleep(5);
    }

    disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls);

    disconnect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth);
    disconnect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState);
    disconnect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps);
    disconnect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors);
    connectState = false;

    disconnect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);

    socket->close();
    socket->deleteLater();
    socket = NULL;

    emit simulationDisconnected();
    emit simulationConnected(false);
}

以上代码中，运行HIL的代码是：_vehicle->uas()->startHil()，它的代码是：

void UAS::startHil()
{
    if (hilEnabled) return;
    hilEnabled = true;
    sensorHil = false;
    _vehicle->setHilMode(true);
    qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    // Connect HIL simulation link
    simulation->connectSimulation();
}

它首先运行_vehicle->setHilMode(true);告诉mavlink要开始运行HIL了：

void Vehicle::setHilMode(bool hilMode)
{
    mavlink_message_t msg;

    uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_HIL;
    if (hilMode) {
        newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED;
    }

    mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
                                   _mavlink->getComponentId(),
                                   priorityLink()->mavlinkChannel(),
                                   &msg,
                                   id(),
                                   newBaseMode,
                                   _custom_mode);
    sendMessageOnLink(priorityLink(), msg);
}