diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml
index ea6ba0c3835c72251d471390bed08609111f42c7..b1cb8f47e903a5555a305b69ee54ab5832cb31b9 100644
--- a/settings/ParameterList.xml
+++ b/settings/ParameterList.xml
@@ -46,15 +46,95 @@
        QGCParamID="ELE_DOWN_IN"
        />
     <Parameter
-       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
        SimulinkParameterName="Setpoint1"
        QGCParamID="ELE_CENTER_IN"
        />
     <Parameter
-       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
        SimulinkParameterName="Setpoint2"
        QGCParamID="ELE_UP_IN"
+       />
+    <!-- Settings for Throttle Servo -->
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
+       SimulinkParameterName="Setpoint0"
+       QGCParamID="THR_SET0_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
+       SimulinkParameterName="Setpoint1"
+       QGCParamID="THR_SET1_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
+       SimulinkParameterName="Setpoint2"
+       QGCParamID="THR_SET2_IN"
+       />    
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
+       SimulinkParameterName="Setpoint3"
+       QGCParamID="THR_SET3_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
+       SimulinkParameterName="Setpoint4"
+       QGCParamID="THR_SET4_IN"
+       />
+    <!-- Settings for Rudder Servo -->
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
+       SimulinkParameterName="Setpoint0"
+       QGCParamID="RUD_LEFT_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
+       SimulinkParameterName="Setpoint1"
+       QGCParamID="RUD_CENTER_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
+       SimulinkParameterName="Setpoint2"
+       QGCParamID="RUD_RIGHT_IN"
+       />
+    <!-- Settings for Gyro Mode/Gain Switch -->
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/GyroInput/"
+       SimulinkParameterName="Setpoint0"
+       QGCParamID="GYRO_DEF_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/GyroInput/"
+       SimulinkParameterName="Setpoint1"
+       QGCParamID="GYRO_TOG_IN"
        />    
+    <!-- Settings for Pitch Servo -->
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
+       SimulinkParameterName="Setpoint0"
+       QGCParamID="PIT_SET0_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
+       SimulinkParameterName="Setpoint1"
+       QGCParamID="PIT_SET1_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
+       SimulinkParameterName="Setpoint2"
+       QGCParamID="PIT_SET2_IN"
+       />    
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
+       SimulinkParameterName="Setpoint3"
+       QGCParamID="PIT_SET3_IN"
+       />
+    <Parameter
+       SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
+       SimulinkParameterName="Setpoint4"
+       QGCParamID="PIT_SET4_IN"
+       />
+
   </Block>
 
   <!-- Parameters for the servo output block -->
diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index 3c63fef6c2f25d3365d003ac112e91e438c68d83..c096a24abf7e0bd2ed4d6fea49775ff96a890f3f 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -147,20 +147,62 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
         {
             mavlink_radio_calibration_t radio;
             mavlink_msg_radio_calibration_decode(&msg, &radio);
-            qDebug() << "RADIO CALIBRATION RECEIVED";
-            qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
-            qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2];
-            qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2];
-            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];
-
+//            qDebug() << "RADIO CALIBRATION RECEIVED";
+//            qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
+//            qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2];
+//            qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2];
+//            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];
+
+            /* AILERON SERVO */
             if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN"))
-                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(radio.aileron[0]);
+                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
             if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN"))
-                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(radio.aileron[1]);
+                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
             if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN"))
-                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(radio.aileron[2]);
+                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
+            /* ELEVATOR SERVO */
+            if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "ELE_UP_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "ELE_UP_IN").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
+            /* THROTTLE SERVO */
+            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET1_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET1_IN").setValue(((radio.throttle[1]>900 /*in us?*/)?radio.throttle[1]/1000:radio.throttle[1]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET2_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET2_IN").setValue(((radio.throttle[2]>900 /*in us?*/)?radio.throttle[2]/1000:radio.throttle[2]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET3_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET3_IN").setValue(((radio.throttle[3]>900 /*in us?*/)?radio.throttle[3]/1000:radio.throttle[3]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET4_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET4_IN").setValue(((radio.throttle[4]>900 /*in us?*/)?radio.throttle[4]/1000:radio.throttle[4]));
+            /* RUDDER SERVO */
+            if (params->contains(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN").setValue(((radio.rudder[0]>900 /*in us?*/)?radio.rudder[0]/1000:radio.rudder[0]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN").setValue(((radio.rudder[1]>900 /*in us?*/)?radio.rudder[1]/1000:radio.rudder[1]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN").setValue(((radio.rudder[2]>900 /*in us?*/)?radio.rudder[2]/1000:radio.rudder[2]));
+            /* GYRO MODE/GAIN SWITCH */
+            if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN").setValue(((radio.gyro[0]>900 /*in us?*/)?radio.gyro[0]/1000:radio.gyro[0]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN").setValue(((radio.gyro[1]>900 /*in us?*/)?radio.gyro[1]/1000:radio.gyro[1]));
+            /* PITCH SERVO */
+            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET0_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET0_IN").setValue(((radio.pitch[0]>900 /*in us?*/)?radio.pitch[0]/1000:radio.pitch[0]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET1_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET1_IN").setValue(((radio.pitch[1]>900 /*in us?*/)?radio.pitch[1]/1000:radio.pitch[1]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET2_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET2_IN").setValue(((radio.pitch[2]>900 /*in us?*/)?radio.pitch[2]/1000:radio.pitch[2]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET3_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET3_IN").setValue(((radio.pitch[3]>900 /*in us?*/)?radio.pitch[3]/1000:radio.pitch[3]));
+            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET4_IN"))
+                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET4_IN").setValue(((radio.pitch[4]>900 /*in us?*/)?radio.pitch[4]/1000:radio.pitch[4]));
         }
         break;
 #endif
@@ -291,17 +333,23 @@ void OpalLink::getSignals()
                                              duty2PulseMicros(values[OpalRT::RAW_CHANNEL_6]),
                                              duty2PulseMicros(values[OpalRT::RAW_CHANNEL_7]),
                                              duty2PulseMicros(values[OpalRT::RAW_CHANNEL_8]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_1]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_2]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_3]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_4]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_5]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_6]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_7]),
-                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_8]),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_1], OpalRT::NORM_CHANNEL_1),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_2], OpalRT::NORM_CHANNEL_2),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_3], OpalRT::NORM_CHANNEL_3),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_4], OpalRT::NORM_CHANNEL_4),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_5], OpalRT::NORM_CHANNEL_5),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_6], OpalRT::NORM_CHANNEL_6),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_7], OpalRT::NORM_CHANNEL_7),
+                                             rescaleNorm(values[OpalRT::NORM_CHANNEL_8], OpalRT::NORM_CHANNEL_8),
                                              0 //rssi unused
                                              );
                 receiveMessage(rc);
+//                qDebug() << __FILE__  << __LINE__ << "Aileron: " << values[OpalRT::NORM_CHANNEL_1];
+//                qDebug() << __FILE__  << __LINE__ << "Elevator: " << values[OpalRT::NORM_CHANNEL_2];
+//                qDebug() << __FILE__  << __LINE__ << "Rudder: " << values[OpalRT::NORM_CHANNEL_4];
+//                qDebug() << __FILE__  << __LINE__ << "Pitch: " << values[OpalRT::NORM_CHANNEL_6];
+
+
             }
         }        
         else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
@@ -357,9 +405,26 @@ uint16_t OpalLink::duty2PulseMicros(double duty)
     return static_cast<uint16_t>(duty/70*1000000);
 }
 
-uint8_t OpalLink::rescaleNorm(double norm)
+uint8_t OpalLink::rescaleNorm(double norm, int ch)
 {
-    return static_cast<uint8_t>((norm+1)/2*255);
+    switch(ch)
+    {
+    case OpalRT::NORM_CHANNEL_1:
+    case OpalRT::NORM_CHANNEL_2:
+    case OpalRT::NORM_CHANNEL_4:
+    default:
+        // three setpoints
+        return static_cast<uint8_t>((norm+1)/2*255);
+        break;
+    case OpalRT::NORM_CHANNEL_5:
+        //two setpoints
+    case OpalRT::NORM_CHANNEL_3:
+    case OpalRT::NORM_CHANNEL_6:        
+        return static_cast<uint8_t>(norm*255);
+        break;
+    }
+
+
 }
 
 
diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h
index 841ec7a1ee15528bcbc17c879b92b292b73ef063..0e128c3cb38848920ad47fc193c34446a9bc3d44 100644
--- a/src/comm/OpalLink.h
+++ b/src/comm/OpalLink.h
@@ -153,7 +153,7 @@ protected:
     unsigned short opalInstID;
 
     uint16_t duty2PulseMicros(double duty);
-    uint8_t rescaleNorm(double norm);
+    uint8_t rescaleNorm(double norm, int ch);
 
     bool sendRCValues;
 };
diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc
index 45e2b601331e4006a4eb3f723c5146c5cc30dcd1..682489ee54464aaf4e47b0efc94d9f8bfabb7d4e 100644
--- a/src/comm/ParameterList.cc
+++ b/src/comm/ParameterList.cc
@@ -48,6 +48,9 @@ ParameterList::ParameterList()
     reqdServoParams->append("ELE_DOWN_IN");
     reqdServoParams->append("ELE_CENTER_IN");
     reqdServoParams->append("ELE_UP_IN");
+    reqdServoParams->append("RUD_LEFT_IN");
+    reqdServoParams->append("RUD_CENTER_IN");
+    reqdServoParams->append("RUD_RIGHT_IN");
 
     QString filename(settingsDir.path() + "/ParameterList.xml");
     if ((QFile::exists(filename)) && open(filename))
diff --git a/src/ui/RadioCalibration/AbstractCalibrator.cc b/src/ui/RadioCalibration/AbstractCalibrator.cc
index c57a46e0757c29224361e21a5e9010ed1f0b4e16..82df4a4c1074ae3cda5641a622898f1b62b86168 100644
--- a/src/ui/RadioCalibration/AbstractCalibrator.cc
+++ b/src/ui/RadioCalibration/AbstractCalibrator.cc
@@ -30,6 +30,7 @@ float AbstractCalibrator::logExtrema()
             if (log->value(i) < extrema)
                 extrema = log->value(i);
         }
+        extrema -= 5; // add 5us to prevent integer overflow
     }
     else
     {
@@ -38,6 +39,7 @@ float AbstractCalibrator::logExtrema()
             if (log->value(i) > extrema)
                 extrema = log->value(i);
         }
+        extrema += 5; // subtact 5us to prevent integer overflow
     }
 
     return extrema;