Added support for text messages from robot to operator

src/comm/MAVLinkSimulationLink.cc

@@ -365,6 +365,15 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
+        // Pack debug text message
+        statustext_t text;
+        text.severity = 0;
+        strcpy((char*)(text.text), "DEBUG MESSAGE TEXT");
+        message_statustext_encode(systemId, componentId, &msg, &text);
+        bufferlength = message_to_send_buffer(buffer, &msg);
+        memcpy(stream+streampointer, buffer, bufferlength);
+        streampointer += bufferlength;
+
         /*
         // Pack message and get size of encoded byte string
         messageSize = message_boot_pack(systemId, componentId, &msg, version);
@@ -529,6 +538,14 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                     }
                 }
                 break;
+
+            case MAVLINK_MSG_ID_MANUAL_CONTROL:
+                {
+                    manual_control_t control;
+                    message_manual_control_decode(&msg, &control);
+                    qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch;
+                }
+                break;
             }
 
 

src/uas/UAS.cc

@@ -353,8 +353,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 //b.append('\0');
                 QString text = QString(b);
                 int severity = message_statustext_get_severity(&message);
-                qDebug() << "RECEIVED STATUS:" << text;
+                //qDebug() << "RECEIVED STATUS:" << text;
                 //emit statusTextReceived(severity, text);
+                emit textMessageReceived(uasId, severity, text);
             }
             break;
         case MAVLINK_MSG_ID_PATTERN_DETECTED:

src/uas/UASInterface.h

@@ -208,6 +208,16 @@ signals:
      * @param description longer textual description. Should be however limited to a short text, e.g. 200 chars.
      */
     void statusChanged(UASInterface* uas, QString status, QString description);
+    /**
+     * @brief Received a plain text message from the robot
+     * This signal should NOT be used for standard communication, but rather for VERY IMPORTANT
+     * messages like critical errors.
+     *
+     * @param uasid ID of the sending system
+     * @param text the status text
+     * @param severity The severity of the message, 0 for plain debug messages, 10 for very critical messages
+     */
+    void textMessageReceived(int uasid, int severity, QString text);
     /**
      * @brief Drop rate of communication link updated
      *

src/ui/DebugConsole.cc

@@ -161,6 +161,11 @@ void DebugConsole::setAutoHold(bool hold)
     autoHold = hold;
 }
 
+void DebugConsole::receiveTextMessage(int id, int severity, QString text)
+{
+    m_ui->receiveText->appendHtml(QString("<b color=\"red\">(MAV" + QString::number(id) + QString(":") + QString::number(severity) + QString(") ") + text + QString("</b>")));
+}
+
 void DebugConsole::updateTrafficMeasurements()
 {
     lowpassDataRate = lowpassDataRate * 0.9f + (0.1f * ((float)snapShotBytes / (float)snapShotInterval) * 1000.0f);

src/ui/DebugConsole.h

@@ -68,6 +68,8 @@ public slots:
     void hold(bool hold);
     /** @brief Enable auto-freeze mode if traffic intensity is too high to display */
     void setAutoHold(bool hold);
+    /** @brief Receive plain text message to output to the user */
+    void receiveTextMessage(int id, int severity, QString text);
 
     protected slots:
     /** @brief Draw information overlay */

src/ui/MainWindow.cc

@@ -327,6 +327,9 @@ void MainWindow::UASCreated(UASInterface* uas)
         sysPresent = true;
     }
 
+    // FIXME Should be not inside the mainwindow
+    connect(uas, SIGNAL(textMessageReceived(int,int,QString)), debugConsole, SLOT(receiveTextMessage(int,int,QString)));
+
     // Health / System status indicator
     info->addUAS(uas);