Merge pull request #3077 from DonLakeFlyer/SoloFixes

Solo fixes

src/AutoPilotPlugins/APM/APMSensorsComponent.cc

@@ -67,6 +67,7 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const
     
     // Compass triggers
     triggers << "COMPASS_DEV_ID" << "COMPASS_DEV_ID2" << "COMPASS_DEV_ID3"
+             << "COMPASS_USE" << "COMPASS_USE2" << "COMPASS_USE3"
              << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X"
              << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" << "COMPASS_OFS2_X"
              << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X";
@@ -103,16 +104,19 @@ bool APMSensorsComponent::compassSetupNeeded(void) const
 {
     const size_t cCompass = 3;
     const size_t cOffset = 3;
-    QStringList devicesIds;
+    QStringList rgDevicesIds;
+    QStringList rgCompassUse;
     QStringList rgOffsets[cCompass];
 
-    devicesIds << QStringLiteral("COMPASS_DEV_ID") << QStringLiteral("COMPASS_DEV_ID2") << QStringLiteral("COMPASS_DEV_ID3");
+    rgDevicesIds << QStringLiteral("COMPASS_DEV_ID") << QStringLiteral("COMPASS_DEV_ID2") << QStringLiteral("COMPASS_DEV_ID3");
+    rgCompassUse << QStringLiteral("COMPASS_USE") << QStringLiteral("COMPASS_USE2") << QStringLiteral("COMPASS_USE3");
     rgOffsets[0] << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_Y") << QStringLiteral("COMPASS_OFS_Z");
     rgOffsets[1] << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_Y") << QStringLiteral("COMPASS_OFS2_Z");
     rgOffsets[2] << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_Y") << QStringLiteral("COMPASS_OFS3_Z");
 
     for (size_t i=0; i<cCompass; i++) {
-        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, devicesIds[i])->rawValue().toInt() != 0) {
+        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgDevicesIds[i])->rawValue().toInt() != 0 &&
+            _autopilot->getParameterFact(FactSystem::defaultComponentId, rgCompassUse[i])->rawValue().toInt() != 0) {
             for (size_t j=0; j<cOffset; j++) {
                 if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
                     return true;

src/FactSystem/ParameterLoader.cc

@@ -112,7 +112,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
 #if 0
     // Handy for testing retry logic
     static int counter = 0;
-    if (counter++ & 0x3) {
+    if (counter++ & 0x8) {
         qCDebug(ParameterLoaderLog) << "Artificial discard" << counter;
         return;
     }
@@ -132,9 +132,6 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
     }
     _dataMutex.lock();
 
-    // Restart our waiting for param timer
-    _waitingParamTimeoutTimer.start();
-
     // Update our total parameter counts
     if (!_paramCountMap.contains(componentId)) {
         _paramCountMap[componentId] = parameterCount;
@@ -170,6 +167,14 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
         componentParamsComplete = true;
     }
 
+    if (_waitingReadParamIndexMap[componentId].contains(parameterId) ||
+        _waitingReadParamNameMap[componentId].contains(parameterName) ||
+        _waitingWriteParamNameMap[componentId].contains(parameterName)) {
+        // We were waiting for this parameter, restart wait timer. Otherwise it is a spurious parameter update which
+        // means we should not reset the wait timer.
+        _waitingParamTimeoutTimer.start();
+    }
+
     // Remove this parameter from the waiting lists
     _waitingReadParamIndexMap[componentId].remove(parameterId);
     _waitingReadParamNameMap[componentId].remove(parameterName);

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -32,6 +32,7 @@
 QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
 
 static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)");
+static const QRegExp APM_SOLO_REXP("^(APM:Copter solo-)");
 static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)");
 static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)");
 static const QRegExp APM_PX4NUTTX_REXP("^PX4: .*NuttX: .*");
@@ -43,6 +44,7 @@ static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|ArduCopter|A
 
 // minimum firmware versions that don't suffer from mavlink severity inversion bug.
 // https://github.com/diydrones/apm_planner/issues/788
+static const QString MIN_SOLO_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter solo-1.2.0");
 static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0");
 static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0");
 static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");
@@ -300,7 +302,7 @@ void APMFirmwarePlugin::_handleParamSet(Vehicle* vehicle, mavlink_message_t* mes
     mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
 }
 
-void APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* message)
+bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* message)
 {
     QString messageText;
 
@@ -311,7 +313,7 @@ void APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m
         messageText = _getMessageText(message);
         qCDebug(APMFirmwarePluginLog) << messageText;
 
-        if (!_firmwareVersion.isValid()) {
+        if (!_firmwareVersion.isValid() && !messageText.contains(APM_SOLO_REXP)) {
             // if don't know firmwareVersion yet, try and see if this message contains it
             if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP)) {
                 // found version string
@@ -355,7 +357,7 @@ void APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m
 
         if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
             _adjustCalibrationMessageSeverity(message);
-            return;
+            return true;
         }
     }
 
@@ -370,10 +372,21 @@ void APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m
 
     // The following messages are incorrectly labeled as warning message.
     // Fixed in newer firmware (unreleased at this point), but still in older firmware.
-    if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) ||
+    if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_SOLO_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) ||
             messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
         _setInfoSeverity(message);
     }
+
+    // ArduPilot PreArm messages can come across very frequently especially on Solo, which seems to send them once a second.
+    // Filter them out if they come too quickly.
+    if (messageText.startsWith("PreArm")) {
+        if (_noisyPrearmMap.contains(messageText) && _noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
+            return false;
+        }
+        _noisyPrearmMap[messageText] = QTime::currentTime();
+    }
+
+    return true;
 }
 
 void APMFirmwarePlugin::_handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message)
@@ -396,11 +409,11 @@ void APMFirmwarePlugin::_handleHeartbeat(Vehicle* vehicle, mavlink_message_t* me
     vehicle->setFlying(flying);
 }
 
-void APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
     //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
     if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
-        return;
+        return true;
     }
 
     switch (message->msgid) {
@@ -408,12 +421,13 @@ void APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m
         _handleParamValue(vehicle, message);
         break;
     case MAVLINK_MSG_ID_STATUSTEXT:
-        _handleStatusText(vehicle, message);
-        break;
+        return _handleStatusText(vehicle, message);
     case MAVLINK_MSG_ID_HEARTBEAT:
         _handleHeartbeat(vehicle, message);
         break;
     }
+
+    return true;
 }
 
 void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -91,7 +91,7 @@ public:
     bool        isGuidedMode(const Vehicle* vehicle) const final;
     void        pauseVehicle(Vehicle* vehicle);
     int         manualControlReservedButtonCount(void) final;
-    void        adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
+    bool        adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
     void        adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
     void        initializeVehicle(Vehicle* vehicle) final;
     bool        sendHomePositionToVehicle(void) final;
@@ -116,12 +116,13 @@ private:
     QString _getMessageText(mavlink_message_t* message) const;
     void _handleParamValue(Vehicle* vehicle, mavlink_message_t* message);
     void _handleParamSet(Vehicle* vehicle, mavlink_message_t* message);
-    void _handleStatusText(Vehicle* vehicle, mavlink_message_t* message);
+    bool _handleStatusText(Vehicle* vehicle, mavlink_message_t* message);
     void _handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
 
     APMFirmwareVersion      _firmwareVersion;
     bool                    _textSeverityAdjustmentNeeded;
     QList<APMCustomMode>    _supportedModes;
+    QMap<QString, QTime>    _noisyPrearmMap;
 };
 
 #endif

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -93,11 +93,12 @@ int FirmwarePlugin::manualControlReservedButtonCount(void)
     return -1;
 }
 
-void FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
     Q_UNUSED(vehicle);
     Q_UNUSED(message);    
     // Generic plugin does no message adjustment
+    return true;
 }
 
 void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)

src/FirmwarePlugin/FirmwarePlugin.h

@@ -120,7 +120,8 @@ public:
     /// spec implementations such that the base code can remain mavlink generic.
     ///     @param vehicle Vehicle message came from
     ///     @param message[in,out] Mavlink message to adjust if needed.
-    virtual void adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
+    /// @return false: skip message, true: process message
+    virtual bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
     
     /// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics.
     /// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain

src/Vehicle/Vehicle.cc

@@ -377,7 +377,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     }
 
     // Give the plugin a change to adjust the message contents
-    _firmwarePlugin->adjustIncomingMavlinkMessage(this, &message);
+    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
+        return;
+    }
 
     switch (message.msgid) {
     case MAVLINK_MSG_ID_HOME_POSITION: