diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 588cd9e6dad1d0edc546165785c11e69daa102ac..3341c6b9c2ceb78de1e1fa6a08d743eda32c5b86 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -117,7 +117,7 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
                 paramUnion.param_int32 = (int32_t)paramValue.param_value;
                 break;
             case MAV_PARAM_TYPE_REAL32:
-                // Already in param_float
+                paramUnion.param_float = paramValue.param_value;
                 break;
             default:
                 qCritical() << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
@@ -125,6 +125,8 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
         
         paramValue.param_value = paramUnion.param_float;
         
+        mavlink_msg_param_value_encode(message->sysid, message->compid, message, &paramValue);
+        
     } else if (message->msgid == MAVLINK_MSG_ID_PARAM_SET) {
         mavlink_param_set_t     paramSet;
         mavlink_param_union_t   paramUnion;
@@ -161,6 +163,8 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
             default:
                 qCritical() << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
         }
+        
+        mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
     }
 
     // FIXME: Need to implement mavlink message severity adjustment
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 161177ee00cd7a84a829a1bee9af02aee977265d..7a1b4416ed5693a22ed0307ed5a74df79f0d6d81 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -260,20 +260,16 @@ void Vehicle::_sendMessage(mavlink_message_t message)
         if (link->isConnected()) {
             MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
             
+            // Give the plugin a chance to adjust
+            _firmwarePlugin->adjustMavlinkMessage(&message);
+            
             // Write message into buffer, prepending start sign
             uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
             int len = mavlink_msg_to_send_buffer(buffer, &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 {
-                qWarning() << "Link not connected";
-            }
+            link->writeBytes((const char*)buffer, len);
         }
     }
 }