diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml
index 3801990f610e5bb1fc053b0e65b5aafd534dfe2c..ea6ba0c3835c72251d471390bed08609111f42c7 100644
--- a/settings/ParameterList.xml
+++ b/settings/ParameterList.xml
@@ -23,7 +23,38 @@
 
   <!-- Paremters for the Pilot Input/Raw RC block -->
   <Block name="ServoInputs">
-
+    <!-- Settings for Aileron Servo -->
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkParameterName="Setpoint0"
+       QGCParamID="AIL_RIGHT_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkParameterName="Setpoint1"
+       QGCParamID="AIL_CENTER_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkParameterName="Setpoint2"
+       QGCParamID="AIL_LEFT_IN"
+       />
+    <!-- Settings for Elevator Servo -->
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
+       SimulinkParameterName="Setpoint0"
+       QGCParamID="ELE_DOWN_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkParameterName="Setpoint1"
+       QGCParamID="ELE_CENTER_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkParameterName="Setpoint2"
+       QGCParamID="ELE_UP_IN"
+       />    
   </Block>
 
   <!-- Parameters for the servo output block -->
diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index ab56524ead787d00dd9127de98ab275c752adb03..3c63fef6c2f25d3365d003ac112e91e438c68d83 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -154,6 +154,13 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
             qDebug() << "GYRO: " << radio.gyro[0] << " " << radio.gyro[1];
             qDebug() << "PITCH: " << radio.pitch[0] << radio.pitch[1] << radio.pitch[2] << radio.pitch[3] << radio.pitch[4];
             qDebug() << "THROTTLE: " << radio.throttle[0] << radio.throttle[1] << radio.throttle[2] << radio.throttle[3] << radio.throttle[4];
+
+            if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(radio.aileron[0]);
+            if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(radio.aileron[1]);
+            if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(radio.aileron[2]);
         }
         break;
 #endif
diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc
index 4f787285406a0024ebc9750c7dd5f41606fdf9b8..45e2b601331e4006a4eb3f723c5146c5cc30dcd1 100644
--- a/src/comm/ParameterList.cc
+++ b/src/comm/ParameterList.cc
@@ -32,7 +32,8 @@ using namespace OpalRT;
 
 ParameterList::ParameterList()
     :params(new QMap<int, QMap<QGCParamID, Parameter> >),
-    paramList(new QList<QList<Parameter*> >())
+    paramList(new QList<QList<Parameter*> >()),
+    reqdServoParams(new QStringList())
 {
 
     QDir settingsDir = QDir(qApp->applicationDirPath());
@@ -40,40 +41,17 @@ ParameterList::ParameterList()
         settingsDir.cdUp();
     settingsDir.cd("settings");
 
+    // Enforce a list of parameters which are necessary for flight
+    reqdServoParams->append("AIL_RIGHT_IN");
+    reqdServoParams->append("AIL_CENTER_IN");
+    reqdServoParams->append("AIL_LEFT_IN");
+    reqdServoParams->append("ELE_DOWN_IN");
+    reqdServoParams->append("ELE_CENTER_IN");
+    reqdServoParams->append("ELE_UP_IN");
+
     QString filename(settingsDir.path() + "/ParameterList.xml");
     if ((QFile::exists(filename)) && open(filename))
     {
-        /* Populate the map with parameter names.  There is no elegant way of doing this so all
-       parameter paths and names must be known at compile time and defined here.
-       Note: This function is written in a way that calls a lot of copy constructors and is
-           therefore not particularly efficient.  However since it is only called once memory
-           and computation time are sacrificed for code clarity when adding and modifying
-           parameters.
-           When defining the path, the trailing slash is necessary
-       */
-        //    Parameter *p;
-        //    /* Component: Navigation Filter */
-        //    p = new Parameter("avionics_src/sm_ampro/NAV_FILT_INIT/",
-        //                            "Value",
-        //                            OpalRT::NAV_ID,
-        //                            QGCParamID("NAV_FILT_INIT"));
-        //    (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p);
-        //    delete p;
-        //
-        //    p = new Parameter("avionics_src/sm_ampro/Gain/",
-        //                            "Gain",
-        //                            OpalRT::NAV_ID,
-        //                            QGCParamID("TEST_OUTP_GAIN"));
-        //    (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p);
-        //    delete p;
-        //
-        //    /* Component: Log Facility */
-        //    p = new Parameter("avionics_src/sm_ampro/LOG_FILE_ON/",
-        //                            "Value",
-        //                            OpalRT::LOG_ID,
-        //                            QGCParamID("LOG_FILE_ON"));
-        //    (*params)[OpalRT::LOG_ID].insert(p->getParamID(), *p);
-        //    delete p;
 
         /* Get a list of the available parameters from opal-rt */
         QMap<QString, unsigned short> *opalParams = new QMap<QString, unsigned short>;
@@ -338,12 +316,26 @@ void ParameterList::parseBlock(const QDomElement &block)
                                   static_cast<uint8_t>(id),
                                   QGCParamID(e.attribute("QGCParamID")));
                 (*params)[id].insert(p->getParamID(), *p);
+                if (reqdServoParams->contains((QString)p->getParamID()))
+                    reqdServoParams->removeAt(reqdServoParams->indexOf((QString)p->getParamID()));
+
                 delete p;
-            }
+
+
+            }            
             else
             {
                 qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc in block" << block.attribute("name");
             }
         }
 
+        if (!reqdServoParams->empty())
+        {
+            qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
+            foreach(QString s, *reqdServoParams)
+            {
+                qDebug() << s;
+            }
+        }
+
 }
diff --git a/src/comm/ParameterList.h b/src/comm/ParameterList.h
index f93ffac779a3a4b96f55408ab7bc655409812bae..5a891f20b11a179409e00ce6cbe84f5acdb71726 100644
--- a/src/comm/ParameterList.h
+++ b/src/comm/ParameterList.h
@@ -31,6 +31,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QDir>
 #include <QApplication>
 #include <QtXml>
+#include <QStringList>
 
 #include "mavlink_types.h"
 #include "QGCParamID.h"
@@ -119,6 +120,10 @@ namespace OpalRT
           are made through the map container.
           */
         QList<QList<Parameter*> > *paramList;
+        /**
+          List of parameters which are necessary to control the servos.
+          */
+        QStringList *reqdServoParams;
         /**
           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