diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index de1f54a8d5aa3fa1f65e748d3e25ccfaf81061e5..aa07a527ea1596b52ab7509d13bbe8609b3486b3 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -225,21 +225,20 @@ SOURCES += src/main.cc \
 RESOURCES = mavground.qrc
 
 # Include RT-LAB Library
-win32 { 
-exists(src/lib/opalrt/OpalApi.h){
+win32:exists(src/lib/opalrt/OpalApi.h) { 
     message("Building support for Opal-RT")
     LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \
         -lOpalApi
     INCLUDEPATH += src/lib/opalrt
-    SOURCES += src/comm/OpalLink.cc \
-        src/comm/Parameter.cc \
-        src/comm/QGCParamID.cc \
-        src/comm/ParameterList.cc
-    HEADERS += src/comm/OpalLink.h \
-        src/comm/OpalRT.h \
+    HEADERS += src/comm/OpalRT.h \
+        src/comm/OpalLink.h \
         src/comm/Parameter.h \
         src/comm/QGCParamID.h \
         src/comm/ParameterList.h
+    SOURCES += src/comm/OpalRT.cc \
+        src/comm/OpalLink.cc \
+        src/comm/Parameter.cc \
+        src/comm/QGCParamID.cc \
+        src/comm/ParameterList.cc
     DEFINES += OPAL_RT
 }
-}
diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index d6928f46c6d37fc6c24ddb35795ae9e6a1175e27..f2343959daea52eaa065cb98f242f69553e607a7 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -81,16 +81,9 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
         case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
             {
                 qDebug() << "OpalLink::writeBytes(): request params";
-//                getParameterList();
 
                 mavlink_message_t param;
-//                char paramName[] = "NAV_FILT_INIT";
-//                mavlink_msg_param_value_pack(systemID, componentID, &param,
-//                                             (int8_t*)(paramName),
-//                                             0,
-//                                             1,
-//                                             0);
-//                receiveMessage(param);
+
 
                 OpalRT::ParameterList::const_iterator paramIter;
                 for (paramIter = params->begin(); paramIter != params->end(); ++paramIter)
@@ -109,23 +102,32 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
             }
         case MAVLINK_MSG_ID_PARAM_SET:
             {
-                qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";
+
+//                qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";
+
                 mavlink_param_set_t param;
                 mavlink_msg_param_set_decode(&msg, &param);
-                QString paramName((char*)param.param_id);
-                qDebug() << "OpalLink::writeBytes():paramName: " << paramName;
-                if (paramName == "NAV_FILT_INIT")
+                OpalRT::QGCParamID paramName((char*)param.param_id);
+
+//                qDebug() << "OpalLink::writeBytes():paramName: " << paramName;
+
+                if ((*params).contains(param.target_component, paramName))
                 {
-                    if (param.param_value == 1 || param.param_value == 0)
-                    {
-                        double values[2] = {};
-                        values[0] = param.param_value;
-                        setSignals(values);
-                    }
-                    else
-                    {
-                        qDebug() << "OpalLink::writeBytes(): Param NAV_FILT_INIT must be 1 or 0";
-                    }
+                    OpalRT::Parameter p = (*params)(param.target_component, paramName);
+//                    qDebug() << __FILE__ << ":" << __LINE__ << ": "  << p;
+                    // Set the param value in Opal-RT
+                    p.setValue(param.param_value);
+
+                    // Get the param value from Opal-RT to make sure it was set properly
+                    mavlink_message_t paramMsg;
+                    mavlink_msg_param_value_pack(systemID,
+                                                 p.getComponentID(),
+                                                 &paramMsg,
+                                                 p.getParamID().toInt8_t(),
+                                                 p.getValue(),
+                                                 params->count(),
+                                                 params->indexOf(p));
+                    receiveMessage(paramMsg);
                 }
             }
             break;
@@ -141,10 +143,6 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
 void OpalLink::readBytes()
 {
     receiveDataMutex.lock();
-//    qDebug() << "OpalLink::readBytes(): Reading a message.  size of buffer: " << receiveBuffer->count();
-//    QByteArray message = receiveBuffer->dequeue();
-
-
     emit bytesReceived(this, receiveBuffer->dequeue());
     receiveDataMutex.unlock();
 
@@ -173,7 +171,6 @@ void OpalLink::heartbeat()
 
     if (m_heartbeatsEnabled)
     {
-//        qDebug() << "OpalLink::heartbeat(): Generate a heartbeat";
         mavlink_message_t beat;
         mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC);
         receiveMessage(beat);
@@ -185,30 +182,17 @@ void OpalLink::setSignals(double *values)
     unsigned short numSignals = 2;
     unsigned short logicalId = 1;
     unsigned short signalIndex[] = {0,1};
-//    double values[] = {0.5, // ch 1
-//                       0.5, // ch2
-//                       0.5, // ch3
-//                       0.5, // ch4
-//                       0.5, // ch5
-//                       0.5, // ch6
-//                       0.5, // ch7
-//                       0.5, // ch8
-//                       0.5}; // ch9
 
     int returnValue;
     returnValue =  OpalSetSignals( numSignals, logicalId, signalIndex, values);
     if (returnValue != EOK)
     {
-        setLastErrorMsg();
-        displayLastErrorMsg();
+//        OpalRT::setLastErrorMsg();
+        OpalRT::OpalErrorMsg::displayLastErrorMsg();
     }
 }
 void OpalLink::getSignals()
 {
-//    getSignalsMutex.lock();
-//    qDebug() <<  "OpalLink::getSignals(): Attempting to acquire signals";
-
-
     unsigned long  timeout = 0;
     unsigned short acqGroup = 0; //this is actually group 1 in the model
     unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS;
@@ -224,8 +208,7 @@ void OpalLink::getSignals()
                                        values, lastValues, decimation);
 
         if (returnVal == EOK )
-        {
-            //        qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues);
+        {            
             /* Send position info to qgroundcontrol */
             mavlink_message_t local_position;
             mavlink_msg_local_position_pack(systemID, componentID, &local_position,
@@ -263,16 +246,11 @@ void OpalLink::getSignals()
                                              values[OpalRT::B_W_2]
                                              );
             receiveMessage(bias);
-        }
-//        else if (returnVal == EAGAIN)
-//        {
-//            qDebug() << "OpalLink::getSignals: Data was not ready";
-//        }
-        // if returnVal == EAGAIN => data just wasn't ready
-        else if (returnVal != EAGAIN)
+        }        
+        else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
         {
             getSignalsTimer->stop();
-            displayLastErrorMsg();
+            OpalRT::OpalErrorMsg::displayLastErrorMsg();
         }
     }
 
@@ -282,82 +260,10 @@ void OpalLink::getSignals()
     delete timestep;
     delete lastValues;
     delete decimation;
-//    getSignalsMutex.unlock();
 
 }
 
 
-void OpalLink::getParameterList()
-{
-    /* inputs */
-    unsigned short allocatedParams=0;
-    unsigned short allocatedPathLen=0;
-    unsigned short allocatedNameLen=0;
-    unsigned short allocatedVarLen=0;
-
-    /* outputs */
-    unsigned short numParams;
-    unsigned short *idParam=NULL;
-    unsigned short maxPathLen;
-    char **path=NULL;
-    unsigned short maxNameLen;
-    char **name=NULL;
-    unsigned short maxVarLen;
-    char **var=NULL;
-
-    int returnValue;
-
-    returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
-                             allocatedPathLen, &maxPathLen, path,
-                             allocatedNameLen, &maxNameLen, name,
-                             allocatedVarLen, &maxVarLen, var);
-    if (returnValue!=E2BIG)
-    {
-        setLastErrorMsg();
-        displayLastErrorMsg();
-        return;
-    }
-
-    // allocate memory for parameter list
-
-    idParam = new unsigned short[numParams];
-    allocatedParams = numParams;
-
-    path = new char*[numParams];
-    for (int i=0; i<numParams; i++)
-        path[i]=new char[maxPathLen];
-    allocatedPathLen = maxPathLen;
-
-    name = new char*[numParams];
-    for (int i=0; i<numParams; i++)
-        name[i] = new char[maxNameLen];
-    allocatedNameLen = maxNameLen;
-
-    var = new char*[numParams];
-    for (int i=0; i<numParams; i++)
-        var[i] = new char[maxVarLen];
-    allocatedVarLen = maxVarLen;
-
-    returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
-                             allocatedPathLen, &maxPathLen, path,
-                             allocatedNameLen, &maxNameLen, name,
-                             allocatedVarLen, &maxVarLen, var);
-
-    if (returnValue != EOK)
-    {
-        setLastErrorMsg();
-        displayLastErrorMsg();
-        return;
-    }
-
-    qDebug() << "Num params: " << numParams << endl;
-    qDebug() << "Name\tPath\tVar" << endl;
-    for (int i=0; i<numParams; i++)
-        qDebug() << qSetFieldWidth(20) << name[i] << qSetFieldWidth(5) << idParam[i]
-                << qSetFieldWidth(50) << path[i];
-
-}
-
 /*
  *
   Administrative
@@ -384,8 +290,7 @@ void OpalLink::setName(QString name)
     emit nameChanged(this->name);
 }
 
-bool OpalLink::isConnected() {
-    //qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
+bool OpalLink::isConnected() {    
     return connectState;
 }
 
@@ -397,7 +302,9 @@ bool OpalLink::connect()
     short modelState;
 
     /// \todo allow configuration of instid in window    
-    if ((OpalConnect(101, false, &modelState) == EOK) && (OpalGetSignalControl(0, true) == EOK))
+    if ((OpalConnect(101, false, &modelState) == EOK)
+        && (OpalGetSignalControl(0, true) == EOK)
+        && (OpalGetParameterControl(true) == EOK))
     {
         connectState = true;
         /// \todo try/catch a delete in case params has already been allocated
@@ -409,7 +316,7 @@ bool OpalLink::connect()
     else
     {
         connectState = false;
-        displayLastErrorMsg();
+        OpalRT::OpalErrorMsg::displayLastErrorMsg();
     }
 
     emit connected(connectState);
@@ -421,25 +328,7 @@ bool OpalLink::disconnect()
     return false;
 }
 
-void OpalLink::displayLastErrorMsg()
-{
-    static QString lastErrorMsg;
-    setLastErrorMsg();
-    QMessageBox msgBox;
-    msgBox.setIcon(QMessageBox::Critical);
-    msgBox.setText(lastErrorMsg);
-    msgBox.exec();
-}
 
-void OpalLink::setLastErrorMsg()
-{
-    char buf[512];
-    unsigned short len;
-    static QString lastErrorMsg;
-    OpalGetLastErrMsg(buf, sizeof(buf), &len);
-    lastErrorMsg.clear();
-    lastErrorMsg.append(buf);
-}
 
 
 /*
diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h
index 29c9b84c788e1d3d44a06560b500b10782a5febc..36f36e16da413b324b00fcac0d562391deb52b02 100644
--- a/src/comm/OpalLink.h
+++ b/src/comm/OpalLink.h
@@ -33,7 +33,6 @@ This file is part of the QGROUNDCONTROL project
 #include <QMutex>
 #include <QDebug>
 #include <QTextStreamManipulator>
-#include <QMessageBox>
 #include <QTimer>
 #include <QQueue>
 #include <QByteArray>
@@ -50,16 +49,9 @@ This file is part of the QGROUNDCONTROL project
 #include "OpalRT.h"
 #include "ParameterList.h"
 #include "Parameter.h"
-
-#include "errno.h"
-
-#ifdef OPAL_RT
+#include "QGCParamID.h"
 #include "OpalApi.h"
-#endif
-
-
-
-
+#include "errno.h"
 #include "string.h"
 
 /*
@@ -110,8 +102,7 @@ public:
     qint64 bytesAvailable();
 
     void run();
-    static void setLastErrorMsg();
-    static void displayLastErrorMsg();
+
 public slots:
 
     void writeBytes(const char *bytes, qint64 length);
@@ -142,8 +133,6 @@ protected:
 
     QMutex statisticsMutex;
     QMutex receiveDataMutex;
-//    QMutex getSignalsMutex;
-    static QString lastErrorMsg;
 
     void setName(QString name);
 
@@ -164,5 +153,4 @@ protected:
     OpalRT::ParameterList *params;
 };
 
-//QString OpalLink::lastErrorMsg = QString();
 #endif // OPALLINK_H
diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h
index 48d0a3cae78e37fb56a59909bd95900987b4f135..7927285155e3c3d53186c05bf0bf446d4f576fcb 100644
--- a/src/comm/OpalRT.h
+++ b/src/comm/OpalRT.h
@@ -21,9 +21,20 @@ This file is part of the QGROUNDCONTROL project
 
 ======================================================================*/
 
+/**
+ * @file
+ *   @brief Types used for Opal-RT interface configuration
+ *   @author Bryan Godbolt <godbolt@ualberta.ca>
+ */
+
 #ifndef OPALRT_H
 #define OPALRT_H
 
+#include <QString>
+#include <QMessageBox>
+
+#include "OpalApi.h"
+
 namespace OpalRT
 {
     /*  ------------------------------ Outputs ------------------------------
@@ -78,5 +89,13 @@ namespace OpalRT
         LOG_ID = 1,
         CONTROLLER_ID = 1
     };
+
+    class OpalErrorMsg
+    {
+        static QString lastErrorMsg;
+    public:
+        static void displayLastErrorMsg();
+        static void setLastErrorMsg();
+    };
 }
 #endif // OPALRT_H
diff --git a/src/comm/Parameter.cc b/src/comm/Parameter.cc
index 9d8c3093e44a45390cde842bf92cfec70799dd1b..ab56c98c84f0b3f03fed142cbbedc53c120efede 100644
--- a/src/comm/Parameter.cc
+++ b/src/comm/Parameter.cc
@@ -1,3 +1,31 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Implementation of class OpalRT::Parameter
+ *   @author Bryan Godbolt <godbolt@ualberta.ca>
+ */
 #include "Parameter.h"
 using namespace OpalRT;
 
@@ -38,11 +66,10 @@ bool Parameter::operator ==(const Parameter& other) const
 
 }
 
-float Parameter::getValue() //const
+float Parameter::getValue()
 {
     unsigned short allocatedParams = 1;
     unsigned short numParams;
-//    unsigned short allocatedValues;
     unsigned short numValues = 1;
     unsigned short returnedNumValues;
     double value;
@@ -52,8 +79,33 @@ float Parameter::getValue() //const
 
     if (returnVal != EOK)
     {
+//        OpalRT::setLastErrorMsg();
+        OpalRT::OpalErrorMsg::displayLastErrorMsg();
         return FLT_MAX;
     }
 
     return static_cast<float>(value);
 }
+
+void Parameter::setValue(float val)
+{
+    unsigned short allocatedParams = 1;
+    unsigned short numParams;
+    unsigned short numValues = 1;
+    unsigned short returnedNumValues;
+    double value = static_cast<double>(val);
+
+    int returnVal = OpalSetParameters(allocatedParams, &numParams, &opalID,
+                                      numValues, &returnedNumValues, &value);
+    if (returnVal != EOK)
+    {
+        qDebug() << __FILE__ << ":" << __LINE__ << ": Error numer: " << QString::number(returnVal);
+        OpalErrorMsg::displayLastErrorMsg();
+    }
+}
+
+Parameter::operator QString() const
+{
+    return *simulinkPath + *simulinkName + " " + QString::number(componentID)
+            + " " + *paramID + " " + QString::number(opalID);
+}
diff --git a/src/comm/Parameter.h b/src/comm/Parameter.h
index 28d2afeefe668f44765cc586c30da092ce9f92a9..48abc6d6a76beec8969162317add64407da1af4f 100644
--- a/src/comm/Parameter.h
+++ b/src/comm/Parameter.h
@@ -21,13 +21,24 @@ This file is part of the QGROUNDCONTROL project
 
 ======================================================================*/
 
+/**
+ * @file
+ *   @brief Parameter Object used to intefrace with an OpalRT Simulink Parameter
+     \see OpalLink
+     \see OpalRT::ParameterList
+ *   @author Bryan Godbolt <godbolt@ualberta.ca>
+ */
+
 #ifndef PARAMETER_H
 #define PARAMETER_H
 
 #include <QString>
+#include <QDebug>
+
 #include "mavlink_types.h"
 #include "QGCParamID.h"
 #include "OpalApi.h"
+#include "OpalRT.h"
 #include <cfloat>
 
 namespace OpalRT
@@ -48,9 +59,11 @@ namespace OpalRT
         const QString& getSimulinkPath() {return *simulinkPath;}
         const QString& getSimulinkName() {return *simulinkName;}
         uint8_t getComponentID() const {return componentID;}
-        float getValue();// const;
+        float getValue();
+        void setValue(float value);
 
         bool operator==(const Parameter& other) const;
+        operator QString() const;
 
     protected:
         QString *simulinkPath;
diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc
index 60cdff1b88f8ea06cf8787358664b4fb431450ff..7ffc66d6830d256b5243af9ae39a52d59d101abe 100644
--- a/src/comm/ParameterList.cc
+++ b/src/comm/ParameterList.cc
@@ -1,9 +1,38 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Implementation of class OpalRT::ParameterList
+ *   @author Bryan Godbolt <godbolt@ualberta.ca>
+ */
+
 #include "ParameterList.h"
 using namespace OpalRT;
 
 ParameterList::ParameterList()
     :params(new QMap<int, QMap<QGCParamID, Parameter> >),
-    paramVector(new QVector<Parameter>)
+    paramList(new QList<QList<Parameter*> >())
 {
 //    params = new QMap<int, QMap<QGCParamID, Parameter> >;
 
@@ -49,18 +78,19 @@ ParameterList::ParameterList()
     QString s;
     for (componentIter = params->begin(); componentIter != params->end(); ++componentIter)
     {
+        paramList->append(QList<Parameter*>());
         for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter)
         {
+            paramList->last().append(paramIter.operator ->());
             s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName();
-            paramVector->append((*paramIter));
             if (opalParams->contains(s))
             {
                 (*paramIter).setOpalID(opalParams->value(s));
-                qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
+//                qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
             }
             else
             {
-                qDebug() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
+                qWarning() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
             }
         }
     }
@@ -71,35 +101,7 @@ ParameterList::ParameterList()
 ParameterList::~ParameterList()
 {
     delete params;
-    delete paramVector;
-}
-
-ParameterList::const_iterator::const_iterator(QList<Parameter> paramList)
-{
-    this->paramList = QList<Parameter>(paramList);
-    index = 0;
-}
-
-ParameterList::const_iterator::const_iterator(const const_iterator &other)
-{
-    paramList = QList<Parameter>(other.paramList);
-    index = other.index;
-}
-
-ParameterList::const_iterator ParameterList::begin() const
-{
-    QList<QMap<QGCParamID, Parameter> > compList = params->values();
-    QList<Parameter> paramList;
-    QList<QMap<QGCParamID, Parameter> >::const_iterator compIter;
-    for (compIter = compList.begin(); compIter != compList.end(); ++compIter)
-        paramList.append((*compIter).values());
-    return const_iterator(paramList);
-}
-
-ParameterList::const_iterator ParameterList::end() const
-{
-    const_iterator iter = begin();
-    return iter+=iter.paramList.size();
+    delete paramList;
 }
 
 /**
@@ -134,8 +136,8 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
                              allocatedVarLen, &maxVarLen, var);
     if (returnValue!=E2BIG)
     {
-        OpalLink::setLastErrorMsg();
-        OpalLink::displayLastErrorMsg();
+//        OpalRT::setLastErrorMsg();
+        OpalRT::OpalErrorMsg::displayLastErrorMsg();
         return;
     }
 
@@ -166,8 +168,8 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
 
     if (returnValue != EOK)
     {
-        OpalLink::setLastErrorMsg();
-        OpalLink::displayLastErrorMsg();
+//        OpalRT::setLastErrorMsg();
+        OpalRT::OpalErrorMsg::displayLastErrorMsg();
         return;
     }
 
@@ -191,8 +193,55 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
 
 }
 
-int ParameterList::count()
+int ParameterList::indexOf(const Parameter &p)
+{
+    // incase p is a copy of the actual parameter we want (i.e., addresses differ)
+    Parameter *pPtr = &((*params)[p.getComponentID()][p.getParamID()]);
+
+    QList<QList<Parameter*> >::const_iterator iter;
+    int index = -1;
+    for (iter = paramList->begin(); iter != paramList->end(); ++iter)
+    {
+        if ((index = (*iter).indexOf(pPtr)) != -1)
+            return index;
+    }
+    return index;
+}
+
+
+ParameterList::const_iterator::const_iterator(QList<Parameter> paramList)
+{
+    this->paramList = QList<Parameter>(paramList);
+    index = 0;
+}
+
+ParameterList::const_iterator::const_iterator(const const_iterator &other)
+{
+    paramList = QList<Parameter>(other.paramList);
+    index = other.index;
+}
+
+ParameterList::const_iterator ParameterList::begin() const
+{
+    QList<QMap<QGCParamID, Parameter> > compList = params->values();
+    QList<Parameter> paramList;
+    QList<QMap<QGCParamID, Parameter> >::const_iterator compIter;
+    for (compIter = compList.begin(); compIter != compList.end(); ++compIter)
+        paramList.append((*compIter).values());
+    return const_iterator(paramList);
+}
+
+ParameterList::const_iterator ParameterList::end() const
 {
     const_iterator iter = begin();
-    return iter.paramList.count();
+    return iter+=iter.paramList.size();
+}
+
+int ParameterList::count()
+{
+    int count = 0;
+    QList<QList<Parameter*> >::const_iterator iter;
+    for (iter = paramList->begin(); iter != paramList->end(); ++iter)
+        count += (*iter).count();
+    return count;
 }
diff --git a/src/comm/ParameterList.h b/src/comm/ParameterList.h
index 2f8596c0d39a1b340186bf1c4d7eb702c567ee3e..a674f8ce9e18a2ccb5d441680de0fc649202f9c6 100644
--- a/src/comm/ParameterList.h
+++ b/src/comm/ParameterList.h
@@ -25,19 +25,13 @@ This file is part of the QGROUNDCONTROL project
 #define PARAMETERLIST_H
 
 #include <QMap>
+#include <QVector>
 
 #include "mavlink_types.h"
 #include "QGCParamID.h"
 #include "Parameter.h"
-#include "QVector"
 #include "OpalRT.h"
 
-// Forward declare ParameterList before including OpalLink.h because a member of type ParameterList is used in OpalLink
-namespace OpalRT
-{
-        class ParameterList;
-}
-#include "OpalLink.h"
 namespace OpalRT
 {
     class ParameterList
@@ -71,21 +65,61 @@ namespace OpalRT
 
         ParameterList();
         ~ParameterList();
-        int setValue(int compid, QGCParamID paramid, float value);
-        float getValue(int compid, QGCParamID paramid);
+
+        /** Count the number of parameters in the list.
+          \return Total number of parameters
+          */
         int count();
-        int indexOf(const Parameter& p) {return paramVector->indexOf(p);}
+
+        /** Find p in the list and return its index.
+          \note In order to use this index to look up p, the component is also needed.
+          \return the index of p or -1 if p is not found
+          \example
+          \code
+          int compid = OpalRT::CONTROLLER_ID;
+          Parameter p("simulinkpath", "simulinkparamname", compid, QGCParamID("PID_GAIN"));
+          ParameterList pList;
+          if ((int index=pList.indexOf(p)) != -1)
+             qDebug() << "PID_GAIN is at index " << index;
+          \endcode          
+          */
+
+        int indexOf(const Parameter& p);
+        bool contains(int compid, QGCParamID paramid) const {return (*params)[compid].contains(paramid);}
+
+        /// Get a parameter from the list
+        const Parameter getParameter(int compid, QGCParamID paramid) const {return (*params)[compid][paramid];}
+        Parameter& getParameter(int compid, QGCParamID paramid) {return (*params)[compid][paramid];}
+        const Parameter getParameter(int compid, int index) const {return *((*paramList)[compid][index]);}        
+
+        /** Convenient syntax for calling OpalRT::Parameter::getParameter() */
+        Parameter& operator()(int compid, QGCParamID paramid) {return getParameter(compid, paramid);}
+        Parameter& operator()(uint8_t compid, QGCParamID paramid) {return getParameter(static_cast<int>(compid), paramid);}
 
         const_iterator begin() const;
         const_iterator end() const;
 
     protected:
+        /** Store the parameters mapped by componentid, and paramid.
+           \code
+           // Look up a parameter
+           int compid = 1;
+           QGCParamID paramid("PID_GAIN");
+           Parameter p = params[compid][paramid];
+           \endcode
+           */
         QMap<int, QMap<QGCParamID, Parameter> > *params;
-        QVector<Parameter> *paramVector;
-        void getParameterList(QMap<QString, unsigned short>*);
-
-
-
+        /**
+          Store pointers to the parameters to allow fast lookup by index.
+          This variable may be changed to const pointers to ensure all changes
+          are made through the map container.
+          */
+        QList<QList<Parameter*> > *paramList;
+        /**
+          Get the list of available parameters from Opal-RT.
+          \param[out] opalParams Map of parameter paths/names to ids which are valid in Opal-RT
+          */
+        void getParameterList(QMap<QString, unsigned short>* opalParams);
     };
 }
 #endif // PARAMETERLIST_H
diff --git a/src/comm/QGCParamID.cc b/src/comm/QGCParamID.cc
index e486d0957a34dfe66c09e99da74df0c361be304e..804835a6540fce7ac406b42d96d11a5acb56cd3e 100644
--- a/src/comm/QGCParamID.cc
+++ b/src/comm/QGCParamID.cc
@@ -1,7 +1,36 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Implementation of class OpalRT::QGCParamID
+ *   @author Bryan Godbolt <godbolt@ualberta.ca>
+ */
+
 #include "QGCParamID.h"
 using namespace OpalRT;
 
-QGCParamID::QGCParamID(const char *paramid):data(paramid)
+QGCParamID::QGCParamID(const char paramid[]):data(paramid)
 {
 }
 
@@ -15,9 +44,8 @@ QGCParamID::QGCParamID(const QGCParamID &other):data(other.data)
 
 }
 
-//int8_t* QGCParamID::toInt8_t()
-//{
-//    int8_t
-//    for (int i=0; ((i < data.size()) && (i < 15)); ++i)
 //
+//QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid)
+//{
+//    return stream << paramid.data;
 //}
diff --git a/src/comm/QGCParamID.h b/src/comm/QGCParamID.h
index 280e3cd54c584f09d6479f74b76a15d0a00832a7..2f9e2a10119c512b2128e095d8ece0bb5cf22bc9 100644
--- a/src/comm/QGCParamID.h
+++ b/src/comm/QGCParamID.h
@@ -21,25 +21,44 @@ This file is part of the QGROUNDCONTROL project
 
 ======================================================================*/
 
+/**
+ * @file
+ *   @brief Stores the paramid used for mavlink
+ *   @author Bryan Godbolt <godbolt@ualberta.ca>
+ */
+
 #ifndef QGCPARAMID_H
 #define QGCPARAMID_H
 
 #include <QString>
+
 #include "mavlink_types.h"
 
+//namespace OpalRT
+//{
+//    class QGCParamID;
+//}
+//
+//QDataStream& operator<<(QDataStream&, const OpalRT::QGCParamID&);
+
 namespace OpalRT
 {
+    /** Stores a param_id for the mavlink parameter packets.  This class adds the convenience
+      of storing the id as a string (e.g., easy comparison).
+      */
     class QGCParamID
     {
+//        friend QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid);
     public:
 
-        QGCParamID(const char *paramid);
+        QGCParamID(const char[]);
         QGCParamID(const QString);
         QGCParamID() {}
         QGCParamID(const QGCParamID& other);
 
         bool operator<(const QGCParamID& other) const {return data<other.data;}
         bool operator==(const QGCParamID& other) const {return data == other.data;}
+        operator QString() const {return data;}
 
         const QString getParamString() const {return static_cast<const QString>(data);}
         int8_t* toInt8_t() const {return (int8_t*)data.toAscii().data();}