diff --git a/QGCApplication.pro b/QGCApplication.pro
index 1087d22aafabeb85470fb9bf594b5bff445eefa6..58f700c779b912d2b5c1641d0a278965f7d5ab03 100644
--- a/QGCApplication.pro
+++ b/QGCApplication.pro
@@ -578,6 +578,7 @@ HEADERS+= \
     src/AutoPilotPlugins/PX4/SensorsComponentController.h \
     src/FirmwarePlugin/FirmwarePluginManager.h \
     src/FirmwarePlugin/FirmwarePlugin.h \
+    src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
     src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h \
     src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
     src/Vehicle/MultiVehicleManager.h \
@@ -614,6 +615,7 @@ SOURCES += \
     src/AutoPilotPlugins/PX4/SafetyComponent.cc \
     src/AutoPilotPlugins/PX4/SensorsComponent.cc \
     src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
+    src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
     src/FirmwarePlugin/FirmwarePluginManager.cc \
     src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc \
     src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \
diff --git a/src/FactSystem/FactSystemTestGeneric.cc b/src/FactSystem/FactSystemTestGeneric.cc
index 80536cc3d39369f8088fa947c27070c5595b6aa6..2bb68940248499cc7fa856b6e8bf671bbd7e7798 100644
--- a/src/FactSystem/FactSystemTestGeneric.cc
+++ b/src/FactSystem/FactSystemTestGeneric.cc
@@ -38,5 +38,5 @@ FactSystemTestGeneric::FactSystemTestGeneric(void)
 void FactSystemTestGeneric::init(void)
 {
     UnitTest::init();
-    _init(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _init(MAV_AUTOPILOT_GENERIC);
 }
diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc
index 0c152ccbccf69c5573fa3f508b9218ae5b83fbca..fdc526d767b8f1fa8e1677a1705fd8f06b8db854 100644
--- a/src/FactSystem/ParameterLoader.cc
+++ b/src/FactSystem/ParameterLoader.cc
@@ -30,6 +30,7 @@
 #include "QGCApplication.h"
 #include "QGCMessageBox.h"
 #include "UASMessageHandler.h"
+#include "FirmwarePlugin.h"
 
 #include <QFile>
 #include <QDebug>
@@ -499,8 +500,6 @@ void ParameterLoader::_readParameterRaw(int componentId, const QString& paramNam
 
 void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value)
 {
-    bool floatHack = _vehicle->firmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA;
-
     mavlink_param_set_t     p;
     mavlink_param_union_t   union_value;
     
@@ -509,43 +508,23 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
     
     switch (factType) {
         case FactMetaData::valueTypeUint8:
-            if (floatHack) {
-                union_value.param_float = (uint8_t)value.toUInt();
-            } else {                
-                union_value.param_uint8 = (uint8_t)value.toUInt();
-            }
+            union_value.param_uint8 = (uint8_t)value.toUInt();
             break;
             
         case FactMetaData::valueTypeInt8:
-            if (floatHack) {
-                union_value.param_float = (int8_t)value.toInt();
-            } else {
-                union_value.param_int8 = (int8_t)value.toInt();
-            }
+            union_value.param_int8 = (int8_t)value.toInt();
             break;
             
         case FactMetaData::valueTypeUint16:
-            if (floatHack) {
-                union_value.param_float = (uint16_t)value.toUInt();
-            } else {
-                union_value.param_uint16 = (uint16_t)value.toUInt();
-            }
+            union_value.param_uint16 = (uint16_t)value.toUInt();
             break;
             
         case FactMetaData::valueTypeInt16:
-            if (floatHack) {
-                union_value.param_float = (int16_t)value.toInt();
-            } else {
-                union_value.param_int16 = (int16_t)value.toInt();
-            }
+            union_value.param_int16 = (int16_t)value.toInt();
             break;
             
         case FactMetaData::valueTypeUint32:
-            if (floatHack) {
-                union_value.param_float = (uint32_t)value.toUInt();
-            } else {
-                union_value.param_uint32 = (uint32_t)value.toUInt();
-            }
+            union_value.param_uint32 = (uint32_t)value.toUInt();
             break;
             
         case FactMetaData::valueTypeFloat:
@@ -557,11 +536,7 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
             // fall through
             
         case FactMetaData::valueTypeInt32:
-            if (floatHack) {
-                union_value.param_float = (int32_t)value.toInt();
-            } else {
-                union_value.param_int32 = (int32_t)value.toInt();
-            }
+            union_value.param_int32 = (int32_t)value.toInt();
             break;
     }
     
@@ -578,10 +553,14 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
 
 void ParameterLoader::_saveToEEPROM(void)
 {
-    mavlink_message_t msg;
-    mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
-    _vehicle->sendMessage(msg);
-    qCDebug(ParameterLoaderLog) << "_saveToEEPROM";
+    if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) {
+        mavlink_message_t msg;
+        mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
+        _vehicle->sendMessage(msg);
+        qCDebug(ParameterLoaderLog) << "_saveToEEPROM";
+    } else {
+        qCDebug(ParameterLoaderLog) << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable";
+    }
 }
 
 QString ParameterLoader::readParametersFromStream(QTextStream& stream)
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
new file mode 100644
index 0000000000000000000000000000000000000000..588cd9e6dad1d0edc546165785c11e69daa102ac
--- /dev/null
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -0,0 +1,167 @@
+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2015 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
+///     @author Don Gagne <don@thegagnes.com>
+
+#include "APMFirmwarePlugin.h"
+#include "Generic/GenericFirmwarePlugin.h"
+
+#include <QDebug>
+
+IMPLEMENT_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin)
+
+APMFirmwarePlugin::APMFirmwarePlugin(QObject* parent) :
+    FirmwarePlugin(parent)
+{
+    
+}
+
+bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+{
+    Q_UNUSED(capabilities);
+    
+    // FIXME: No capabilitis yet supported
+    
+    return false;
+}
+
+QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
+{
+    Q_UNUSED(vehicle);
+    
+    return QList<VehicleComponent*>();
+}
+
+QStringList APMFirmwarePlugin::flightModes(void)
+{
+    // FIXME: NYI
+    
+    qWarning() << "APMFirmwarePlugin::flightModes not supported";
+    
+    return QStringList();
+}
+
+QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
+{
+    // FIXME: Nothing more than generic support yet
+    return GenericFirmwarePlugin::instance()->flightMode(base_mode, custom_mode);
+}
+
+bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
+{
+    Q_UNUSED(flightMode);
+    Q_UNUSED(base_mode);
+    Q_UNUSED(custom_mode);
+    
+    qWarning() << "APMFirmwarePlugin::setFlightMode called on base class, not supported";
+    
+    return false;
+}
+
+int APMFirmwarePlugin::manualControlReservedButtonCount(void)
+{
+    // We don't know whether the firmware is going to used any of these buttons.
+    // So reserve them all.
+    return -1;
+}
+
+void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
+{
+    if (message->msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
+        mavlink_param_value_t paramValue;
+        mavlink_param_union_t paramUnion;
+        
+        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
+        // type they are. Fix that up to correct usage.
+        
+        mavlink_msg_param_value_decode(message, &paramValue);
+        
+        switch (paramValue.param_type) {
+            case MAV_PARAM_TYPE_UINT8:
+                paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
+                break;
+            case MAV_PARAM_TYPE_INT8:
+                paramUnion.param_int8 = (int8_t)paramValue.param_value;
+                break;
+            case MAV_PARAM_TYPE_UINT16:
+                paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
+                break;
+            case MAV_PARAM_TYPE_INT16:
+                paramUnion.param_int16 = (int16_t)paramValue.param_value;
+                break;
+            case MAV_PARAM_TYPE_UINT32:
+                paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
+                break;
+            case MAV_PARAM_TYPE_INT32:
+                paramUnion.param_int32 = (int32_t)paramValue.param_value;
+                break;
+            case MAV_PARAM_TYPE_REAL32:
+                // Already in param_float
+                break;
+            default:
+                qCritical() << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
+        }
+        
+        paramValue.param_value = paramUnion.param_float;
+        
+    } else if (message->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+        mavlink_param_set_t     paramSet;
+        mavlink_param_union_t   paramUnion;
+        
+        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
+        // type they are. Fix it back to the wrong way on the way out.
+        
+        mavlink_msg_param_set_decode(message, &paramSet);
+        
+        paramUnion.param_float = paramSet.param_value;
+
+        switch (paramSet.param_type) {
+            case MAV_PARAM_TYPE_UINT8:
+                paramSet.param_value = paramUnion.param_uint8;
+                break;
+            case MAV_PARAM_TYPE_INT8:
+                paramSet.param_value = paramUnion.param_int8;
+                break;
+            case MAV_PARAM_TYPE_UINT16:
+                paramSet.param_value = paramUnion.param_uint16;
+                break;
+            case MAV_PARAM_TYPE_INT16:
+                paramSet.param_value = paramUnion.param_int16;
+                break;
+            case MAV_PARAM_TYPE_UINT32:
+                paramSet.param_value = paramUnion.param_uint32;
+                break;
+            case MAV_PARAM_TYPE_INT32:
+                paramSet.param_value = paramUnion.param_int32;
+                break;
+            case MAV_PARAM_TYPE_REAL32:
+                // Already in param_float
+                break;
+            default:
+                qCritical() << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
+        }
+    }
+
+    // FIXME: Need to implement mavlink message severity adjustment
+}
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..524a36f3850bee872d70fc5fbca5fd7aec62406d
--- /dev/null
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -0,0 +1,54 @@
+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2014 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
+///     @author Don Gagne <don@thegagnes.com>
+
+#ifndef APMFirmwarePlugin_H
+#define APMFirmwarePlugin_H
+
+#include "FirmwarePlugin.h"
+
+class APMFirmwarePlugin : public FirmwarePlugin
+{
+    Q_OBJECT
+
+    DECLARE_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin)
+    
+public:
+    // Overrides from FirmwarePlugin
+    
+    virtual bool isCapable(FirmwareCapabilities capabilities);
+    virtual QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle);
+    virtual QStringList flightModes(void);
+    virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
+    virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
+    virtual int manualControlReservedButtonCount(void);
+    virtual void adjustMavlinkMessage(mavlink_message_t* message);
+
+private:
+    /// All access to singleton is through AutoPilotPluginManager::instance
+    APMFirmwarePlugin(QObject* parent = NULL);
+};
+
+#endif
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index d718fc8e7c00aa794a40fad579d113404b5ed906..c84a8ece94e9a9b6dd71726f495faeeb571bbfc5 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -51,7 +51,9 @@ class FirmwarePlugin : public QGCSingleton
 public:
     /// Set of optional capabilites which firmware may support
     typedef enum {
-        SetFlightModeCapability,
+        SetFlightModeCapability,            ///< FirmwarePlugin::setFlightMode method is supported
+        MavCmdPreflightStorageCapability,   ///< MAV_CMD_PREFLIGHT_STORAGE is supported
+        
     } FirmwareCapabilities;
     
     /// @return true: Firmware supports all specified capabilites
@@ -76,12 +78,20 @@ public:
     ///     @param[out] custom_mode Custom mode for SET_MODE mavlink message
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) = 0;
     
+    /// FIXME: This isn't quite correct being here. All code for Joystick support is currently firmware specific
+    /// not just this. I'm going to try to change that. If not, this will need to be removed.
     /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
     /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
     /// The remainder can be assigned to Vehicle actions.
     /// @return -1: reserver all buttons, >0 number of buttons to reserve
     virtual int manualControlReservedButtonCount(void) = 0;
     
+    /// Called before any mavlink message is processed by Vehicle such taht the firmwre plugin
+    /// can adjust any message characteristics. This is handy to adjust or differences in mavlink
+    /// spec implementations such that the base code can remain mavlink generic.
+    ///     @param message[in,out] Mavlink message to adjust if needed.
+    virtual void adjustMavlinkMessage(mavlink_message_t* message) = 0;
+    
 protected:
     FirmwarePlugin(QObject* parent = NULL) : QGCSingleton(parent) { }
 };
diff --git a/src/FirmwarePlugin/FirmwarePluginManager.cc b/src/FirmwarePlugin/FirmwarePluginManager.cc
index 06c36a889b0c13eff2f20fbcfd6e6f7b938017fb..b64edde08a4105b689a97341c32a8e79e6cb2506 100644
--- a/src/FirmwarePlugin/FirmwarePluginManager.cc
+++ b/src/FirmwarePlugin/FirmwarePluginManager.cc
@@ -26,6 +26,7 @@
 
 #include "FirmwarePluginManager.h"
 #include "Generic/GenericFirmwarePlugin.h"
+#include "APM/APMFirmwarePlugin.h"
 #include "PX4/PX4FirmwarePlugin.h"
 
 IMPLEMENT_QGC_SINGLETON(FirmwarePluginManager, FirmwarePluginManager)
@@ -43,9 +44,12 @@ FirmwarePluginManager::~FirmwarePluginManager()
 
 FirmwarePlugin* FirmwarePluginManager::firmwarePluginForAutopilot(MAV_AUTOPILOT autopilotType)
 {
-    if (autopilotType == MAV_AUTOPILOT_PX4) {
-        return PX4FirmwarePlugin::instance();
-    } else {
-        return GenericFirmwarePlugin::instance();
+    switch (autopilotType) {
+        case MAV_AUTOPILOT_ARDUPILOTMEGA:
+            return APMFirmwarePlugin::instance();
+        case MAV_AUTOPILOT_PX4:
+            return PX4FirmwarePlugin::instance();
+        default:
+            return GenericFirmwarePlugin::instance();
     }
 }
diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc
index cb47d987290d059ccf5b7cb9984b8b493ff5d149..5b11fdf1c3702b3974ec820b74d2ee1cae33fd40 100644
--- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc
@@ -96,3 +96,10 @@ int GenericFirmwarePlugin::manualControlReservedButtonCount(void)
     // So reserve them all.
     return -1;
 }
+
+void GenericFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
+{
+    Q_UNUSED(message);
+    
+    // Generic plugin does no message adjustment
+}
diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h
index 109e14ba4da4815495a99da5bb9072d7b19cad66..505a3de3df34b2f3ba9cddbc31cf3ddcfd29d736 100644
--- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h
+++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h
@@ -44,7 +44,8 @@ public:
     virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
     virtual int manualControlReservedButtonCount(void);
-    
+    virtual void adjustMavlinkMessage(mavlink_message_t* message);
+
 private:
     /// All access to singleton is through AutoPilotPluginManager::instance
     GenericFirmwarePlugin(QObject* parent = NULL);
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index f8d3aa966afcb8573b60fa11918f90461b700620..9f64cd692119e5585ed8430137b28c4c0557b150 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -181,3 +181,10 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
 {
     return 8;   // 8 buttons reserved for rc switch simulation
 }
+
+void PX4FirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
+{
+    Q_UNUSED(message);
+    
+    // PX4 Flight Stack plugin does no message adjustment
+}
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
index ea3314895bf70cc39297e1bf5b042a4bfc149d92..4381fb4144f01a99e1369083703fca452dad35cd 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
@@ -44,7 +44,8 @@ public:
     virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
     virtual int manualControlReservedButtonCount(void);
-    
+    virtual void adjustMavlinkMessage(mavlink_message_t* message);
+
 private:
     /// All access to singleton is through AutoPilotPluginManager::instance
     PX4FirmwarePlugin(QObject* parent = NULL);
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index f10fe697f9a471f5057c9ee2f3bf482708fd5ce0..e1ceee7e00e59304e52e166ebdd7c7e59b8265cb 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -74,6 +74,7 @@
 #include "FirmwarePluginManager.h"
 #include "MultiVehicleManager.h"
 #include "Generic/GenericFirmwarePlugin.h"
+#include "APM/APMFirmwarePlugin.h"
 #include "PX4/PX4FirmwarePlugin.h"
 #include "Vehicle.h"
 #include "MavlinkQmlSingleton.h"
@@ -547,6 +548,7 @@ void QGCApplication::_createSingletons(void)
     
     // No dependencies
     firmwarePlugin = PX4FirmwarePlugin::_createSingleton();
+    firmwarePlugin = APMFirmwarePlugin::_createSingleton();
     
     // No dependencies
     FirmwarePluginManager* firmwarePluginManager = FirmwarePluginManager::_createSingleton();
@@ -628,6 +630,7 @@ void QGCApplication::_destroySingletons(void)
     FirmwarePluginManager::_deleteSingleton();
     GenericFirmwarePlugin::_deleteSingleton();
     PX4FirmwarePlugin::_deleteSingleton();
+    APMFirmwarePlugin::_deleteSingleton();
 }
 
 void QGCApplication::informationMessageBoxOnMainThread(const QString& title, const QString& msg)
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 8188cb292389efe475eb9f93d8610ffd6e95277f..161177ee00cd7a84a829a1bee9af02aee977265d 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -200,6 +200,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         _addLink(link);
     }
     
+    // Give the plugin a change to adjust the message contents
+    _firmwarePlugin->adjustMavlinkMessage(&message);
+    
     emit mavlinkMessageReceived(message);
     
     _uas->receiveMessage(message);
@@ -263,6 +266,9 @@ void Vehicle::_sendMessage(mavlink_message_t message)
             static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
             mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
             
+            // Give the plugin a chance to adjust
+            _firmwarePlugin->adjustMavlinkMessage(&message);
+            
             if (link->isConnected()) {
                 link->writeBytes((const char*)buffer, len);
             } else {
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 06a362a3e51d500331ed15f780037b566c2f9101..1c30ed9bc221eca2cdcd47cd8f1bd428b9cacda6 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -145,6 +145,9 @@ public:
     /// Provides access to uas from vehicle. Temporary workaround until AutoPilotPlugin is fully phased out.
     AutoPilotPlugin* autopilotPlugin(void) { return _autopilotPlugin; }
     
+    /// Provides access to the Firmware Plugin for this Vehicle
+    FirmwarePlugin* firmwarePlugin(void) { return _firmwarePlugin; }
+    
     QList<LinkInterface*> links(void);
     
     int manualControlReservedButtonCount(void);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index b0d3cec41ea314b4771d0580f09975b88bfc4ee7..3c815878e281b6d6b0d79b444eb1a23ed760e61b 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2286,54 +2286,30 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
     QVariant paramValue;
 
     // Insert with correct type
-    // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling.
 
     switch (rawValue.param_type) {
         case MAV_PARAM_TYPE_REAL32:
-            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-                paramValue = QVariant(paramUnion.param_float);
-            } else {
-                paramValue = QVariant(paramUnion.param_float);
-            }
+            paramValue = QVariant(paramUnion.param_float);
             break;
 
         case MAV_PARAM_TYPE_UINT8:
-            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-                paramValue = QVariant((unsigned short)paramUnion.param_float);
-            } else {
-                paramValue = QVariant(paramUnion.param_uint8);
-            }
+            paramValue = QVariant(paramUnion.param_uint8);
             break;
 
         case MAV_PARAM_TYPE_INT8:
-            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-                paramValue = QVariant((short)paramUnion.param_float);
-            } else  {
-                paramValue = QVariant(paramUnion.param_int8);
-            }
+            paramValue = QVariant(paramUnion.param_int8);
             break;
 
         case MAV_PARAM_TYPE_INT16:
-            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-                paramValue = QVariant((short)paramUnion.param_float);
-            } else {
-                paramValue = QVariant(paramUnion.param_int16);
-            }
+            paramValue = QVariant(paramUnion.param_int16);
             break;
 
         case MAV_PARAM_TYPE_UINT32:
-            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-                paramValue = QVariant((unsigned int)paramUnion.param_float);
-            } else {
-                paramValue = QVariant(paramUnion.param_uint32);
-            }
+            paramValue = QVariant(paramUnion.param_uint32);
             break;
+            
         case MAV_PARAM_TYPE_INT32:
-            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-                paramValue = QVariant((int)paramUnion.param_float);
-            } else {
-                paramValue = QVariant(paramUnion.param_int32);
-            }
+            paramValue = QVariant(paramUnion.param_int32);
             break;
 
         default: