From 2fd39fe177cabdfd620beba5b33b45c8a883c902 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Sep 2018 13:52:37 -0700 Subject: [PATCH] Move ArduPilot preflight check handling to generic QGC --- .../APM/APMSensorsComponentController.cc | 4 +- .../PX4/PowerComponentController.cc | 8 ++-- .../PX4/SensorsComponentController.cc | 4 +- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 11 ------ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 1 - src/QGCApplication.cc | 4 +- src/Vehicle/Vehicle.cc | 37 +++++++++++++++++++ src/Vehicle/Vehicle.h | 4 ++ src/uas/UAS.cc | 32 ---------------- src/uas/UAS.h | 3 -- src/uas/UASInterface.h | 3 -- src/uas/UASMessageHandler.cc | 6 +-- 12 files changed, 54 insertions(+), 63 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 92a021f0d..adc0019f3 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/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); diff --git a/src/AutoPilotPlugins/PX4/PowerComponentController.cc b/src/AutoPilotPlugins/PX4/PowerComponentController.cc index f26def0bb..9ab268d13 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponentController.cc +++ b/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) diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 89085b8c4..b13372e8d 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/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); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 416a61851..492cf96fa 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/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; } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 54ae72454..30a9fd8d8 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -145,7 +145,6 @@ public: APMFirmwarePluginInstanceData(QObject* parent = NULL); bool textSeverityAdjustmentNeeded; - QMap noisyPrearmMap; }; #endif diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 342132430..40b974761 100644 --- a/src/QGCApplication.cc +++ b/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"))) { return; } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a6d01db90..c64faf627 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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,39 @@ 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); + + bool skipSpoken = false; + if (messageText.startsWith(QStringLiteral("PreArm")) || messageText.startsWith(QStringLiteral("PREFLIGHT"))) { + // 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. + int severity = mavlink_msg_statustext_get_severity(&message); + 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; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ff745c59a..38bb64fb7 100644 --- a/src/Vehicle/Vehicle.h +++ b/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 _noisySpokenPrearmMap; ///< Used to prevent PreArm messages from being spoken too often + // FactGroup facts Fact _rollFact; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index cec9a70b7..861b59a0b 100644 --- a/src/uas/UAS.cc +++ b/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__ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 92cb0cf16..5cba2f0b8 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -322,9 +322,6 @@ protected: quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent -private: - void _say(const QString& text, int severity = 6); - private: Vehicle* _vehicle; FirmwarePluginManager* _firmwarePluginManager; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index ffee6b2f6..87fd501e9 100644 --- a/src/uas/UASInterface.h +++ b/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 * diff --git a/src/uas/UASMessageHandler.cc b/src/uas/UASMessageHandler.cc index 34f3c41b6..775d74323 100644 --- a/src/uas/UASMessageHandler.cc +++ b/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); } } -- 2.22.0