Merge pull request #6898 from DonLakeFlyer/PreArmMessaging

Move ArduPilot preflight check handling to generic QGC

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -102,7 +102,7 @@ void APMSensorsComponentController::_startLogCalibration(void)
 {
     _hideAllCalAreas();
     
-    connect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
+    connect(_vehicle, &Vehicle::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
     
     emit setAllCalButtonsEnabled(false);
     if (_calTypeInProgress == CalTypeAccel || _calTypeInProgress == CalTypeCompassMot) {
@@ -152,7 +152,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
 {
     _vehicle->setConnectionLostEnabled(true);
 
-    disconnect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
+    disconnect(_vehicle, &Vehicle::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
     
     emit setAllCalButtonsEnabled(true);
     _nextButton->setEnabled(false);

src/AutoPilotPlugins/PX4/PowerComponentController.cc

@@ -26,26 +26,26 @@ PowerComponentController::PowerComponentController(void)
 void PowerComponentController::calibrateEsc(void)
 {
     _warningMessages.clear();
-    connect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
+    connect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
     _uas->startCalibration(UASInterface::StartCalibrationEsc);
 }
 
 void PowerComponentController::busConfigureActuators(void)
 {
     _warningMessages.clear();
-    connect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
+    connect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
     _uas->startBusConfig(UASInterface::StartBusConfigActuators);
 }
 
 void PowerComponentController::stopBusConfigureActuators(void)
 {
-    disconnect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
+    disconnect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
     _uas->startBusConfig(UASInterface::EndBusConfigActuators);
 }
 
 void PowerComponentController::_stopCalibration(void)
 {
-    disconnect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
+    disconnect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
 }
 
 void PowerComponentController::_stopBusConfig(void)

src/AutoPilotPlugins/PX4/SensorsComponentController.cc

@@ -89,7 +89,7 @@ void SensorsComponentController::_startLogCalibration(void)
     _unknownFirmwareVersion = false;
     _hideAllCalAreas();
     
-    connect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
+    connect(_vehicle, &Vehicle::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
     
     _cancelButton->setEnabled(false);
 }
@@ -137,7 +137,7 @@ void SensorsComponentController::_resetInternalState(void)
 
 void SensorsComponentController::_stopCalibration(SensorsComponentController::StopCalibrationCode code)
 {
-    disconnect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
+    disconnect(_vehicle, &Vehicle::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
     
     _compassButton->setEnabled(true);
     _gyroButton->setEnabled(true);

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -434,17 +434,6 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
         }
     }
 
-    if (messageText.startsWith("PreArm")) {
-        // 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 (instanceData->noisyPrearmMap.contains(messageText) && instanceData->noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
-            return false;
-        }
-        instanceData->noisyPrearmMap[messageText] = QTime::currentTime();
-
-        vehicle->setPrearmError(messageText);
-    }
-
     return true;
 }
 

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -145,7 +145,6 @@ public:
     APMFirmwarePluginInstanceData(QObject* parent = NULL);
 
     bool                    textSeverityAdjustmentNeeded;
-    QMap<QString, QTime>    noisyPrearmMap;
 };
 
 #endif

src/FlightDisplay/FlightDisplayViewWidgets.qml

@@ -104,37 +104,51 @@ Item {
     }
 
     //-- Map warnings
+
+    Rectangle {
+        anchors.margins:    -ScreenTools.defaultFontPixelHeight
+        anchors.fill:       warningsCol
+        color:              "white"
+        opacity:            0.5
+        radius:             ScreenTools.defaultFontPixelWidth / 2
+        visible:            warningsCol.noGPSLockVisible || warningsCol.prearmErrorVisible
+    }
+
     Column {
+        id:                         warningsCol
         anchors.horizontalCenter:   parent.horizontalCenter
         anchors.top:                parent.verticalCenter
         spacing:                    ScreenTools.defaultFontPixelHeight
 
+        property bool noGPSLockVisible:     _activeVehicle && !_activeVehicle.coordinate.isValid && _mainIsMap
+        property bool prearmErrorVisible:   _activeVehicle && _activeVehicle.prearmError
+
         QGCLabel {
             anchors.horizontalCenter:   parent.horizontalCenter
-            visible:                    _activeVehicle && !_activeVehicle.coordinate.isValid && _mainIsMap
+            visible:                    warningsCol.noGPSLockVisible
             z:                          QGroundControl.zOrderTopMost
-            color:                      mapPal.text
+            color:                      "black"
             font.pointSize:             ScreenTools.largeFontPointSize
             text:                       qsTr("No GPS Lock for Vehicle")
         }
 
         QGCLabel {
             anchors.horizontalCenter:   parent.horizontalCenter
-            visible:                    _activeVehicle && _activeVehicle.prearmError
+            visible:                    warningsCol.prearmErrorVisible
             z:                          QGroundControl.zOrderTopMost
-            color:                      mapPal.text
+            color:                      "black"
             font.pointSize:             ScreenTools.largeFontPointSize
             text:                       _activeVehicle ? _activeVehicle.prearmError : ""
         }
 
         QGCLabel {
             anchors.horizontalCenter:   parent.horizontalCenter
-            visible:                    _activeVehicle && _activeVehicle.prearmError
+            visible:                    warningsCol.prearmErrorVisible
             width:                      ScreenTools.defaultFontPixelWidth * 50
             horizontalAlignment:        Text.AlignHCenter
             wrapMode:                   Text.WordWrap
             z:                          QGroundControl.zOrderTopMost
-            color:                      mapPal.text
+            color:                      "black"
             font.pointSize:             ScreenTools.largeFontPointSize
             text:                       "The vehicle has failed a pre-arm check. In order to arm the vehicle, resolve the failure."
         }

src/QGCApplication.cc

@@ -674,8 +674,8 @@ QObject* QGCApplication::_rootQmlObject()
 
 void QGCApplication::showMessage(const QString& message)
 {
-    // Special case hack for ArduPilot prearm messages. These show up in the center of the map, so no need for popup.
-    if (message.contains("PreArm:")) {
+    // PreArm messages are handled by Vehicle and shown in Map
+    if (message.startsWith(QStringLiteral("PreArm")) || message.startsWith(QStringLiteral("preflight"), Qt::CaseInsensitive)) {
         return;
     }
 

src/Vehicle/Vehicle.cc

@@ -756,6 +756,10 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_ESTIMATOR_STATUS:
         _handleEstimatorStatus(message);
         break;
+    case MAVLINK_MSG_ID_STATUSTEXT:
+        _handleStatusText(message);
+        break;
+
     case MAVLINK_MSG_ID_PING:
         _handlePing(link, message);
         break;
@@ -813,6 +817,41 @@ void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message)
     }
 }
 
+void Vehicle::_handleStatusText(mavlink_message_t& message)
+{
+    QByteArray b;
+
+    b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
+    mavlink_msg_statustext_get_text(&message, b.data());
+    b[b.length()-1] = '\0';
+    QString messageText = QString(b);
+    int severity = mavlink_msg_statustext_get_severity(&message);
+
+    bool skipSpoken = false;
+    bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm"));
+    bool px4Prearm = messageText.startsWith(QStringLiteral("preflight"), Qt::CaseInsensitive) && severity >= MAV_SEVERITY_CRITICAL;
+    if (ardupilotPrearm || px4Prearm) {
+        // Limit repeated PreArm message to once every 10 seconds
+        if (_noisySpokenPrearmMap.contains(messageText) && _noisySpokenPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
+            skipSpoken = true;
+        } else {
+            _noisySpokenPrearmMap[messageText] = QTime::currentTime();
+            setPrearmError(messageText);
+        }
+    }
+
+
+    // If the message is NOTIFY or higher severity, or starts with a '#',
+    // then read it aloud.
+    if (messageText.startsWith("#") || severity <= MAV_SEVERITY_NOTICE) {
+        messageText.remove("#");
+        if (!skipSpoken) {
+            qgcApp()->toolbox()->audioOutput()->say(messageText);
+        }
+    }
+    emit textMessageReceived(id(), message.compid, severity, messageText);
+}
+
 void Vehicle::_handleVfrHud(mavlink_message_t& message)
 {
     mavlink_vfr_hud_t vfrHud;

src/Vehicle/Vehicle.h

@@ -1093,6 +1093,7 @@ signals:
     void priorityLinkNameChanged(const QString& priorityLinkName);
     void linksChanged(void);
     void linksPropertiesChanged(void);
+    void textMessageReceived(int uasid, int componentid, int severity, QString text);
 
     void messagesReceivedChanged    ();
     void messagesSentChanged        ();
@@ -1233,6 +1234,7 @@ private:
     void _handleAttitudeTarget(mavlink_message_t& message);
     void _handleDistanceSensor(mavlink_message_t& message);
     void _handleEstimatorStatus(mavlink_message_t& message);
+    void _handleStatusText(mavlink_message_t& message);
     // ArduPilot dialect messages
 #if !defined(NO_ARDUPILOT_DIALECT)
     void _handleCameraFeedback(const mavlink_message_t& message);
@@ -1433,6 +1435,8 @@ private:
     uint64_t    _mavlinkLossCount       = 0;
     float       _mavlinkLossPercent     = 0.0f;
 
+    QMap<QString, QTime> _noisySpokenPrearmMap; ///< Used to prevent PreArm messages from being spoken too often
+
     // FactGroup facts
 
     Fact _rollFact;

src/uas/UAS.cc

@@ -308,32 +308,6 @@ void UAS::receiveMessage(mavlink_message_t message)
         }
             break;
 
-        case MAVLINK_MSG_ID_STATUSTEXT:
-        {
-            QByteArray b;
-            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
-            mavlink_msg_statustext_get_text(&message, b.data());
-
-            // Ensure NUL-termination
-            b[b.length()-1] = '\0';
-            QString text = QString(b);
-            int severity = mavlink_msg_statustext_get_severity(&message);
-
-        // If the message is NOTIFY or higher severity, or starts with a '#',
-        // then read it aloud.
-            if (text.startsWith("#") || severity <= MAV_SEVERITY_NOTICE)
-            {
-                text.remove("#");
-                emit textMessageReceived(uasId, message.compid, severity, text);
-                _say(text.toLower(), severity);
-            }
-            else
-            {
-                emit textMessageReceived(uasId, message.compid, severity, text);
-            }
-        }
-            break;
-
         case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
         {
             mavlink_data_transmission_handshake_t p;
@@ -1613,12 +1587,6 @@ void UAS::unsetRCToParameterMap()
     }
 }
 
-void UAS::_say(const QString& text, int severity)
-{
-    Q_UNUSED(severity);
-    qgcApp()->toolbox()->audioOutput()->say(text);
-}
-
 void UAS::shutdownVehicle(void)
 {
 #ifndef __mobile__

src/uas/UAS.h

@@ -323,9 +323,6 @@ protected:
     quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
 
 private:
-    void _say(const QString& text, int severity = 6);
-
-private:
     Vehicle*                _vehicle;
     FirmwarePluginManager*  _firmwarePluginManager;
 };

src/uas/UASInterface.h

@@ -111,9 +111,6 @@ public slots:
     virtual void unsetRCToParameterMap() = 0;
 
 signals:
-    /** @brief A text message from the system has been received */
-    void textMessageReceived(int uasid, int componentid, int severity, QString text);
-
     /**
      * @brief Update the error count of a device
      *

src/uas/UASMessageHandler.cc

@@ -17,7 +17,7 @@
 #include "QGCApplication.h"
 #include "UASMessageHandler.h"
 #include "MultiVehicleManager.h"
-#include "UAS.h"
+#include "Vehicle.h"
 
 UASMessage::UASMessage(int componentid, int severity, QString text)
 {
@@ -88,7 +88,7 @@ void UASMessageHandler::_activeVehicleChanged(Vehicle* vehicle)
 {
     // If we were already attached to an autopilot, disconnect it.
     if (_activeVehicle) {
-        disconnect(_activeVehicle->uas(), &UASInterface::textMessageReceived, this, &UASMessageHandler::handleTextMessage);
+        disconnect(_activeVehicle, &Vehicle::textMessageReceived, this, &UASMessageHandler::handleTextMessage);
         _activeVehicle = NULL;
         clearMessages();
         emit textMessageReceived(NULL);
@@ -99,7 +99,7 @@ void UASMessageHandler::_activeVehicleChanged(Vehicle* vehicle)
         // Connect to the new UAS.
         clearMessages();
         _activeVehicle = vehicle;
-        connect(_activeVehicle->uas(), &UASInterface::textMessageReceived, this, &UASMessageHandler::handleTextMessage);
+        connect(_activeVehicle, &Vehicle::textMessageReceived, this, &UASMessageHandler::handleTextMessage);
     }
 }