diff --git a/libs/mavlink/include/mavlink/v1.0 b/libs/mavlink/include/mavlink/v1.0 index 28ea42ccb6ab1d764cacf3f0f55358dc3c66faa9..621bb1f70d17548b3f159fb30514023960be41f1 160000 --- a/libs/mavlink/include/mavlink/v1.0 +++ b/libs/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 28ea42ccb6ab1d764cacf3f0f55358dc3c66faa9 +Subproject commit 621bb1f70d17548b3f159fb30514023960be41f1 diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index c3acf9761096740d9ae743c6a9a5bc81e7edf519..74838577fd8407d2f3df2afdf2a2c4e8feaf6527 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -49,6 +49,7 @@ QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()), simUpdateHz(0), _sensorHilEnabled(true), + _useHilActuatorControls(true), _should_exit(false) { // We're doing it wrong - because the Qt folks got the API wrong: @@ -88,6 +89,7 @@ void QGCXPlaneLink::loadSettings() setVersion(settings.value("XPLANE_VERSION", 10).toInt()); selectAirframe(settings.value("AIRFRAME", "default").toString()); _sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool(); + _useHilActuatorControls = settings.value("ACTUATOR_HIL", _useHilActuatorControls).toBool(); settings.endGroup(); } @@ -100,6 +102,7 @@ void QGCXPlaneLink::storeSettings() settings.setValue("XPLANE_VERSION", xPlaneVersion); settings.setValue("AIRFRAME", airframeName); settings.setValue("SENSOR_HIL", _sensorHilEnabled); + settings.setValue("ACTUATOR_HIL", _useHilActuatorControls); settings.endGroup(); } @@ -136,6 +139,23 @@ void QGCXPlaneLink::setVersion(unsigned int version) if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); } +void QGCXPlaneLink::enableHilActuatorControls(bool enable) +{ + if (enable != _useHilActuatorControls) { + _useHilActuatorControls = enable; + } + + /* Only use override for new message and specific airframes */ + MAV_TYPE type = _vehicle->vehicleType(); + float value = 0.0f; + if (type == MAV_TYPE_VTOL_RESERVED2) { + value = (enable ? 1.0f : 0.0f); + } + + sendDataRef("sim/operation/override/override_control_surfaces", value); + emit useHilActuatorControlsChanged(enable); +} + /** * @brief Runs the thread @@ -170,6 +190,7 @@ void QGCXPlaneLink::run() QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes); connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection); + connect(_vehicle->uas(), &UAS::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); @@ -218,6 +239,9 @@ void QGCXPlaneLink::run() 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) { @@ -338,6 +362,11 @@ void QGCXPlaneLink::setRemoteHost(const QString& newHost) void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) { + /* Only use HIL_CONTROL when the checkbox is unchecked */ + if (_useHilActuatorControls) { + //qDebug() << "received HIL_CONTROL but not using it"; + return; + } #pragma pack(push, 1) struct payload { char b[5]; @@ -400,6 +429,113 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch } } +void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode) +{ + if (!_useHilActuatorControls) { + //qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it"; + return; + } + + Q_UNUSED(time); + Q_UNUSED(flags); + Q_UNUSED(mode); + Q_UNUSED(ctl_12); + Q_UNUSED(ctl_13); + Q_UNUSED(ctl_14); + Q_UNUSED(ctl_15); + + #pragma pack(push, 1) + struct payload { + char b[5]; + int index; + float f[8]; + } p; + #pragma pack(pop) + + p.b[0] = 'D'; + p.b[1] = 'A'; + p.b[2] = 'T'; + p.b[3] = 'A'; + p.b[4] = '\0'; + + /* Initialize with zeroes */ + memset(p.f, 0, sizeof(p.f)); + + switch (_vehicle->vehicleType()) { + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + { + p.f[0] = ctl_0; ///< X-Plane Engine 1 + p.f[1] = ctl_1; ///< X-Plane Engine 2 + p.f[2] = ctl_2; ///< X-Plane Engine 3 + p.f[3] = ctl_3; ///< X-Plane Engine 4 + p.f[4] = ctl_4; ///< X-Plane Engine 5 + p.f[5] = ctl_5; ///< X-Plane Engine 6 + p.f[6] = ctl_6; ///< X-Plane Engine 7 + p.f[7] = ctl_7; ///< X-Plane Engine 8 + + /* Direct throttle control */ + p.index = 25; + writeBytesSafe((const char*)&p, sizeof(p)); + break; + } + case MAV_TYPE_VTOL_RESERVED2: + { + /** + * Tailsitter with four control flaps and eight motors. + */ + + /* Throttle channels */ + p.f[0] = ctl_0; + p.f[1] = ctl_1; + p.f[2] = ctl_2; + p.f[3] = ctl_3; + p.f[4] = ctl_4; + p.f[5] = ctl_5; + p.f[6] = ctl_6; + p.f[7] = ctl_7; + p.index = 25; + writeBytesSafe((const char*)&p, sizeof(p)); + + /* Control individual actuators */ + float max_surface_deflection = 30.0f; // Degrees + sendDataRef("sim/flightmodel/controls/wing1l_ail1def", ctl_8 * max_surface_deflection); + sendDataRef("sim/flightmodel/controls/wing1r_ail1def", ctl_9 * max_surface_deflection); + sendDataRef("sim/flightmodel/controls/wing2l_ail1def", ctl_10 * max_surface_deflection); + sendDataRef("sim/flightmodel/controls/wing2r_ail1def", ctl_11 * max_surface_deflection); + + break; + } + default: + { + /* direct pass-through, normal fixed-wing. */ + p.f[0] = -ctl_1; ///< X-Plane Elevator + p.f[1] = ctl_0; ///< X-Plane Aileron + p.f[2] = ctl_2; ///< X-Plane Rudder + + /* Send to group 8, which equals manual controls */ + p.index = 8; + writeBytesSafe((const char*)&p, sizeof(p)); + + /* Send throttle to all eight motors */ + p.index = 25; + p.f[0] = ctl_3; + p.f[1] = ctl_3; + p.f[2] = ctl_3; + p.f[3] = ctl_3; + p.f[4] = ctl_3; + p.f[5] = ctl_3; + p.f[6] = ctl_3; + p.f[7] = ctl_3; + writeBytesSafe((const char*)&p, sizeof(p)); + break; + } + + } + +} + Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) { double c__ = cos(yaw); double _c_ = cos(pitch); @@ -984,3 +1120,39 @@ void QGCXPlaneLink::setName(QString name) this->name = name; // emit nameChanged(this->name); } + +void QGCXPlaneLink::sendDataRef(QString ref, float value) +{ + #pragma pack(push, 1) + struct payload { + char b[5]; + float value; + char name[500]; + } dref; + #pragma pack(pop) + + dref.b[0] = 'D'; + dref.b[1] = 'R'; + dref.b[2] = 'E'; + dref.b[3] = 'F'; + dref.b[4] = '0'; + + /* Set value */ + dref.value = value; + + /* Fill name with zeroes */ + memset(dref.name, 0, sizeof(dref.name)); + + /* Set dref name */ + + /* Send command */ + QByteArray ba = ref.toUtf8(); + if (ba.length() > 500) { + return; + } + + for (int i = 0; i < ba.length(); i++) { + dref.name[i] = ba.at(i); + } + writeBytesSafe((const char*)&dref, sizeof(dref)); +} diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index a51bab9294df2df6a6a7fe836cb5294b87478f24..e6f9222107cd92a49bd94eae31fdfdde706c8a9f 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -92,6 +92,14 @@ public: return _sensorHilEnabled; } + bool useHilActuatorControls() { + return _useHilActuatorControls; + } + +signals: + /** @brief Sensor leve HIL state changed */ + void useHilActuatorControlsChanged(bool enabled); + public slots: // void setAddress(QString address); void setPort(int port); @@ -99,6 +107,25 @@ public slots: void setRemoteHost(const QString& host); /** @brief Send new control states to the simulation */ void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); + /** @brief Send new control commands to the simulation */ + void updateActuatorControls(quint64 time, quint64 flags, + float ctl_0, + float ctl_1, + float ctl_2, + float ctl_3, + float ctl_4, + float ctl_5, + float ctl_6, + float ctl_7, + float ctl_8, + float ctl_9, + float ctl_10, + float ctl_11, + float ctl_12, + float ctl_13, + float ctl_14, + float ctl_15, + quint8 mode); /** @brief Set the simulator version as text string */ void setVersion(const QString& version); /** @brief Set the simulator version as integer */ @@ -110,6 +137,8 @@ public slots: emit sensorHilChanged(enable); } + void enableHilActuatorControls(bool enable); + void processError(QProcess::ProcessError err); void readBytes(); @@ -199,9 +228,11 @@ protected: quint64 simUpdateLastGroundTruth; float simUpdateHz; bool _sensorHilEnabled; + bool _useHilActuatorControls; bool _should_exit; void setName(QString name); + void sendDataRef(QString ref, float value); }; #endif // QGCXPLANESIMULATIONLINK_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 32b74d9d0b1aab91e5f78e41499f31addfa245ee..8b760d2c72402ad6d66164603bd20bfdab5fb2bb 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -429,6 +429,30 @@ void UAS::receiveMessage(mavlink_message_t message) emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); } break; + case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: + { + mavlink_hil_actuator_controls_t hil; + mavlink_msg_hil_actuator_controls_decode(&message, &hil); + emit hilActuatorControlsChanged(hil.time_usec, hil.flags, + hil.controls[0], + hil.controls[1], + hil.controls[2], + hil.controls[3], + hil.controls[4], + hil.controls[5], + hil.controls[6], + hil.controls[7], + hil.controls[8], + hil.controls[9], + hil.controls[10], + hil.controls[11], + hil.controls[12], + hil.controls[13], + hil.controls[14], + hil.controls[15], + hil.mode); + } + break; case MAVLINK_MSG_ID_VFR_HUD: { mavlink_vfr_hud_t hud; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 1e93a06702404203b6b6e57469d983edf5c0dcf6..b77a1b1ba2c2fd4510e708b04f59effb96beb50b 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -554,6 +554,8 @@ signals: void imageReady(UASInterface* uas); /** @brief HIL controls have changed */ void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); + /** @brief HIL actuator controls (replaces HIL controls) */ + void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode); void localXChanged(double val,QString name); void localYChanged(double val,QString name); diff --git a/src/ui/QGCHilXPlaneConfiguration.cc b/src/ui/QGCHilXPlaneConfiguration.cc index 1709e11b9902e00278e1319507fbd34e0c383636..bc0bd438566fe239d28621dc4c081dbb2abd3c63 100644 --- a/src/ui/QGCHilXPlaneConfiguration.cc +++ b/src/ui/QGCHilXPlaneConfiguration.cc @@ -35,8 +35,11 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilCon // 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(&QGCHilLink::versionChanged), this, &QGCHilXPlaneConfiguration::setVersion); diff --git a/src/ui/QGCHilXPlaneConfiguration.ui b/src/ui/QGCHilXPlaneConfiguration.ui index 1f5007d7ff4b24d56987604aa3d6f8dadaa3dd4f..89b1d852301e04a99bf01dc0a65bb66f70e67669 100644 --- a/src/ui/QGCHilXPlaneConfiguration.ui +++ b/src/ui/QGCHilXPlaneConfiguration.ui @@ -6,14 +6,14 @@ 0 0 - 226 + 269 150 Form - + 0 @@ -26,16 +26,11 @@ 0 - - - - true + + + + Start - - - 127.0.0.1:49000 - - @@ -45,14 +40,7 @@ - - - - Enable sensor level HIL - - - - + Qt::Vertical @@ -65,10 +53,35 @@ - - + + - Start + Enable sensor level HIL + + + + + + + true + + + + 127.0.0.1:49000 + + + + + + + + + 0 + 0 + + + + Use newer actuator format