From e70c38bfe5405378d46acaeadfe22768d6cef0d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Sep 2012 00:24:36 +0200 Subject: [PATCH] Fully hooked up HIL and vehicle config, time to flesh it out --- src/comm/QGCXPlaneLink.cc | 60 +++++++++++++++++++++++++------------- src/comm/QGCXPlaneLink.h | 3 ++ src/uas/UAS.cc | 4 +-- src/ui/QGCVehicleConfig.cc | 48 ++++++++++++++++++++---------- src/ui/QGCVehicleConfig.h | 3 ++ 5 files changed, 80 insertions(+), 38 deletions(-) diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index cff35be9b1..b47f591b13 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCXPlaneLink.h" #include "QGC.h" #include +#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(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(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(mav); + if (uas) + { + uas->startHil(); + } // XXX This will later be enabled to start X-Plane from within QGroundControl with the right arguments diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index 2decbbb797..2e11ea2d0f 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -112,6 +112,9 @@ protected: QProcess* process; QProcess* terraSync; + bool gpsReceived; + bool attitudeReceived; + void setName(QString name); }; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index d18c8e3c57..e299fafba1 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -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()); } diff --git a/src/ui/QGCVehicleConfig.cc b/src/ui/QGCVehicleConfig.cc index a0012c8c4f..8926be3eef 100644 --- a/src/ui/QGCVehicleConfig.cc +++ b/src/ui/QGCVehicleConfig.cc @@ -3,6 +3,7 @@ #include #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; } } diff --git a/src/ui/QGCVehicleConfig.h b/src/ui/QGCVehicleConfig.h index 1d75e9af90..a9b09f8693 100644 --- a/src/ui/QGCVehicleConfig.h +++ b/src/ui/QGCVehicleConfig.h @@ -2,6 +2,7 @@ #define QGCVEHICLECONFIG_H #include +#include #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; -- GitLab