Skip to content

Q
qgroundcontrol
Commit e70c38bf authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Fully hooked up HIL and vehicle config, time to flesh it out

parent cb2dec43
Hide whitespace changes
Inline Side-by-side
Showing with 80 additions and 38 deletions
src/comm/QGCXPlaneLink.cc
View file @ e70c38bf
......@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCXPlaneLink.h"
#include "QGC.h"
#include <QHostInfo>
#include "UAS.h"
#include "MainWindow.h"
QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
......@@ -199,9 +200,9 @@ void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
ascii.append(219);
}
}
qDebug() << "Sent" << size << "bytes to" << remoteHost.toString() << ":" << remotePort << "data:";
qDebug() << bytes;
qDebug() << "ASCII:" << ascii;
//qDebug() << "Sent" << size << "bytes to" << remoteHost.toString() << ":" << remotePort << "data:";
//qDebug() << bytes;
//qDebug() << "ASCII:" << ascii;
#endif
if (connectState && socket) socket->writeDatagram(data, size, remoteHost, remotePort);
}
......@@ -232,7 +233,7 @@ void QGCXPlaneLink::readBytes()
// XPlane always has 5 bytes header: 'DATA@'
unsigned nsegs = (s-5)/36;
qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
//qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
#pragma pack(push, 1)
struct payload {
......@@ -248,6 +249,8 @@ void QGCXPlaneLink::readBytes()
float airspeed;
float groundspeed;
float man_roll, man_pitch, man_yaw;
if (data[0] == 'D' &&
data[1] == 'A' &&
data[2] == 'T' &&
......@@ -262,24 +265,30 @@ void QGCXPlaneLink::readBytes()
if (p.index == 3)
{
//qDebug() << "SPEEDS:" << p.f[0] << p.f[1] << p.f[2];
airspeed = p.f[6] * 0.44704f;
groundspeed = p.f[7] * 0.44704;
qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
}
else if (p.index == 8)
{
//qDebug() << "MAN:" << p.f[0] << p.f[3] << p.f[7];
man_roll = p.f[0];
man_pitch = p.f[1];
man_yaw = p.f[2];
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6);
}
else if (p.index == 16)
{
qDebug() << "ANG VEL:" << p.f[0] << p.f[3] << p.f[7];
//qDebug() << "ANG VEL:" << p.f[0] << p.f[3] << p.f[7];
rollspeed = p.f[2];
pitchspeed = p.f[1];
yawspeed = p.f[0];
}
else if (p.index == 17)
{
qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
//qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
// XXX Feeding true heading - might need fix
pitch = (p.f[0] - 180.0f) / 180.0f * M_PI;
roll = (p.f[1] - 180.0f) / 180.0f * M_PI;
......@@ -292,30 +301,30 @@ void QGCXPlaneLink::readBytes()
// }
else if (p.index == 20)
{
qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
//qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
lat = p.f[0];
lon = p.f[1];
alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
}
else if (p.index == 12)
{
qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
//qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
}
else if (p.index == 25)
{
qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
//qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
}
else if (p.index == 0)
{
qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6];
//qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6];
}
else if (p.index == 11)
{
qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
//qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
}
else
{
qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
//qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
}
}
}
......@@ -360,12 +369,19 @@ bool QGCXPlaneLink::disconnectSimulation()
{
if (!connectState) return true;
connectState = false;
if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)),
this, SLOT(processError(QProcess::ProcessError)));
if (mav)
{
disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
{
uas->stopHil();
}
}
if (process)
......@@ -387,8 +403,6 @@ bool QGCXPlaneLink::disconnectSimulation()
socket = NULL;
}
connectState = false;
emit simulationDisconnected();
emit simulationConnected(false);
return !connectState;
......@@ -402,6 +416,8 @@ bool QGCXPlaneLink::disconnectSimulation()
bool QGCXPlaneLink::connectSimulation()
{
if (!mav) return false;
if (connectState) return false;
socket = new QUdpSocket(this);
connectState = socket->bind(localHost, localPort);
if (!connectState) return false;
......@@ -414,9 +430,11 @@ bool QGCXPlaneLink::connectSimulation()
connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
// XXX ugly hack to set MAVLINK_MODE_HIL_FLAG_ENABLED
// without pulling MAVLINK dependencies in here
mav->setMode(32);
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
{
uas->startHil();
}
// XXX This will later be enabled to start X-Plane from within QGroundControl with the right arguments
......
src/comm/QGCXPlaneLink.h
View file @ e70c38bf
......@@ -112,6 +112,9 @@ protected:
QProcess* process;
QProcess* terraSync;
bool gpsReceived;
bool attitudeReceived;
void setName(QString name);
};
......
src/uas/UAS.cc
View file @ e70c38bf
......@@ -2405,12 +2405,12 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
manualThrust = thrust * thrustScaling;
// If system has manual inputs enabled and is armed
if((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY))
if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
{
mavlink_message_t message;
mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
sendMessage(message);
qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
//qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
}
......
src/ui/QGCVehicleConfig.cc
View file @ e70c38bf
......@@ -3,6 +3,7 @@
#include <QTimer>
#include "QGCVehicleConfig.h"
#include "UASManager.h"
#include "QGC.h"
#include "ui_QGCVehicleConfig.h"
......@@ -20,6 +21,19 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
connect(ui->rcCalibrationButton, SIGNAL(clicked(bool)), this, SLOT(toggleCalibrationRC(bool)));
connect(ui->storeButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
setActiveUAS(UASManager::instance()->getActiveUAS());
for (int i = 0; i < chanMax; i++)
{
rcValue[i] = 1500;
}
updateTimer.setInterval(150);
connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateView()));
updateTimer.start();
}
QGCVehicleConfig::~QGCVehicleConfig()
......@@ -68,7 +82,10 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
// Connect new system
mav = active;
connect(active, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*, QString,QString)));
connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
SLOT(remoteControlChannelRawChanged(int,float)));
connect(active, SIGNAL(parameterChanged(int,int,QString,QVariant)), this,
SLOT(parameterChanged(int,int,QString,QVariant)));
}
void QGCVehicleConfig::resetCalibrationRC()
......@@ -153,7 +170,7 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
if (chan == rcMapping[0])
{
// ROLL
if (rcRoll > rcTrim[chan])
if (rcRoll >= rcTrim[chan])
{
rcRoll = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
}
......@@ -162,12 +179,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
rcRoll = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
}
rcValue[0] = val;
rcRoll = qBound(-1.0f, rcRoll, 1.0f);
}
else if (chan == rcMapping[1])
{
// PITCH
if (rcPitch > rcTrim[chan])
if (rcPitch >= rcTrim[chan])
{
rcPitch = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
}
......@@ -175,13 +193,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
{
rcPitch = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
}
rcValue[1] = val;
rcPitch = qBound(-1.0f, rcPitch, 1.0f);
}
else if (chan == rcMapping[2])
{
// YAW
if (rcYaw > rcTrim[chan])
if (rcYaw >= rcTrim[chan])
{
rcYaw = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
}
......@@ -189,13 +207,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
{
rcYaw = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
}
rcValue[2] = val;
rcYaw = qBound(-1.0f, rcYaw, 1.0f);
}
else if (chan == rcMapping[3])
{
// THROTTLE
if (rcThrottle > rcTrim[chan])
if (rcThrottle >= rcTrim[chan])
{
rcThrottle = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
}
......@@ -203,13 +221,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
{
rcThrottle = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
}
rcValue[3] = val;
rcThrottle = qBound(-1.0f, rcThrottle, 1.0f);
}
else if (chan == rcMapping[4])
{
// MODE SWITCH
if (rcMode > rcTrim[chan])
if (rcMode >= rcTrim[chan])
{
rcMode = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
}
......@@ -217,7 +235,7 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
{
rcMode = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
}
rcValue[4] = val;
rcMode = qBound(-1.0f, rcMode, 1.0f);
}
else if (chan == rcMapping[5])
......@@ -238,7 +256,7 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
changed = true;
qDebug() << "RC CHAN:" << chan << "PPM:" << val;
//qDebug() << "RC CHAN:" << chan << "PPM:" << val;
}
void QGCVehicleConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
......@@ -293,10 +311,10 @@ void QGCVehicleConfig::updateView()
{
if (changed)
{
ui->rollSlider->setValue(rcRoll);
ui->pitchSlider->setValue(rcPitch);
ui->yawSlider->setValue(rcYaw);
ui->throttleSlider->setValue(rcThrottle);
ui->rollSlider->setValue(rcValue[0]);
ui->pitchSlider->setValue(rcValue[1]);
ui->yawSlider->setValue(rcValue[2]);
ui->throttleSlider->setValue(rcValue[3]);
changed = false;
}
}
src/ui/QGCVehicleConfig.h
View file @ e70c38bf
......@@ -2,6 +2,7 @@
#define QGCVEHICLECONFIG_H
#include <QWidget>
#include <QTimer>
#include "UASInterface.h"
......@@ -60,6 +61,7 @@ protected:
int rcTrim[chanMax]; ///< Zero-position (center for roll/pitch/yaw, 0 throttle for throttle)
int rcMapping[chanMax]; ///< PWM to function mappings
bool rcRev[chanMax]; ///< Channel reverse
int rcValue[chanMax]; ///< Last values
float rcRoll; ///< PPM input channel used as roll control input
float rcPitch; ///< PPM input channel used as pitch control input
float rcYaw; ///< PPM input channel used as yaw control input
......@@ -70,6 +72,7 @@ protected:
float rcAux3; ///< PPM input channel used as aux 1 input
bool rcCalChanged; ///< Set if the calibration changes (and needs to be written)
bool changed; ///< Set if any of the input data changed
QTimer updateTimer; ///< Controls update intervals
private:
Ui::QGCVehicleConfig *ui;
......
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