diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 0c781d991a1747325b9b3c62b9e87a0ac063e61a..0fa69e62a11c2213db1615d6c86897ab8b345120 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -69,6 +69,12 @@ static const struct Modes2Name rgModes2Name[] = { { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::takeoffFlightMode, false, true, true }, }; +PX4FirmwarePlugin::PX4FirmwarePlugin(void) + : _versionNotified(false) +{ + +} + QList PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) { Q_UNUSED(vehicle); @@ -365,3 +371,51 @@ bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const return (vehicle->flightMode() == holdFlightMode || vehicle->flightMode() == takeoffFlightMode || vehicle->flightMode() == landingFlightMode); } + +bool PX4FirmwarePlugin::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 true; + } + + switch (message->msgid) { + case MAVLINK_MSG_ID_AUTOPILOT_VERSION: + _handleAutopilotVersion(vehicle, message); + break; + } + + return true; +} + +void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message) +{ + Q_UNUSED(vehicle); + + if (!_versionNotified) { + bool notifyUser = false; + int supportedMajorVersion = 1; + int supportedMinorVersion = 4; + int supportedPatchVersion = 1; + + mavlink_autopilot_version_t version; + mavlink_msg_autopilot_version_decode(message, &version); + + if (version.flight_sw_version != 0) { + int majorVersion, minorVersion, patchVersion; + + majorVersion = (version.flight_sw_version >> (8*3)) & 0xFF; + minorVersion = (version.flight_sw_version >> (8*2)) & 0xFF; + patchVersion = (version.flight_sw_version >> (8*1)) & 0xFF; + + notifyUser = majorVersion < supportedMajorVersion || minorVersion < supportedMinorVersion || patchVersion < supportedPatchVersion; + } else { + notifyUser = true; + } + + if (notifyUser) { + _versionNotified = true; + qgcApp()->showMessage(QString("QGroundControl supports PX4 Pro firmware Version %1.%2.%3 and above. You are using a version prior to that which will lead to unpredictable results. Please upgrade your firmware.").arg(supportedMajorVersion).arg(supportedMinorVersion).arg(supportedPatchVersion)); + } + } +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index a5c2229b0cf7fe924e715b851037e31c35164b1d..6a712f96a82fa9434493e27edee5ee07c391cf7c 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -23,6 +23,8 @@ class PX4FirmwarePlugin : public FirmwarePlugin Q_OBJECT public: + PX4FirmwarePlugin(void); + // Overrides from FirmwarePlugin QList componentsForVehicle(AutoPilotPlugin* vehicle) final; @@ -50,6 +52,7 @@ public: QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); } void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } QObject* loadParameterMetaData (const QString& metaDataFile); + bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the // names may change. @@ -69,6 +72,11 @@ public: static const char* landingFlightMode; static const char* rtgsFlightMode; static const char* followMeFlightMode; + +private: + void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message); + + bool _versionNotified; ///< true: user notified over version issue }; #endif diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index f69dc9030bf23334cd885a46a5d4da182228fb6d..733f77d1e8579edafed199b5210fd75d9351f7b1 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -794,24 +794,34 @@ void MockLink::_respondWithAutopilotVersion(void) { mavlink_message_t msg; - uint8_t customVersion[8]; - memset(customVersion, 0, sizeof(customVersion)); + uint8_t customVersion[8] = { }; + uint32_t flightVersion = 0; + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + flightVersion |= 3 << (8*3); + flightVersion |= 3 << (8*2); + flightVersion |= 0 << (8*1); + flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0); + } else if (_firmwareType == MAV_AUTOPILOT_PX4) { + flightVersion |= 1 << (8*3); + flightVersion |= 4 << (8*2); + flightVersion |= 1 << (8*1); + flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0); + } - // Only flight_sw_version is supported a this point mavlink_msg_autopilot_version_pack(_vehicleSystemId, _vehicleComponentId, &msg, - 0, // capabilities, - (1 << (8*3)) | FIRMWARE_VERSION_TYPE_DEV, // flight_sw_version, - 0, // middleware_sw_version, - 0, // os_sw_version, - 0, // board_version, - (uint8_t *)&customVersion, // flight_custom_version, - (uint8_t *)&customVersion, // middleware_custom_version, - (uint8_t *)&customVersion, // os_custom_version, - 0, // vendor_id, - 0, // product_id, - 0); // uid + 0, // capabilities, + flightVersion, // flight_sw_version, + 0, // middleware_sw_version, + 0, // os_sw_version, + 0, // board_version, + (uint8_t *)&customVersion, // flight_custom_version, + (uint8_t *)&customVersion, // middleware_custom_version, + (uint8_t *)&customVersion, // os_custom_version, + 0, // vendor_id, + 0, // product_id, + 0); // uid respondWithMavlinkMessage(msg); }