From 6e9d785cebfa18b78c86b385edb8287ef3f879cd Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 24 Mar 2016 17:56:45 -0700 Subject: [PATCH] Drop high rate PreArm messages Also filter solo version string --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 30 +++++++++++++++------ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 5 ++-- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index a979ac789..185d8f268 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/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, ¶mSet); } -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) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 2e5eeeb71..bacd4dafa 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/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 _supportedModes; + QMap _noisyPrearmMap; }; #endif -- 2.22.0