From cd6c890e6ebce79e7499f9033f293ba36c1d578c Mon Sep 17 00:00:00 2001 From: pixhawk Date: Tue, 8 Feb 2011 11:18:45 +0100 Subject: [PATCH] Made MAVLink parser aware of line breaks in comments, added new servo output packet to QGC and simulation, enabled servo output display in new Actuator View (previous Payload view) --- src/comm/MAVLinkSimulationLink.cc | 1 - src/comm/MAVLinkSimulationMAV.cc | 14 ++++++++++++- src/comm/MAVLinkXMLParser.cc | 5 +++-- src/uas/UAS.cc | 22 ++++++++++++++++--- src/ui/HDDisplay.cc | 35 +++++++++++++++++-------------- src/ui/HDDisplay.h | 9 +++++--- src/ui/MainWindow.cc | 20 ++++++++++++------ src/ui/linechart/Linecharts.cc | 2 +- 8 files changed, 75 insertions(+), 33 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 400514673..6f08e619e 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -114,7 +114,6 @@ void MAVLinkSimulationLink::run() status.mode = MAV_MODE_UNINIT; status.status = MAV_STATE_UNINIT; status.vbat = 0; - status.motor_block = 1; status.packet_drop = 0; diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index c14efdda5..094fa64c0 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -64,6 +64,19 @@ void MAVLinkSimulationMAV::mainloop() mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); + + mavlink_servo_output_raw_t servos; + servos.servo1_raw = 1000; + servos.servo2_raw = 1250; + servos.servo3_raw = 1400; + servos.servo4_raw = 1500; + servos.servo5_raw = 1800; + servos.servo6_raw = 1500; + servos.servo7_raw = 1500; + servos.servo8_raw = 2000; + + mavlink_msg_servo_output_raw_encode(systemid, MAV_COMP_ID_IMU, &msg, &servos); + link->sendMAVLinkMessage(&msg); timer1Hz = 50; } @@ -140,7 +153,6 @@ void MAVLinkSimulationMAV::mainloop() // SYSTEM STATUS mavlink_sys_status_t status; status.load = 300; - status.motor_block = 1; status.mode = sys_mode; status.nav_mode = nav_mode; status.packet_drop = 0; diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index 34b830fe6..2b25075c7 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -285,12 +285,13 @@ bool MAVLinkXMLParser::generate() if (e2.text().length() > 0) { fieldComment = " /* " + e2.text() + "*/"; + fieldComment = fieldComment.replace("\n", " "); } currEnum += "\t" + fieldName.toUpper() + "=" + fieldValue + "," + fieldComment + "\n"; } else if(!e2.isNull() && e2.tagName() == "description") { - comment = e2.text() + comment; + comment = e2.text().replace("\n", " ") + comment; } f = f.nextSibling(); } @@ -407,7 +408,7 @@ bool MAVLinkXMLParser::generate() { // Send arguments are the same for integral types and arrays sendArguments += ", " + fieldName; - commentLines += commentEntry.arg(fieldName, fieldText); + commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); } // MAVLink version field diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 6877babf3..8937591ba 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -191,7 +191,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { QString uasState; QString stateDescription; - QString patternPath; // Receive named value message receiveMessageNamedValue(message); @@ -735,7 +734,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); } break; - + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + { + qDebug() << "Received servo raw message"; + mavlink_servo_output_raw_t servos; + mavlink_msg_servo_output_raw_decode(&message, &servos); + quint64 time = getUnixTime(0); + emit valueChanged(uasId, "servo #1", "us", servos.servo1_raw, time); + emit valueChanged(uasId, "servo #2", "us", servos.servo2_raw, time); + emit valueChanged(uasId, "servo #3", "us", servos.servo3_raw, time); + emit valueChanged(uasId, "servo #4", "us", servos.servo4_raw, time); + emit valueChanged(uasId, "servo #5", "us", servos.servo5_raw, time); + emit valueChanged(uasId, "servo #6", "us", servos.servo6_raw, time); + emit valueChanged(uasId, "servo #7", "us", servos.servo7_raw, time); + emit valueChanged(uasId, "servo #8", "us", servos.servo8_raw, time); + } + break; case MAVLINK_MSG_ID_STATUSTEXT: { QByteArray b; @@ -826,7 +840,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!unknownPackets.contains(message.msgid)) { unknownPackets.append(message.msgid); - //GAudioOutput::instance()->say("UNABLE TO DECODE MESSAGE WITH ID " + QString::number(message.msgid) + " FROM SYSTEM " + QString::number(message.sysid)); + QString errString = tr("UNABLE TO DECODE MESSAGE WITH ID %1").arg(message.msgid); + GAudioOutput::instance()->say(errString+tr(", please check the communication console for details.")); + emit textMessageReceived(uasId, message.compid, 255, errString); std::cout << "Unable to decode message from system " << std::dec << static_cast(message.sysid) << " with message id:" << static_cast(message.msgid) << std::endl; //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast(message.acid) << " with message id:" << static_cast(message.msgid) << std::endl; } diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 608dc665f..5f655587e 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -18,28 +18,17 @@ #include #include #include +#include #include "UASManager.h" #include "HDDisplay.h" #include "ui_HDDisplay.h" #include "MG.h" #include "QGC.h" -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif #include HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : QGraphicsView(parent), uas(NULL), - values(QMap()), - valuesDot(QMap()), - valuesMean(QMap()), - valuesCount(QMap()), - lastUpdate(QMap()), - minValues(), - maxValues(), - goodRanges(), - critRanges(), xCenterOffset(0.0f), yCenterOffset(0.0f), vwidth(80.0f), @@ -474,18 +463,17 @@ void HDDisplay::renderOverlay() */ void HDDisplay::setActiveUAS(UASInterface* uas) { - //qDebug() << "ATTEMPTING TO SET UAS"; if (this->uas != NULL) { // Disconnect any previously connected active MAV disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); + disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); } // Now connect the new UAS - - //qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); this->uas = uas; } @@ -629,7 +617,16 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float //drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter); QString label; - label.sprintf("% 06.1f", value); + + // Show integer values without decimal places + if (intValues.contains(name)) + { + label.sprintf("% 05d", (int)value); + } + else + { + label.sprintf("% 06.1f", value); + } // Text @@ -857,6 +854,12 @@ float HDDisplay::refLineWidthToPen(float line) return line * 2.50f; } +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec) { Q_UNUSED(uasId); diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index f92bcb122..63af2e57c 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -63,8 +63,10 @@ public: ~HDDisplay(); public slots: - /** @brief Update a HDD value */ + /** @brief Update a HDD double value */ void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); + /** @brief Update a HDD integer value */ + void updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); virtual void setActiveUAS(UASInterface* uas); /** @brief Removes a plot item by the action data */ @@ -137,14 +139,15 @@ protected: UASInterface* uas; ///< The uas currently monitored QMap values; ///< The variables this HUD displays - QMap units; ///< The units + QMap units; ///< The units QMap valuesDot; ///< First derivative of the variable QMap valuesMean; ///< Mean since system startup for this variable QMap valuesCount; ///< Number of values received so far QMap lastUpdate; ///< The last update time for this variable QMap minValues; ///< The minimum value this variable is assumed to have QMap maxValues; ///< The maximum value this variable is assumed to have - QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes + QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes + QMap intValues; ///< Is the gauge value an integer? QMap > goodRanges; ///< The range of good values QMap > critRanges; ///< The range of critical values double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 1b4c64e92..758adc23d 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -411,10 +411,18 @@ void MainWindow::buildPxWidgets() //FIXME: memory of acceptList2 will never be freed again QStringList* acceptList2 = new QStringList(); + acceptList2->append("900,servo #1,us,2100,s"); + acceptList2->append("900,servo #2,us,2100,s"); + acceptList2->append("900,servo #3,us,2100,s"); + acceptList2->append("900,servo #4,us,2100,s"); + acceptList2->append("900,servo #5,us,2100,s"); + acceptList2->append("900,servo #6,us,2100,s"); + acceptList2->append("900,servo #7,us,2100,s"); + acceptList2->append("900,servo #8,us,2100,s"); acceptList2->append("0,abs pressure,hPa,65500"); - acceptList2->append("-2048,accel. x,raw,2048,s"); - acceptList2->append("-2048,accel. y,raw,2048,s"); - acceptList2->append("-2048,accel. z,raw,2048,s"); + //acceptList2->append("-2048,accel. x,raw,2048,s"); + //acceptList2->append("-2048,accel. y,raw,2048,s"); + //acceptList2->append("-2048,accel. z,raw,2048,s"); if (!linechartWidget) { @@ -505,10 +513,10 @@ void MainWindow::buildPxWidgets() if (!headDown2DockWidget) { - headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); - headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); + headDown2DockWidget = new QDockWidget(tr("Actuator Status"), this); + headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Actuator Status", this) ); headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); - addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); + addToToolsMenu (headDown2DockWidget, tr("Actuator Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); } if (!rcViewDockWidget) diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 6fc366537..0ffea43d9 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -101,7 +101,7 @@ void Linecharts::addSystem(UASInterface* uas) // Values with unit as double connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); // Values with unit as integer - connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), widget, SLOT(appendData(int,QString,QString,int,quint64))); #endif connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString))); // Set system active if this is the only system -- 2.22.0