Skip to content

Q
qgroundcontrol
Commit 0bc63269 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4025 from Aerovinci/hil_actuator_controls_pr 

Add support for HIL_ACTUATOR_CONTROLS with X-Plane HITL
parents f1bd234d 01d4d0b2
Hide whitespace changes
Inline Side-by-side
Showing with 276 additions and 23 deletions
v1.0 @ e93ac629
Subproject commit 28ea42ccb6ab1d764cacf3f0f55358dc3c66faa9
Subproject commit e93ac62981a338a7c823364e7c4ff1077e3f8fc1
src/Vehicle/Vehicle.cc
View file @ 0bc63269
......@@ -469,6 +469,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_WIND_COV:
_handleWindCov(message);
break;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
_handleHilActuatorControls(message);
break;
// Following are ArduPilot dialect messages
......@@ -499,6 +502,30 @@ void Vehicle::_handleAutopilotVersion(mavlink_message_t& message)
}
}
void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
{
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);
}
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
mavlink_command_ack_t ack;
......
src/Vehicle/Vehicle.h
View file @ 0bc63269
......@@ -590,6 +590,8 @@ signals:
void armedChanged(bool armed);
void flightModeChanged(const QString& flightMode);
void hilModeChanged(bool hilMode);
/** @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 connectionLostChanged(bool connectionLost);
void connectionLostEnabledChanged(bool connectionLostEnabled);
void autoDisconnectChanged(bool autoDisconnectChanged);
......@@ -683,6 +685,7 @@ private:
void _handleExtendedSysState(mavlink_message_t& message);
void _handleCommandAck(mavlink_message_t& message);
void _handleAutopilotVersion(mavlink_message_t& message);
void _handleHilActuatorControls(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg);
void _geoFenceManagerError(int errorCode, const QString& errorMsg);
void _mapTrajectoryStart(void);
......
src/comm/QGCXPlaneLink.cc
View file @ 0bc63269
......@@ -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, &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);
......@@ -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,117 @@ 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);
sendDataRef("sim/flightmodel/controls/wing1l_ail2def", ctl_12 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing1r_ail2def", ctl_13 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2l_ail2def", ctl_14 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2r_ail2def", ctl_15 * 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 +1124,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));
}
src/comm/QGCXPlaneLink.h
View file @ 0bc63269
......@@ -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
src/ui/QGCHilXPlaneConfiguration.cc
View file @ 0bc63269
......@@ -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<void (QGCHilLink::*)(int)>(&QGCHilLink::versionChanged),
this, &QGCHilXPlaneConfiguration::setVersion);
......
src/ui/QGCHilXPlaneConfiguration.ui
View file @ 0bc63269
......@@ -6,14 +6,14 @@
<rect>
<x>0</x>
<y>0</y>
<width>226</width>
<width>269</width>
<height>150</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0" columnstretch="20,0,0">
<layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,0,0,0" columnstretch="20,0,0">
<property name="leftMargin">
<number>0</number>
</property>
......@@ -26,16 +26,11 @@
<property name="bottomMargin">
<number>0</number>
</property>
<item row="0" column="1" colspan="2">
<widget class="QComboBox" name="hostComboBox">
<property name="editable">
<bool>true</bool>
<item row="1" column="1" colspan="2">
<widget class="QPushButton" name="startButton">
<property name="text">
<string>Start</string>
</property>
<item>
<property name="text">
<string>127.0.0.1:49000</string>
</property>
</item>
</widget>
</item>
<item row="0" column="0">
......@@ -45,14 +40,7 @@
</property>
</widget>
</item>
<item row="3" column="0" colspan="3">
<widget class="QCheckBox" name="sensorHilCheckBox">
<property name="text">
<string>Enable sensor level HIL</string>
</property>
</widget>
</item>
<item row="4" column="0" colspan="3">
<item row="7" column="0" colspan="3">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
......@@ -65,10 +53,35 @@
</property>
</spacer>
</item>
<item row="1" column="1" colspan="2">
<widget class="QPushButton" name="startButton">
<item row="3" column="0" colspan="3">
<widget class="QCheckBox" name="sensorHilCheckBox">
<property name="text">
<string>Start</string>
<string>Enable sensor level HIL</string>
</property>
</widget>
</item>
<item row="0" column="1" colspan="2">
<widget class="QComboBox" name="hostComboBox">
<property name="editable">
<bool>true</bool>
</property>
<item>
<property name="text">
<string>127.0.0.1:49000</string>
</property>
</item>
</widget>
</item>
<item row="4" column="0" colspan="3">
<widget class="QCheckBox" name="useHilActuatorControlsCheckBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Use newer actuator format</string>
</property>
</widget>
</item>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment