Commit d422adad authored by pixhawk's avatar pixhawk

Merge branch 'master' of git@pixhawk.ethz.ch:qgroundcontrol

parents 91c1b826 f8a80de5
......@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
#include <QDebug>
#include <QTime>
#include <QApplication>
#include "MG.h"
#include "MAVLinkProtocol.h"
......@@ -56,7 +57,9 @@ This file is part of the PIXHAWK project
MAVLinkProtocol::MAVLinkProtocol() :
heartbeatTimer(new QTimer(this)),
heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
m_heartbeatsEnabled(false)
m_heartbeatsEnabled(false),
m_loggingEnabled(false),
m_logfile(NULL)
{
start(QThread::LowPriority);
// Start heartbeat timer, emitting a heartbeat at the configured rate
......@@ -77,12 +80,23 @@ MAVLinkProtocol::MAVLinkProtocol() :
MAVLinkProtocol::~MAVLinkProtocol()
{
if (m_logfile)
{
m_logfile->close();
delete m_logfile;
}
}
void MAVLinkProtocol::run()
{
}
QString MAVLinkProtocol::getLogfileName()
{
return QCoreApplication::applicationDirPath()+"/mavlink.log";
}
/**
......@@ -111,6 +125,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
if (decodeState == 1)
{
// Log data
if (m_loggingEnabled)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_to_send_buffer(buf, &message);
m_logfile->write((const char*) buf);
qDebug() << "WROTE LOGFILE";
}
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
......@@ -316,11 +339,32 @@ void MAVLinkProtocol::enableHeartbeats(bool enabled)
emit heartbeatChanged(enabled);
}
void MAVLinkProtocol::enableLogging(bool enabled)
{
if (enabled && !m_loggingEnabled)
{
m_logfile = new QFile(getLogfileName());
m_logfile->open(QIODevice::WriteOnly | QIODevice::Append);
}
else
{
m_logfile->close();
delete m_logfile;
m_logfile = NULL;
}
m_loggingEnabled = enabled;
}
bool MAVLinkProtocol::heartbeatsEnabled(void)
{
return m_heartbeatsEnabled;
}
bool MAVLinkProtocol::loggingEnabled(void)
{
return m_loggingEnabled;
}
/**
* The default rate is 1 Hertz.
*
......
......@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
#include <QMutex>
#include <QString>
#include <QTimer>
#include <QFile>
#include <QMap>
#include <QByteArray>
#include "ProtocolInterface.h"
......@@ -65,6 +66,10 @@ public:
int getHeartbeatRate();
/** @brief Get heartbeat state */
bool heartbeatsEnabled(void);
/** @brief Get logging state */
bool loggingEnabled(void);
/** @brief Get the name of the packet log file */
static QString getLogfileName();
public slots:
/** @brief Receive bytes from a communication interface */
......@@ -79,6 +84,9 @@ public slots:
/** @brief Enable / disable the heartbeat emission */
void enableHeartbeats(bool enabled);
/** @brief Enable/disable binary packet logging */
void enableLogging(bool enabled);
/** @brief Send an extra heartbeat to all connected units */
void sendHeartbeat();
......@@ -86,6 +94,8 @@ protected:
QTimer* heartbeatTimer; ///< Timer to emit heartbeats
int heartbeatRate; ///< Heartbeat rate, controls the timer interval
bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission
bool m_loggingEnabled; ///< Enable/disable packet logging
QFile* m_logfile; ///< Logfile
QMutex receiveMutex; ///< Mutex to protect receiveBytes function
int lastIndex[256][256];
int totalReceiveCounter;
......@@ -98,6 +108,8 @@ signals:
void messageReceived(LinkInterface* link, mavlink_message_t message);
/** @brief Emitted if heartbeat emission mode is changed */
void heartbeatChanged(bool heartbeats);
/** @brief Emitted if logging is started / stopped */
void loggingChanged(bool enabled);
};
#endif // MAVLINKPROTOCOL_H_
......@@ -38,6 +38,7 @@ This file is part of the PIXHAWK project
#include <QDebug>
#include "MG.h"
#include "LinkManager.h"
#include "MAVLinkProtocol.h"
#include "MAVLinkSimulationLink.h"
// MAVLINK includes
#include <mavlink.h>
......@@ -92,6 +93,10 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
maxTimeNoise = 0;
this->id = getNextLinkId();
LinkManager::instance()->add(this);
// Open packet log
mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
mavlinkLogFile->open(QIODevice::ReadOnly);
}
MAVLinkSimulationLink::~MAVLinkSimulationLink()
......@@ -121,6 +126,21 @@ void MAVLinkSimulationLink::run()
if (_isConnected)
{
mainloop();
// FIXME Hacky code to read from packet log file
// readyBufferMutex.lock();
// for (int i = 0; i < streampointer; i++)
// {
// readyBuffer.enqueue(*(stream + i));
// }
// readyBufferMutex.unlock();
emit bytesReady(this);
}
last = MG::TIME::getGroundTimeNow();
......
......@@ -98,6 +98,7 @@ protected:
QTimer* timer;
/** File which contains the input data (simulated robot messages) **/
QFile* simulationFile;
QFile* mavlinkLogFile;
QString simulationHeader;
/** File where the commands sent by the groundstation are stored **/
QFile* receiveFile;
......
......@@ -73,6 +73,8 @@ bool MAVLinkXMLParser::generate()
QList< QPair<QString, QString> > cFiles;
QString lcmStructDefs = "";
QString pureFileName;
// Run through root children
while(!n.isNull())
{
......@@ -103,6 +105,7 @@ bool MAVLinkXMLParser::generate()
{
QFileInfo rInfo(this->fileName);
fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName;
pureFileName = rInfo.baseName().split(".").first();
}
QFile file(fileName);
......@@ -365,6 +368,7 @@ bool MAVLinkXMLParser::generate()
// Mark all code as C code
mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n";
mainHeader += "\n#include \"protocol.h\"\n";
mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n";
QString includeLine = "#include \"%1\"\n";
QString mainHeaderName = "mavlink.h";
QString messagesDirName = "generated";
......
......@@ -22,10 +22,36 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
//qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid;
// Only compile this portion if matching MAVLink packets have been compiled
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Handle your special messages
switch (msg->msgid)
if (message.sysid == uasId)
{
QString uasState;
QString stateDescription;
QString patternPath;
switch (message.msgid)
{
case MAVLINK_MSG_ID_RAW_AUX:
{
mavlink_raw_aux_t raw;
mavlink_msg_raw_aux_decode(&message, &raw);
quint64 time = getUnixTime(0);
emit valueChanged(uasId, "Pressure", raw.baro, time);
emit valueChanged(uasId, "Temperature", raw.temp, time);
}
break;
case MAVLINK_MSG_ID_PATTERN_DETECTED:
{
QByteArray b;
b.resize(256);
mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
b.append('\0');
QString path = QString(b);
bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
}
break;
case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
{
mavlink_watchdog_heartbeat_t payload;
......@@ -103,10 +129,14 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Do nothing
break;
}
}
#endif
}
void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
mavlink_watchdog_command_t payload;
payload.target_system_id = uasId;
payload.watchdog_id = watchdogId;
......@@ -116,4 +146,5 @@ void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int c
mavlink_message_t msg;
mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload);
sendMessage(msg);
#endif
}
......@@ -267,15 +267,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
}
break;
case MAVLINK_MSG_ID_RAW_AUX:
{
mavlink_raw_aux_t raw;
mavlink_msg_raw_aux_decode(&message, &raw);
quint64 time = getUnixTime(0);
emit valueChanged(uasId, "Pressure", raw.baro, time);
emit valueChanged(uasId, "Temperature", raw.temp, time);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
......@@ -339,7 +330,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "lon", pos.lon, time);
emit valueChanged(uasId, "alt", pos.alt, time);
emit valueChanged(uasId, "speed", pos.v, time);
qDebug() << "GOT GPS RAW";
//qDebug() << "GOT GPS RAW";
emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
}
break;
......@@ -347,9 +338,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_gps_status_t pos;
mavlink_msg_gps_status_decode(&message, &pos);
for(int i = 0; i < pos.satellites_visible && i < sizeof(pos.satellite_used); i++)
for(int i = 0; i < (int)pos.satellites_visible; i++)
{
emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
}
}
break;
......@@ -408,17 +399,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit textMessageReceived(uasId, severity, text);
}
break;
case MAVLINK_MSG_ID_PATTERN_DETECTED:
{
QByteArray b;
b.resize(256);
mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
b.append('\0');
QString path = QString(b);
bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
}
break;
default:
{
if (!unknownPackets.contains(message.msgid))
......
......@@ -119,7 +119,7 @@ void HDDisplay::paintEvent(QPaintEvent * event)
Q_UNUSED(event);
//paintGL();
static quint64 interval = 0;
qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
//qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
interval = MG::TIME::getGroundTimeNow();
paintDisplay();
}
......
......@@ -40,9 +40,10 @@ This file is part of the PIXHAWK project
HSIDisplay::HSIDisplay(QWidget *parent) :
HDDisplay(NULL, parent),
gpsSatellites()
gpsSatellites(),
satellitesUsed(0)
{
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
}
void HSIDisplay::paintEvent(QPaintEvent * event)
......@@ -91,6 +92,9 @@ void HSIDisplay::paintDisplay()
drawCircle(vwidth/2.0f, vheight/2.0f, radius, 0.1f, ringColor, &painter);
}
// Draw center indicator
drawCircle(vwidth/2.0f, vheight/2.0f, 1.0f, 0.1f, ringColor, &painter);
drawGPS();
......@@ -133,6 +137,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
//disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
}
connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
// Now connect the new UAS
//if (this->uas != uas)
......@@ -143,40 +149,88 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
//}
}
void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used)
void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used)
{
Q_UNUSED(uasid);
//qDebug() << "UPDATED SATELLITE";
// If slot is empty, insert object
if (gpsSatellites.at(satid) == NULL)
if (gpsSatellites.contains(satid))
{
gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used));
gpsSatellites.value(satid)->update(satid, elevation, azimuth, snr, used);
}
else
{
// Satellite exists, update it
gpsSatellites.at(satid)->update(satid, azimuth, direction, snr, used);
gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used));
}
}
QColor HSIDisplay::getColorForSNR(float snr)
{
return QColor(200, 250, 200);
QColor color;
if (snr > 0 && snr < 30)
{
color = QColor(250, 10, 10);
}
else if (snr >= 30 && snr < 35)
{
color = QColor(230, 230, 10);
}
else if (snr >= 35 && snr < 40)
{
color = QColor(90, 200, 90);
}
else if (snr >= 40)
{
color = QColor(20, 200, 20);
}
else
{
color = QColor(180, 180, 180);
}
return color;
}
void HSIDisplay::drawGPS()
{
float xCenter = vwidth/2.0f;
float yCenter = vwidth/2.0f;
QPainter painter(this);
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
// Max satellite circle radius
const float margin = 0.2f; // 20% margin of total width on each side
const float margin = 0.15f; // 20% margin of total width on each side
float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
quint64 currTime = MG::TIME::getGroundTimeNowUsecs();
// Draw satellite labels
// QString label;
// label.sprintf("%05.1f", value);
// paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);
for (int i = 0; i < gpsSatellites.size(); i++)
QMapIterator<int, GPSSatellite*> i(gpsSatellites);
while (i.hasNext())
{
i.next();
GPSSatellite* sat = i.value();
// Check if update is not older than 5 seconds, else delete satellite
if (sat->lastUpdate + 1000000 < currTime)
{
// Delete and go to next satellite
gpsSatellites.remove(i.key());
if (i.hasNext())
{
GPSSatellite* sat = gpsSatellites.at(i);
i.next();
sat = i.value();
}
else
{
continue;
}
}
if (sat)
{
// Draw satellite
......@@ -195,10 +249,11 @@ void HSIDisplay::drawGPS()
painter.setPen(color);
painter.setBrush(brush);
float xPos = sin(sat->direction) * sat->azimuth * radius;
float yPos = cos(sat->direction) * sat->azimuth * radius;
float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
drawCircle(xPos, yPos, vwidth/10.0f, 1.0f, color, &painter);
drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
paintText(QString::number(sat->id), QColor(255, 255, 255), 2.9f, xPos+1.7f, yPos+2.0f, &painter);
}
}
}
......
......@@ -40,6 +40,7 @@ This file is part of the PIXHAWK project
#include <cmath>
#include "HDDisplay.h"
#include "MG.h"
class HSIDisplay : public HDDisplay {
Q_OBJECT
......@@ -49,6 +50,7 @@ public:
public slots:
void setActiveUAS(UASInterface* uas);
void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
void paintEvent(QPaintEvent * event);
protected slots:
......@@ -59,7 +61,6 @@ protected slots:
protected:
void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
static QColor getColorForSNR(float snr);
/**
......@@ -68,35 +69,39 @@ protected:
class GPSSatellite
{
public:
GPSSatellite(int id, float azimuth, float direction, float snr, bool used) :
GPSSatellite(int id, float elevation, float azimuth, float snr, bool used) :
id(id),
elevation(elevation),
azimuth(azimuth),
direction(direction),
snr(snr),
used(used)
used(used),
lastUpdate(MG::TIME::getGroundTimeNowUsecs())
{
}
void update(int id, float azimuth, float direction, float snr, bool used)
void update(int id, float elevation, float azimuth, float snr, bool used)
{
this->id = id;
this->elevation = elevation;
this->azimuth = azimuth;
this->direction = direction;
this->snr = snr;
this->used = used;
this->lastUpdate = MG::TIME::getGroundTimeNowUsecs();
}
int id;
float elevation;
float azimuth;
float direction;
float snr;
bool used;
quint64 lastUpdate;
friend class HSIDisplay;
};
QVector<GPSSatellite*> gpsSatellites;
QMap<int, GPSSatellite*> gpsSatellites;
unsigned int satellitesUsed;
private:
};
......
......@@ -11,9 +11,12 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget
// Connect actions
connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool)));
connect(m_ui->heartbeatCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableHeartbeats(bool)));
connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->loggingCheckBox, SLOT(setChecked(bool)));
connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableLogging(bool)));
// Initialize state
m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled());
m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled());
}
MAVLinkSettingsWidget::~MAVLinkSettingsWidget()
......
......@@ -24,6 +24,13 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="loggingCheckBox">
<property name="text">
<string>Log all MAVLink packets</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
......
......@@ -85,7 +85,7 @@ settings()
waypoints->setVisible(false);
info = new UASInfoWidget(this);
info->setVisible(false);
detection = new ObjectDetectionView("test", this);
detection = new ObjectDetectionView("patterns", this);
detection->setVisible(false);
hud = new HUD(640, 480, this);
hud->setVisible(false);
......@@ -107,9 +107,9 @@ settings()
acceptList->append("roll IMU");
acceptList->append("pitch IMU");
acceptList->append("yaw IMU");
acceptList->append("vx");
acceptList->append("vy");
acceptList->append("vz");
acceptList->append("rollspeed IMU");
acceptList->append("pitchspeed IMU");
acceptList->append("yawspeed IMU");
headDown1 = new HDDisplay(acceptList, this);
headDown1->setVisible(false);
......
......@@ -94,7 +94,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
{
//qDebug() << "REDETECTED";
/*
QList<QAction*> actions = m_ui->listWidget->actions();
// Find action and update it
foreach (QAction* act, actions)
......@@ -107,7 +106,9 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
act->setText(patternPath + separator + "(#" + QString::number(count) + ")" + separator + QString::number(confidence));
}
}
QPixmap image = QPixmap(patternFolder + "/" + patternPath);
QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/").last();
qDebug() << "Loading:" << filePath;
QPixmap image = QPixmap(filePath);
image = image.scaledToWidth(m_ui->imageLabel->width());
m_ui->imageLabel->setPixmap(image);
QString patternName = patternPath.split("//").last(); // Remove preceding folder names
......@@ -115,7 +116,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
// Set name and label
m_ui->nameLabel->setText(patternName);
*/
}
else
{
......
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