Skip to content

Q
qgroundcontrol
Commit e6a125ee authored by Bart Slinger's avatar Bart Slinger
Browse files
Options

Add support for HIL_ACTUATOR_CONTROLS mavlink message

parent 99b298c6
Hide whitespace changes
Inline Side-by-side
Showing with 268 additions and 23 deletions
v1.0 @ 621bb1f7
Subproject commit 28ea42ccb6ab1d764cacf3f0f55358dc3c66faa9
Subproject commit 621bb1f70d17548b3f159fb30514023960be41f1
src/comm/QGCXPlaneLink.cc
View file @ e6a125ee
......@@ -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));
}
src/comm/QGCXPlaneLink.h
View file @ e6a125ee
......@@ -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/uas/UAS.cc
View file @ e6a125ee
......@@ -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;
......
src/uas/UAS.h
View file @ e6a125ee
......@@ -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);
......
src/ui/QGCHilXPlaneConfiguration.cc
View file @ e6a125ee
......@@ -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 @ e6a125ee
......@@ -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