diff --git a/src/QGC.h b/src/QGC.h
index 6478ae00b1355f2181828db57c020867acf3cd4b..b7e25c81d6ffedf22a090bb561d0349d0e0cc89a 100644
--- a/src/QGC.h
+++ b/src/QGC.h
@@ -6,7 +6,9 @@
 
 namespace QGC
 {
-    const QColor ColorCyan(55, 154, 195);
+    const QColor colorCyan(55, 154, 195);
+    const QColor colorRed(154, 20, 20);
+    const QColor colorGreen(20, 200, 20);
 
     /** @brief Get the current ground time in microseconds */
     quint64 groundTimeUsecs();
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index d4a728d02b20a2ac1041b7c178fca234068f430a..7ede06b9d801221d313f1be1d12e8fc8ed7eaef3 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -149,7 +149,7 @@ void HSIDisplay::paintDisplay()
         lockStatusColor = QColor(255, 20, 20);
     }
 
-    paintText(tr("POS"), QGC::ColorCyan, 1.8f, 2.0f, 2.5f, &painter);
+    paintText(tr("POS"), QGC::colorCyan, 1.8f, 2.0f, 2.5f, &painter);
     painter.setBrush(lockStatusColor);
     painter.setPen(Qt::NoPen);
     painter.drawRect(QRect(refToScreenX(9.5f), refToScreenY(2.0f), refToScreenX(7.0f), refToScreenY(4.0f)));
@@ -204,14 +204,14 @@ void HSIDisplay::paintDisplay()
     if (bodyXSetCoordinate != 0 || bodyYSetCoordinate != 0)
     {
         // Draw setpoint
-        drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::ColorCyan, painter);
+        drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::colorCyan, painter);
         // Draw travel direction line
         QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
         // Transform from body to world coordinates
         m = metricWorldToBody(m);
         // Scale from metric body to screen reference units
         QPointF s = metricBodyToRef(m);
-        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::ColorCyan, &painter);
+        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorCyan, &painter);
     }
 
     // Labels on outer part and bottom
diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc
index 26e1ec44ce667baa836f3cd606f631cf1237880c..d809bc0d963df6d932452e0467020b6fca2eb8df 100644
--- a/src/ui/QGCParamWidget.cc
+++ b/src/ui/QGCParamWidget.cc
@@ -35,6 +35,7 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCParamWidget.h"
 #include "UASInterface.h"
 #include <QDebug>
+#include "QGC.h"
 
 /**
  * @param uas MAV to set the parameters on
@@ -137,11 +138,8 @@ void QGCParamWidget::addComponent(int uas, int component, QString componentName)
 void QGCParamWidget::addParameter(int uas, int component, QString parameterName, float value)
 {
     Q_UNUSED(uas);
-    // Insert parameter into map
-    QStringList plist;
-    plist.append(parameterName);
-    plist.append(QString::number(value));
-    QTreeWidgetItem* item = new QTreeWidgetItem(plist);
+    // Reference to item in tree
+    QTreeWidgetItem* parameterItem;
 
     // Get component
     if (!components->contains(component))
@@ -164,6 +162,34 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName,
             compParamGroups->insert(parent, item);
             components->value(component)->addChild(item);
         }
+
+        // Append child to group
+        bool found = false;
+        QTreeWidgetItem* parentItem = compParamGroups->value(parent);
+        for (int i = 0; i < parentItem->childCount(); i++)
+        {
+            QTreeWidgetItem* child = parentItem->child(i);
+            QString key = child->data(0, Qt::DisplayRole).toString();
+            if (key == parameterName)
+            {
+                //qDebug() << "UPDATED CHILD";
+                parameterItem = child;
+                parameterItem->setData(1, Qt::DisplayRole, value);
+                found = true;
+            }
+        }
+
+        if (!found)
+        {
+            // Insert parameter into map
+            QStringList plist;
+            plist.append(parameterName);
+            plist.append(QString::number(value));
+            parameterItem = new QTreeWidgetItem(plist);
+
+            compParamGroups->value(parent)->addChild(parameterItem);
+            parameterItem>setFlags(item->flags() | Qt::ItemIsEditable);
+        }
     }
     else
     {
@@ -176,19 +202,28 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName,
             if (key == parameterName)
             {
                 //qDebug() << "UPDATED CHILD";
-                child->setData(1, Qt::DisplayRole, value);
+                parameterItem = child;
+                parameterItem->setData(1, Qt::DisplayRole, value);
                 found = true;
             }
         }
 
         if (!found)
         {
-            components->value(component)->addChild(item);
-            item->setFlags(item->flags() | Qt::ItemIsEditable);
+            // Insert parameter into map
+            QStringList plist;
+            plist.append(parameterName);
+            plist.append(QString::number(value));
+            parameterItem = new QTreeWidgetItem(plist);
+
+            compParamGroups->value(parent)->addChild(parameterItem);
+            parameterItem>setFlags(item->flags() | Qt::ItemIsEditable);
         }
-        //connect(item, SIGNAL())
-        tree->expandAll();
+        //tree->expandAll();
     }
+    // Reset background color
+    current->setBackground(0, QBrush(QColor(QGC::colorGreen)));
+    current->setBackground(1, QBrush(QColor(QGC::colorGreen)));
     tree->update();
 }
 
@@ -231,7 +266,8 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column)
                 {
                     qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value;
                     map->insert(str, value);
-                    // FIXME CHANGE COLOR OF CHANGED PARAM
+                    current->setBackground(0, QBrush(QColor(QGC::colorGreen)));
+                    current->setBackground(1, QBrush(QColor(QGC::colorGreen)));
                 }
             }
         }