Merge pull request #1885 from DonLakeFlyer/AdjustFix

Fix APM adjustMavlinkMessage

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

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -188,3 +188,8 @@ void PX4FirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
     
     // PX4 Flight Stack plugin does no message adjustment
 }
+
+bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+{
+    return capabilities == MavCmdPreflightStorageCapability;
+}

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -38,7 +38,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin
 public:
     // Overrides from FirmwarePlugin
     
-    virtual bool isCapable(FirmwareCapabilities capabilities) { Q_UNUSED(capabilities); return false; }
+    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);

src/Vehicle/Vehicle.cc

@@ -260,20 +260,17 @@ void Vehicle::_sendMessage(mavlink_message_t message)
         if (link->isConnected()) {
             MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
             
+            // Give the plugin a chance to adjust
+            _firmwarePlugin->adjustMavlinkMessage(&message);
+            
+            static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
+            mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
+            
             // 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);
         }
     }
 }