Added controller output support

src/comm/MAVLinkProtocol.cc

@@ -17,7 +17,6 @@
 #include <QSettings>
 #include <QDesktopServices>
 
-//#include "MG.h"
 #include "MAVLinkProtocol.h"
 #include "UASInterface.h"
 #include "UASManager.h"
@@ -28,7 +27,6 @@
 #include "ArduPilotMegaMAV.h"
 #include "configuration.h"
 #include "LinkManager.h"
-//#include "MainWindow.h"
 #include "QGCMAVLink.h"
 #include "QGCMAVLinkUASFactory.h"
 #include "QGC.h"
@@ -182,14 +180,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
 
         if (decodeState == 1) {
-#ifdef MAVLINK_MESSAGE_LENGTHS
-	    const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
-	    if (message.msgid >= sizeof(message_lengths) ||
-		message.len != message_lengths[message.msgid]) {
-                    qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
-		    continue;
-	    }
-#endif
+//#ifdef MAVLINK_MESSAGE_LENGTHS
+//	    const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
+//	    if (message.msgid >= sizeof(message_lengths) ||
+//		message.len != message_lengths[message.msgid]) {
+//                    qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
+//		    continue;
+//	    }
+//#endif
             // Log data
             if (m_loggingEnabled && m_logfile) {
                 const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64);

src/uas/UAS.cc

@@ -824,26 +824,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         case MAVLINK_MSG_ID_DEBUG:
             emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow());
             break;
-        case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT:
+        case MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT:
             {
-                mavlink_attitude_controller_output_t out;
-                mavlink_msg_attitude_controller_output_decode(&message, &out);
-                quint64 time = MG::TIME::getGroundTimeNowUsecs();
-                emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time);
-                emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f);
-                emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f);
-                emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f);
-            }
-            break;
-        case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT:
-            {
-                mavlink_position_controller_output_t out;
-                mavlink_msg_position_controller_output_decode(&message, &out);
-                quint64 time = MG::TIME::getGroundTimeNow();
-                //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
-                emit valueChanged(uasId, "pos control x", "raw", out.x, time);
-                emit valueChanged(uasId, "pos control y", "raw", out.y, time);
-                emit valueChanged(uasId, "pos control z", "raw", out.z, time);
+                mavlink_roll_pitch_yaw_thrust_setpoint_t out;
+                mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out);
+                quint64 time = getUnixTime(out.time_ms*1000);
+                emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time);
+                emit valueChanged(uasId, "att control roll", "rad", out.roll, time);
+                emit valueChanged(uasId, "att control pitch", "rad", out.pitch, time);
+                emit valueChanged(uasId, "att control yaw", "rad", out.yaw, time);
+                emit valueChanged(uasId, "att control thrust", "0-1", out.thrust, time);
             }
             break;
         case MAVLINK_MSG_ID_WAYPOINT_COUNT:

src/ui/QGCToolBar.cc

@@ -55,6 +55,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
         disconnect(mav, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
         disconnect(mav, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
         disconnect(mav, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
+        disconnect(mav, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
     }
 
     // Connect new system
@@ -63,6 +64,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
     connect(active, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
     connect(active, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
     connect(active, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
+    connect(active, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
 
     // Update all values once
     nameLabel->setText(mav->getUASName());
@@ -81,6 +83,7 @@ void QGCToolBar::createCustomWidgets()
     stateLabel = new QLabel("------", this);
     wpLabel = new QLabel("---", this);
     distlabel = new QLabel("--- ---- m", this);
+    messageLabel = new QLabel("No system messages.", this);
     //symbolButton->setIcon(":");
     symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px; background-color: none; }");
     addWidget(symbolButton);
@@ -89,6 +92,7 @@ void QGCToolBar::createCustomWidgets()
     addWidget(stateLabel);
     addWidget(wpLabel);
     addWidget(distlabel);
+    addWidget(messageLabel);
 
     toggleLoggingAction = new QAction(QIcon(":"), "Start Logging", this);
     logReplayAction = new QAction(QIcon(":"), "Start Replay", this);
@@ -154,6 +158,14 @@ void QGCToolBar::setSystemType(UASInterface* uas, unsigned int systemType)
         }
 }
 
+void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QString text)
+{
+    Q_UNUSED(uasid);
+    Q_UNUSED(componentid);
+    Q_UNUSED(severity);
+    messageLabel->setText(text);
+}
+
 QGCToolBar::~QGCToolBar()
 {
     delete toggleLoggingAction;

src/ui/QGCToolBar.h

@@ -50,6 +50,8 @@ public slots:
     void updateName(const QString& name);
     /** @brief Set the MAV system type */
     void setSystemType(UASInterface* uas, unsigned int systemType);
+    /** @brief Received system text message */
+    void receiveTextMessage(int uasid, int componentid, int severity, QString text);
 
 protected:
     void createCustomWidgets();
@@ -63,7 +65,7 @@ protected:
     QLabel* stateLabel;
     QLabel* wpLabel;
     QLabel* distlabel;
-
+    QLabel* messageLabel;
 };
 
 #endif // QGCTOOLBAR_H