zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

QGC4.4
Lorenz Meier 13 years ago
parent
cb2dec430d
commit
e70c38bfe5
5 changed files with 80 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 60
      src/comm/QGCXPlaneLink.cc
  2. 3
      src/comm/QGCXPlaneLink.h
  3. 4
      src/uas/UAS.cc
  4. 48
      src/ui/QGCVehicleConfig.cc
  5. 3
      src/ui/QGCVehicleConfig.h

60
src/comm/QGCXPlaneLink.cc
Unescape View File

@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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

3
src/comm/QGCXPlaneLink.h
Unescape View File

@ -112,6 +112,9 @@ protected: @@ -112,6 +112,9 @@ protected:
QProcess* process;
QProcess* terraSync;
bool gpsReceived;
bool attitudeReceived;
void setName(QString name);
};

4
src/uas/UAS.cc
Unescape View File

@ -2405,12 +2405,12 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double @@ -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());
}

48
src/ui/QGCVehicleConfig.cc
Unescape View File

@ -3,6 +3,7 @@ @@ -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) : @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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;
}
}

3
src/ui/QGCVehicleConfig.h
Unescape View File

@ -2,6 +2,7 @@ @@ -2,6 +2,7 @@
#define QGCVEHICLECONFIG_H
#include <QWidget>
#include <QTimer>
#include "UASInterface.h"
@ -60,6 +61,7 @@ protected: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save