Made MAVLink parser aware of line breaks in comments, added new servo output...

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

@@ -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;
 
 

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;

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

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<int>(message.sysid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                     //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                 }

src/ui/HDDisplay.cc

@@ -18,28 +18,17 @@
 #include <QMouseEvent>
 #include <QMenu>
 #include <QSettings>
+#include <qmath.h>
 #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 <QDebug>
 
 HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
         QGraphicsView(parent),
         uas(NULL),
-        values(QMap<QString, float>()),
-        valuesDot(QMap<QString, float>()),
-        valuesMean(QMap<QString, float>()),
-        valuesCount(QMap<QString, int>()),
-        lastUpdate(QMap<QString, quint64>()),
-        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);

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<QString, float> values;       ///< The variables this HUD displays
-    QMap<QString, QString> units;       ///< The units
+    QMap<QString, QString> units;      ///< The units
     QMap<QString, float> valuesDot;    ///< First derivative of the variable
     QMap<QString, float> valuesMean;   ///< Mean since system startup for this variable
     QMap<QString, int> valuesCount;    ///< Number of values received so far
     QMap<QString, quint64> lastUpdate; ///< The last update time for this variable
     QMap<QString, float> minValues;    ///< The minimum value this variable is assumed to have
     QMap<QString, float> maxValues;    ///< The maximum value this variable is assumed to have
-    QMap<QString, bool> symmetric;    ///< Draw the gauge / dial symmetric bool = yes
+    QMap<QString, bool> symmetric;     ///< Draw the gauge / dial symmetric bool = yes
+    QMap<QString, bool> intValues;     ///< Is the gauge value an integer?
     QMap<QString, QPair<float, float> > goodRanges; ///< The range of good values
     QMap<QString, QPair<float, float> > critRanges; ///< The range of critical values
     double scalingFactor;      ///< Factor used to scale all absolute values to screen coordinates

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)

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