diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 37184ecb453f7b332e0aa5a7a74e32586da81b42..3471a773545bc3596b20d0e4adbf13f0ef93938a 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -28,6 +28,7 @@ #include "Generic/GenericFirmwarePlugin.h" #include "AutoPilotPlugins/APM/APMAutoPilotPlugin.h" // FIXME: Hack #include "QGCMAVLink.h" +#include "QGCApplication.h" QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog") @@ -211,7 +212,7 @@ int APMFirmwarePlugin::manualControlReservedButtonCount(void) return -1; } -void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) +void APMFirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) { if (message->msgid == MAVLINK_MSG_ID_PARAM_VALUE) { mavlink_param_value_t paramValue; @@ -296,9 +297,6 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) mavlink_statustext_t statusText; mavlink_msg_statustext_decode(message, &statusText); - // APM user facing calibration messages come through as high severity, we need to parse them out - // and lower the severity on them so that they don't pop in the users face. - if (!_firmwareVersion.isValid() || statusText.severity < MAV_SEVERITY_NOTICE) { QByteArray b; b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); @@ -314,9 +312,42 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) // found version string _firmwareVersion = APMFirmwareVersion(messageText); _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion); + + if (!_firmwareVersion.isBeta() && !_firmwareVersion.isDev()) { + int supportedMajorNumber = -1; + int supportedMinorNumber = -1; + + switch (vehicle->vehicleType()) { + case MAV_TYPE_FIXED_WING: + supportedMajorNumber = 3; + supportedMinorNumber = 4; + break; + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_COAXIAL: + case MAV_TYPE_HELICOPTER: + case MAV_TYPE_SUBMARINE: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_TRICOPTER: + supportedMajorNumber = 3; + supportedMinorNumber = 3; + break; + default: + break; + } + + if (supportedMajorNumber != -1) { + if (_firmwareVersion.majorNumber() < supportedMajorNumber || _firmwareVersion.minorNumber() < supportedMinorNumber) { + qgcApp()->showMessage(QString("QGroundControl fully supports Version %1.%2 and above. You are using a version prior to that. This combination is untested, you may run into unpredictable results.").arg(supportedMajorNumber).arg(supportedMinorNumber)); + } + } + } } } + // APM user facing calibration messages come through as high severity, we need to parse them out + // and lower the severity on them so that they don't pop in the users face. + if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) { _adjustCalibrationMessageSeverity(message); return; diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 2e0ce6be125e038c614ab0af403d1720b7f30e04..775949927d4842c938bc2a688869bfb34da42fbb 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -86,7 +86,7 @@ public: virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode); virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); - virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); virtual bool sendHomePositionToVehicle(void); virtual void addMetaDataToFact(Fact* fact, MAV_TYPE vehicleType); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index e6edfbc6853d656d1e4ab45012273a234508039c..7838dbdb991fc79102c65e27220062fdcf74f0ca 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -90,8 +90,9 @@ public: /// Called before any mavlink message is processed by Vehicle such taht the firmwre 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. + /// @param vehicle Vehicle message came from /// @param message[in,out] Mavlink message to adjust if needed. - virtual void adjustMavlinkMessage(mavlink_message_t* message) = 0; + virtual void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) = 0; /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. virtual void initializeVehicle(Vehicle* vehicle) = 0; diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc index 7a6b17c5c4d3fb3dfdfbaf68c2f331cde7ae6491..b35fddc31734f24a2442449786fecec559d14c7c 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc @@ -90,8 +90,9 @@ int GenericFirmwarePlugin::manualControlReservedButtonCount(void) return -1; } -void GenericFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) +void GenericFirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) { + Q_UNUSED(vehicle); Q_UNUSED(message); // Generic plugin does no message adjustment diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h index bd17140beb9699019024365e9d22a2c7e7f7cd7f..d1b828459c4af34e5fab60b14fafac1d9a1cb10f 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h @@ -42,7 +42,7 @@ public: virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode); virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); - virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); virtual bool sendHomePositionToVehicle(void); virtual void addMetaDataToFact(Fact* fact, MAV_TYPE vehicleType); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 45d330196ceaec782e1a0dd211087f776a7b486e..b9b55570a6be242b2900d760f0bc657a957e3a0e 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -177,8 +177,9 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void) return 0; // 0 buttons reserved for rc switch simulation } -void PX4FirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) +void PX4FirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) { + Q_UNUSED(vehicle); Q_UNUSED(message); // PX4 Flight Stack plugin does no message adjustment diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 07825896d290dade8356dd351a6938ccaea2d6e2..4af5a1c01ffb175aeeb416334607a1051142b8e4 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -42,7 +42,7 @@ public: virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode); virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); - virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); virtual bool sendHomePositionToVehicle(void); virtual void addMetaDataToFact(Fact* fact, MAV_TYPE vehicleType); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9aa7a11012789c939f1e1b5d96c1d497301f4e73..caedb6bf7bda7d7c52383c4261a7f4cb93573f42 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -221,7 +221,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } // Give the plugin a change to adjust the message contents - _firmwarePlugin->adjustMavlinkMessage(&message); + _firmwarePlugin->adjustMavlinkMessage(this, &message); switch (message.msgid) { case MAVLINK_MSG_ID_HOME_POSITION: @@ -442,7 +442,7 @@ void Vehicle::_sendMessage(mavlink_message_t message) MAVLinkProtocol* mavlink = _mavlink; // Give the plugin a chance to adjust - _firmwarePlugin->adjustMavlinkMessage(&message); + _firmwarePlugin->adjustMavlinkMessage(this, &message); static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);