Added APM Firmware Plugin

- Also added new adjustMavlinkMessage support

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 \

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);
 }

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)

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

+/*=====================================================================
+ 
+ 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
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

+/*=====================================================================
+ 
+ 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

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) { }
 };

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();
     }
 }

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
+}

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);

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
+}

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);

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)

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 {

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);

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: