From 72f342d7591270d3a245a207700b6129cb46eafc Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 14 Jun 2020 09:26:27 -0700 Subject: [PATCH] FirmwarePlugin::adjustOutgoingMavlinkMessageThreadSafe not required to be thread safe --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 10 +++++----- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 6 ++++-- src/FirmwarePlugin/FirmwarePlugin.cc | 5 +---- src/FirmwarePlugin/FirmwarePlugin.h | 5 ++++- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 746bbe073..4e66b4418 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -289,10 +289,8 @@ void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_mess ¶mValue); } -void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) +void APMFirmwarePlugin::_handleOutgoingParamSetThreadSafe(Vehicle* /*vehicle*/, LinkInterface* outgoingLink, mavlink_message_t* message) { - Q_UNUSED(vehicle); - mavlink_param_set_t paramSet; mavlink_param_union_t paramUnion; @@ -336,7 +334,9 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type; } + _adjustOutgoingMavlinkMutex.lock(); mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, ¶mSet); + _adjustOutgoingMavlinkMutex.unlock(); } bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message) @@ -508,11 +508,11 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m return true; } -void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) +void APMFirmwarePlugin::adjustOutgoingMavlinkMessageThreadSafe(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) { switch (message->msgid) { case MAVLINK_MSG_ID_PARAM_SET: - _handleOutgoingParamSet(vehicle, outgoingLink, message); + _handleOutgoingParamSetThreadSafe(vehicle, outgoingLink, message); break; } } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 6266067f0..802a1b28f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -93,7 +93,7 @@ public: void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) override; bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; - void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override; + void adjustOutgoingMavlinkMessageThreadSafe(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override; virtual void initializeStreamRates (Vehicle* vehicle); void initializeVehicle (Vehicle* vehicle) override; bool sendHomePositionToVehicle (void) override; @@ -127,7 +127,7 @@ private: void _handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message); bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message); - void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); + void _handleOutgoingParamSetThreadSafe(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); void _soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware); bool _guidedModeTakeoff(Vehicle* vehicle, double altitudeRel); void _handleRCChannels(Vehicle* vehicle, mavlink_message_t* message); @@ -141,6 +141,8 @@ private: QList _supportedModes; QMap> _ardupilotComponentMap; + QMutex _adjustOutgoingMavlinkMutex; + static const char* _artooIP; static const int _artooVideoHandshakePort; }; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 99b47d608..531fd1abe 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -156,11 +156,8 @@ bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_mess return true; } -void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) +void FirmwarePlugin::adjustOutgoingMavlinkMessageThreadSafe(Vehicle* /*vehicle*/, LinkInterface* /*outgoingLink*/, mavlink_message_t* /*message*/) { - Q_UNUSED(vehicle); - Q_UNUSED(outgoingLink); - Q_UNUSED(message); // Generic plugin does no message adjustment } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 2484b9e37..aa6cade47 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -193,10 +193,13 @@ public: /// 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 /// mavlink generic. + /// + /// This method must be thread safe. + /// /// @param vehicle Vehicle message came from /// @param outgoingLink Link that messae is going out on /// @param message[in,out] Mavlink message to adjust if needed. - virtual void adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); + virtual void adjustOutgoingMavlinkMessageThreadSafe(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); /// Determines how to handle the first item of the mission item list. Internally to QGC the first item /// is always the home position. -- 2.22.0