diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 858a140efad30c3aaeac245932a362beddc9e220..53b595e5fc5a838f65b9cba137640a2cdaef42a4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -126,6 +126,7 @@ Vehicle::Vehicle(LinkInterface* link, , _firmwareMajorVersion(versionNotSetValue) , _firmwareMinorVersion(versionNotSetValue) , _firmwarePatchVersion(versionNotSetValue) + , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -208,6 +209,9 @@ Vehicle::Vehicle(LinkInterface* link, connect(_parameterLoader, &ParameterLoader::parametersReady, _autopilotPlugin, &AutoPilotPlugin::_parametersReadyPreChecks); connect(_parameterLoader, &ParameterLoader::parameterListProgress, _autopilotPlugin, &AutoPilotPlugin::parameterListProgress); + // Ask the vehicle for firmware version info + doCommandLong(MAV_COMP_ID_ALL, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, 1 /* request firmware version */); + _firmwarePlugin->initializeVehicle(this); _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay); @@ -437,6 +441,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_COMMAND_ACK: _handleCommandAck(message); break; + case MAVLINK_MSG_ID_AUTOPILOT_VERSION: + _handleAutopilotVersion(message); + break; // Following are ArduPilot dialect messages @@ -450,6 +457,23 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } +void Vehicle::_handleAutopilotVersion(mavlink_message_t& message) +{ + mavlink_autopilot_version_t autopilotVersion; + mavlink_msg_autopilot_version_decode(&message, &autopilotVersion); + + if (autopilotVersion.flight_sw_version != 0) { + int majorVersion, minorVersion, patchVersion; + FIRMWARE_VERSION_TYPE versionType; + + majorVersion = (autopilotVersion.flight_sw_version >> (8*3)) & 0xFF; + minorVersion = (autopilotVersion.flight_sw_version >> (8*2)) & 0xFF; + patchVersion = (autopilotVersion.flight_sw_version >> (8*1)) & 0xFF; + versionType = (FIRMWARE_VERSION_TYPE)((autopilotVersion.flight_sw_version >> (8*0)) & 0xFF); + setFirmwareVersion(majorVersion, minorVersion, patchVersion); + } +} + void Vehicle::_handleCommandAck(mavlink_message_t& message) { mavlink_command_ack_t ack; @@ -1638,13 +1662,36 @@ void Vehicle::_prearmErrorTimeout(void) setPrearmError(QString()); } -void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion) +void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType) { _firmwareMajorVersion = majorVersion; _firmwareMinorVersion = minorVersion; _firmwarePatchVersion = patchVersion; + _firmwareVersionType = versionType; + emit firmwareMajorVersionChanged(_firmwareMajorVersion); + emit firmwareMinorVersionChanged(_firmwareMinorVersion); + emit firmwarePatchVersionChanged(_firmwarePatchVersion); + emit firmwareVersionTypeChanged(_firmwareVersionType); +} + +QString Vehicle::firmwareVersionTypeString(void) const +{ + switch (_firmwareVersionType) { + case FIRMWARE_VERSION_TYPE_DEV: + return QStringLiteral("dev"); + case FIRMWARE_VERSION_TYPE_ALPHA: + return QStringLiteral("alpha"); + case FIRMWARE_VERSION_TYPE_BETA: + return QStringLiteral("beta"); + case FIRMWARE_VERSION_TYPE_RC: + return QStringLiteral("rc"); + case FIRMWARE_VERSION_TYPE_OFFICIAL: + default: + return QStringLiteral(""); + } } + void Vehicle::rebootVehicle() { doCommandLong(id(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index d35c69e63ca272c75ab93449b002999977660061..43c753498669f181ac193b14bf3cd8e0c16e569e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -316,6 +316,12 @@ public: Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) + Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareMajorVersionChanged) + Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareMinorVersionChanged) + Q_PROPERTY(int firmwarePatchVersion READ firmwarePatchVersion NOTIFY firmwarePatchVersionChanged) + Q_PROPERTY(int firmwareVersionType READ firmwareVersionType NOTIFY firmwareVersionTypeChanged) + Q_PROPERTY(QString firmwareVersionTypeString READ firmwareVersionTypeString NOTIFY firmwareVersionTypeChanged) + /// Resets link status counters Q_INVOKABLE void resetCounters (); @@ -530,7 +536,9 @@ public: int firmwareMajorVersion(void) const { return _firmwareMajorVersion; } int firmwareMinorVersion(void) const { return _firmwareMinorVersion; } int firmwarePatchVersion(void) const { return _firmwarePatchVersion; } - void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion); + int firmwareVersionType(void) const { return _firmwareVersionType; } + QString firmwareVersionTypeString(void) const; + void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType = FIRMWARE_VERSION_TYPE_OFFICIAL); static const int versionNotSetValue = -1; public slots: @@ -579,6 +587,11 @@ signals: void flowImageIndexChanged (); void rcRSSIChanged (int rcRSSI); + void firmwareMajorVersionChanged(int major); + void firmwareMinorVersionChanged(int minor); + void firmwarePatchVersionChanged(int patch); + void firmwareVersionTypeChanged(int type); + /// New RC channel values /// @param channelCount Number of available channels, cMaxRcChannels max /// @param pwmValues -1 signals channel not available @@ -637,6 +650,7 @@ private: void _handleVibration(mavlink_message_t& message); void _handleExtendedSysState(mavlink_message_t& message); void _handleCommandAck(mavlink_message_t& message); + void _handleAutopilotVersion(mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); @@ -747,6 +761,7 @@ private: int _firmwareMajorVersion; int _firmwareMinorVersion; int _firmwarePatchVersion; + FIRMWARE_VERSION_TYPE _firmwareVersionType; static const int _lowBatteryAnnounceRepeatMSecs; // Amount of time in between each low battery announcement QElapsedTimer _lowBatteryAnnounceTimer;