From a2a740ddb14c24573c3ad29fe3c7f86065e555a9 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Sun, 30 May 2010 17:16:08 +0200 Subject: [PATCH] Minor improvements and fixes, added a few new MAVLink packets --- src/Core.cc | 9 ++++----- src/GAudioOutput.cc | 2 ++ src/comm/MAVLinkSimulationLink.cc | 2 +- src/uas/UAS.cc | 40 +++++++++++++++++++++++++++++++++++---- src/ui/HDDisplay.cc | 12 +++++++----- src/ui/HDDisplay.h | 4 ++-- 6 files changed, 52 insertions(+), 17 deletions(-) diff --git a/src/Core.cc b/src/Core.cc index 1b23202..9ae80bb 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -23,7 +23,7 @@ This file is part of the PIXHAWK project /** * @file - * @brief Implementation of main class + * @brief Implementation of class Core * * @author Lorenz Meier * @@ -115,7 +115,7 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Could not connect UDP port. Is already an instance of " + qAppName() + " running?"); - msgBox.setInformativeText("You will not be able to receive data via UDP. Do you want to close the application?"); + msgBox.setInformativeText("You will not be able to receive data via UDP. Please check that you're running the right executable and then re-start " + qAppName() + ". Do you want to close the application?"); msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Cancel); int ret = msgBox.exec(); @@ -126,9 +126,8 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Exit application if (ret == QMessageBox::Yes) { - qDebug() << "EXITING"; - mainWindow->close(); - this->exit(EXIT_SUCCESS); + //mainWindow->close(); + QTimer::singleShot(200, mainWindow, SLOT(close())); } } diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index a033b32..293cbb9 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -104,6 +104,8 @@ emergency(false) bool GAudioOutput::say(QString text, int severity) { + // TODO Add severity filter + Q_UNUSED(severity); bool res = false; if (!emergency) { diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index d389666..8af036e 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -321,7 +321,7 @@ void MAVLinkSimulationLink::mainloop() streampointer += bufferlength; // IMU - rawImuValues.msec = time; + rawImuValues.usec = time; rawImuValues.xmag = 0; rawImuValues.ymag = 0; rawImuValues.zmag = 0; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 029e3fd..9b1a97d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -258,7 +258,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_raw_imu_t raw; mavlink_msg_raw_imu_decode(&message, &raw); - quint64 time = getUnixTime(raw.msec); + quint64 time = getUnixTime(raw.usec); emit valueChanged(uasId, "Accel. X", raw.xacc, time); emit valueChanged(uasId, "Accel. Y", raw.yacc, time); @@ -330,12 +330,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) positionLock = true; } break; - case MAVLINK_MSG_ID_POSITION: + case MAVLINK_MSG_ID_LOCAL_POSITION: //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; { - mavlink_position_t pos; - mavlink_msg_position_decode(&message, &pos); + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); emit valueChanged(uasId, "x", pos.x, time); emit valueChanged(uasId, "y", pos.y, time); @@ -346,6 +346,38 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit localPositionChanged(this, pos.x, pos.y, pos.z, time); } break; + case MAVLINK_MSG_ID_GLOBAL_POSITION: + //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; + { + mavlink_global_position_t pos; + mavlink_msg_global_position_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + emit valueChanged(uasId, "lat", pos.lat, time); + emit valueChanged(uasId, "lon", pos.lon, time); + emit valueChanged(uasId, "alt", pos.alt, time); + emit valueChanged(uasId, "g-vx", pos.vx, time); + emit valueChanged(uasId, "g-vy", pos.vy, time); + emit valueChanged(uasId, "g-vz", pos.vz, time); + emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); + } + break; + case MAVLINK_MSG_ID_GPS_RAW: + //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; + { + mavlink_gps_raw_t pos; + mavlink_msg_gps_raw_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + emit valueChanged(uasId, "lat", pos.lat, time); + emit valueChanged(uasId, "lon", pos.lon, time); + emit valueChanged(uasId, "alt", pos.alt, time); + emit valueChanged(uasId, "g-vx", pos.vx, time); + emit valueChanged(uasId, "g-vy", pos.vy, time); + emit valueChanged(uasId, "g-vz", pos.vz, time); + emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); + } + break; case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t value; diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 58481b9..8e74d1d 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -316,7 +316,8 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float circlePen.setColor(color); painter->setBrush(Qt::NoBrush); painter->setPen(circlePen); - drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter); + drawCircle(xRef, yRef+nameHeight, radius, 0.0f, color, painter); + //drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter); QString label; label.sprintf("%05.1f", value); @@ -466,7 +467,8 @@ void HDDisplay::drawChangeIndicatorGauge(float xRef, float yRef, float radius, f circlePen.setColor(defaultColor); painter->setBrush(Qt::NoBrush); painter->setPen(circlePen); - drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter); + drawCircle(xRef, yRef, radius, 200.0f, color, painter); + //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter); QString label; label.sprintf("%05.1f", value); @@ -582,7 +584,7 @@ void HDDisplay::drawLine(float refX1, float refY1, float refX2, float refY2, flo painter->drawLine(QPoint(refToScreenX(refX1), refToScreenY(refY1)), QPoint(refToScreenX(refX2), refToScreenY(refY2))); } -void HDDisplay::drawEllipse(float refX, float refY, float radiusX, float radiusY, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter) +void HDDisplay::drawEllipse(float refX, float refY, float radiusX, float radiusY, float lineWidth, const QColor& color, QPainter* painter) { QPen pen(painter->pen().style()); pen.setWidth(refLineWidthToPen(lineWidth)); @@ -591,9 +593,9 @@ void HDDisplay::drawEllipse(float refX, float refY, float radiusX, float radiusY painter->drawEllipse(QPointF(refToScreenX(refX), refToScreenY(refY)), refToScreenX(radiusX), refToScreenY(radiusY)); } -void HDDisplay::drawCircle(float refX, float refY, float radius, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter) +void HDDisplay::drawCircle(float refX, float refY, float radius, float lineWidth, const QColor& color, QPainter* painter) { - drawEllipse(refX, refY, radius, radius, startDeg, endDeg, lineWidth, color, painter); + drawEllipse(refX, refY, radius, radius, lineWidth, color, painter); } void HDDisplay::changeEvent(QEvent *e) diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 65d13c5..67c11c7 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -73,8 +73,8 @@ protected: void rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin); void drawPolygon(QPolygonF refPolygon, QPainter* painter); void drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter); - void drawEllipse(float refX, float refY, float radiusX, float radiusY, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter); - void drawCircle(float refX, float refY, float radius, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter); + void drawEllipse(float refX, float refY, float radiusX, float radiusY, float lineWidth, const QColor& color, QPainter* painter); + void drawCircle(float refX, float refY, float radius, float lineWidth, const QColor& color, QPainter* painter); void drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter); void drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid=true);