diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index d5546cf1b6a909a98ef8980c3fc7b228179cb1f4..ba21ad028620c0e569249edea23d48729badd013 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -836,7 +836,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                     streampointer+=bufferlength;
                     //qDebug() << "Sending PARAM" << key;
                 }
-                else if (read.param_index < onboardParams.size())
+                else if (read.param_index >= 0 && read.param_index < onboardParams.keys().size())
                 {
                     key = onboardParams.keys().at(read.param_index);
                     float paramValue = onboardParams.value(key);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index a2b4a8374980f67cbf39bb1da7cb2e326e4aff2a..3f7bf42285730261db7be1e1a9e9e01f2a51a015 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1066,7 +1066,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 unknownPackets.append(message.msgid);
                 QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid);
-                GAudioOutput::instance()->say(errString+tr(", please check console for details."));
+                //GAudioOutput::instance()->say(errString+tr(", please check 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;
diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc
index 36f8b5e042a5861090d87091562d28ff6a9527e5..750314535a41d786947cf7e53f3d4f40e4a6b12c 100644
--- a/src/ui/QGCRGBDView.cc
+++ b/src/ui/QGCRGBDView.cc
@@ -1,5 +1,6 @@
 #include <QMenu>
 #include <QContextMenuEvent>
+#include <QSettings>
 
 #include "QGCRGBDView.h"
 #include "UASManager.h"
@@ -24,6 +25,32 @@ QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 
     clearData();
+    loadSettings();
+}
+
+QGCRGBDView::~QGCRGBDView()
+{
+    storeSettings();
+}
+
+void QGCRGBDView::storeSettings()
+{
+    QSettings settings;
+    settings.beginGroup("QGC_RGBDWIDGET");
+    settings.setValue("STREAM_RGB_ON", rgbEnabled);
+    settings.setValue("STREAM_DEPTH_ON", depthEnabled);
+    settings.endGroup();
+    settings.sync();
+}
+
+void QGCRGBDView::loadSettings()
+{
+    QSettings settings;
+    settings.beginGroup("QGC_RGBDWIDGET");
+    rgbEnabled = settings.value("STREAM_RGB_ON", rgbEnabled).toBool();
+    // Only enable depth if RGB is not on
+    if (!rgbEnabled) depthEnabled = settings.value("STREAM_DEPTH_ON", depthEnabled).toBool();
+    settings.endGroup();
 }
 
 void QGCRGBDView::setActiveUAS(UASInterface* uas)
diff --git a/src/ui/QGCRGBDView.h b/src/ui/QGCRGBDView.h
index 2ca599feb77e7a4307138c2408d2e02db28df0df..58b1503dbc9ba1b3c13ce4edd1350fce18232737 100644
--- a/src/ui/QGCRGBDView.h
+++ b/src/ui/QGCRGBDView.h
@@ -8,6 +8,7 @@ class QGCRGBDView : public HUD
     Q_OBJECT
 public:
     explicit QGCRGBDView(int width=640, int height=480, QWidget *parent = 0);
+    ~QGCRGBDView();
 
 signals:
 
@@ -26,6 +27,10 @@ protected:
     QAction* enableDepthAction;
 
     void contextMenuEvent (QContextMenuEvent* event);
+    /** @brief Store current configuration of widget */
+    void storeSettings();
+    /** @brief Load configuration of widget */
+    void loadSettings();
 };
 
 #endif // QGCRGBDVIEW_H
diff --git a/src/ui/mavlink/QGCMAVLinkMessageSender.cc b/src/ui/mavlink/QGCMAVLinkMessageSender.cc
index 227934f72262897296e8ecc66420f63c5a93fd91..82b8d14a65925362b700898df766691cb109d25b 100644
--- a/src/ui/mavlink/QGCMAVLinkMessageSender.cc
+++ b/src/ui/mavlink/QGCMAVLinkMessageSender.cc
@@ -18,7 +18,9 @@ QGCMAVLinkMessageSender::QGCMAVLinkMessageSender(MAVLinkProtocol* mavlink, QWidg
     ui->treeWidget->setHeaderLabels(header);
     createTreeView();
     connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(refresh()));
-    refreshTimer.start(1000); // Refresh at 1 Hz interval
+    //refreshTimer.start(1000); // Refresh at 1 Hz interval
+
+    connect(ui->sendButton, SIGNAL(pressed()), this, SLOT(sendMessage()));
 }
 
 void QGCMAVLinkMessageSender::refresh()
@@ -35,8 +37,17 @@ void QGCMAVLinkMessageSender::refresh()
     // ui->treeWidget->topLevelItem(0)->children();
 }
 
+bool QGCMAVLinkMessageSender::sendMessage()
+{
+    sendMessage(ui->messageIdSpinBox->value());
+}
+
 bool QGCMAVLinkMessageSender::sendMessage(unsigned int msgid)
 {
+    if (msgid == 0 || msgid > 255 || messageInfo[msgid].name == "" || messageInfo[msgid].name == "EMPTY")
+    {
+        return false;
+    }
     bool result = true;
 
     if (treeWidgetItems.contains(msgid))
@@ -91,6 +102,44 @@ bool QGCMAVLinkMessageSender::sendMessage(unsigned int msgid)
                     *u = field->data(1, Qt::DisplayRole).toChar().toAscii();
                 }
                 break;
+            case MAVLINK_TYPE_INT8_T:
+                if (messageInfo[msgid].fields[fieldid].array_length > 0)
+                {
+                    int8_t* nums = reinterpret_cast<int8_t*>((m+messageInfo[msgid].fields[fieldid].wire_offset));
+                    for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
+                    {
+                        if ((unsigned int)(field->data(1, Qt::DisplayRole).toString().split(" ").size()) > j)
+                        {
+                            nums[j] = field->data(1, Qt::DisplayRole).toString().split(" ").at(j).toInt();
+                        }
+                    }
+                }
+                else
+                {
+                    // Single value
+                    int8_t* u = reinterpret_cast<int8_t*>(m+messageInfo[msgid].fields[fieldid].wire_offset);
+                    *u = field->data(1, Qt::DisplayRole).toChar().toAscii();
+                }
+                break;
+            case MAVLINK_TYPE_UINT16_T:
+                if (messageInfo[msgid].fields[fieldid].array_length > 0)
+                {
+                    uint16_t* nums = reinterpret_cast<uint16_t*>(m+messageInfo[msgid].fields[fieldid].wire_offset);
+                    for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j)
+                    {
+                        if ((unsigned int)(field->data(1, Qt::DisplayRole).toString().split(" ").size()) > j)
+                        {
+                            nums[j] = field->data(1, Qt::DisplayRole).toString().split(" ").at(j).toUInt();
+                        }
+                    }
+                }
+                else
+                {
+                    // Single value
+                    uint16_t* u = reinterpret_cast<uint16_t*>(m+messageInfo[msgid].fields[fieldid].wire_offset);
+                    *u = field->data(1, Qt::DisplayRole).toUInt();
+                }
+                break;
                 //            case MAVLINK_TYPE_INT8_T:
                 //                if (messageInfo[msgid].fields[fieldid].array_length > 0)
                 //                {
@@ -292,6 +341,9 @@ bool QGCMAVLinkMessageSender::sendMessage(unsigned int msgid)
                 //                break;
             }
         }
+
+        // Send message
+        protocol->sendMessage(msg);
     }
     else
     {
diff --git a/src/ui/mavlink/QGCMAVLinkMessageSender.h b/src/ui/mavlink/QGCMAVLinkMessageSender.h
index a9f38703f965509da63f4223638e48d57b8623b5..267c9c2d1c0b18f90d9b696bdfcbdb31342385ff 100644
--- a/src/ui/mavlink/QGCMAVLinkMessageSender.h
+++ b/src/ui/mavlink/QGCMAVLinkMessageSender.h
@@ -20,6 +20,10 @@ class QGCMAVLinkMessageSender : public QWidget
 public:
     explicit QGCMAVLinkMessageSender(MAVLinkProtocol* mavlink, QWidget *parent = 0);
     ~QGCMAVLinkMessageSender();
+
+public slots:
+    /** @brief Send message currently selected in ui, taking values from tree view */
+    bool sendMessage();
     
 protected:
     mavlink_message_info_t messageInfo[256];    ///< Meta information about all messages
diff --git a/src/ui/mavlink/QGCMAVLinkMessageSender.ui b/src/ui/mavlink/QGCMAVLinkMessageSender.ui
index db4aa010c553e90b93ba09a36d128322e66d3b96..5b4ea00e9ed0bd1fd0e9e1b554580b8e777c5107 100644
--- a/src/ui/mavlink/QGCMAVLinkMessageSender.ui
+++ b/src/ui/mavlink/QGCMAVLinkMessageSender.ui
@@ -26,6 +26,16 @@
      </column>
     </widget>
    </item>
+   <item row="1" column="0">
+    <widget class="QSpinBox" name="messageIdSpinBox"/>
+   </item>
+   <item row="1" column="1">
+    <widget class="QPushButton" name="sendButton">
+     <property name="text">
+      <string>Send Message</string>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>