diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index da292d71fd49b0588c41c0a90007dffae56487eb..c3e0761cecbe21706aadb1f36fd607dbc0dbb6a2 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -140,7 +140,11 @@ Vehicle::Vehicle(LinkInterface* link, , _firmwareMajorVersion(versionNotSetValue) , _firmwareMinorVersion(versionNotSetValue) , _firmwarePatchVersion(versionNotSetValue) + , _firmwareCustomMajorVersion(versionNotSetValue) + , _firmwareCustomMinorVersion(versionNotSetValue) + , _firmwareCustomPatchVersion(versionNotSetValue) , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL) + , _gitHash(versionNotSetValue) , _lastAnnouncedLowBatteryPercent(100) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) @@ -294,6 +298,10 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _firmwareMajorVersion(versionNotSetValue) , _firmwareMinorVersion(versionNotSetValue) , _firmwarePatchVersion(versionNotSetValue) + , _firmwareCustomMajorVersion(versionNotSetValue) + , _firmwareCustomMinorVersion(versionNotSetValue) + , _firmwareCustomPatchVersion(versionNotSetValue) + , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL) , _gitHash(versionNotSetValue) , _lastAnnouncedLowBatteryPercent(100) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) @@ -714,14 +722,23 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me } // Git hash - if (autopilotVersion.flight_custom_version[0] != 0) { + if (*((uint64_t*)(&autopilotVersion.flight_custom_version)) != 0) { // PX4 Firmware stores the first 16 characters of the git hash as binary, with the individual bytes in reverse order if (px4Firmware()) { + // Lower 3 bytes is custom version + int majorVersion, minorVersion, patchVersion; + majorVersion = autopilotVersion.flight_custom_version[2]; + minorVersion = autopilotVersion.flight_custom_version[1]; + patchVersion = autopilotVersion.flight_custom_version[0]; + setFirmwareCustomVersion(majorVersion, minorVersion, patchVersion); + + qDebug() << majorVersion << minorVersion << patchVersion; + _gitHash = ""; QByteArray array((char*)autopilotVersion.flight_custom_version, 8); for (int i = 7; i >= 0; i--) { _gitHash.append(QString("%1").arg(autopilotVersion.flight_custom_version[i], 2, 16, QChar('0'))); - } + } } else { // APM Firmware stores the first 8 characters of the git hash as an ASCII character string _gitHash = QString::fromUtf8((char*)autopilotVersion.flight_custom_version, 8); @@ -2290,10 +2307,15 @@ void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVe _firmwareMinorVersion = minorVersion; _firmwarePatchVersion = patchVersion; _firmwareVersionType = versionType; - emit firmwareMajorVersionChanged(_firmwareMajorVersion); - emit firmwareMinorVersionChanged(_firmwareMinorVersion); - emit firmwarePatchVersionChanged(_firmwarePatchVersion); - emit firmwareVersionTypeChanged(_firmwareVersionType); + emit firmwareVersionChanged(); +} + +void Vehicle::setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion) +{ + _firmwareCustomMajorVersion = majorVersion; + _firmwareCustomMinorVersion = minorVersion; + _firmwareCustomPatchVersion = patchVersion; + emit firmwareCustomVersionChanged(); } QString Vehicle::firmwareVersionTypeString(void) const diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index a1c1ca943f4e097e37476a80f602be89ec04ba9b..477af21d8328ffbf5077b043149468e2c810f322 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -351,12 +351,15 @@ public: Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup 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) - Q_PROPERTY(QString gitHash READ gitHash NOTIFY gitHashChanged) + Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged) + Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) + Q_PROPERTY(int firmwarePatchVersion READ firmwarePatchVersion NOTIFY firmwareVersionChanged) + Q_PROPERTY(int firmwareVersionType READ firmwareVersionType NOTIFY firmwareVersionChanged) + Q_PROPERTY(QString firmwareVersionTypeString READ firmwareVersionTypeString NOTIFY firmwareVersionChanged) + Q_PROPERTY(int firmwareCustomMajorVersion READ firmwareCustomMajorVersion NOTIFY firmwareCustomVersionChanged) + Q_PROPERTY(int firmwareCustomMinorVersion READ firmwareCustomMinorVersion NOTIFY firmwareCustomVersionChanged) + Q_PROPERTY(int firmwareCustomPatchVersion READ firmwareCustomPatchVersion NOTIFY firmwareCustomVersionChanged) + Q_PROPERTY(QString gitHash READ gitHash NOTIFY gitHashChanged) /// Resets link status counters Q_INVOKABLE void resetCounters (); @@ -631,8 +634,12 @@ public: int firmwareMinorVersion(void) const { return _firmwareMinorVersion; } int firmwarePatchVersion(void) const { return _firmwarePatchVersion; } int firmwareVersionType(void) const { return _firmwareVersionType; } + int firmwareCustomMajorVersion(void) const { return _firmwareCustomMajorVersion; } + int firmwareCustomMinorVersion(void) const { return _firmwareCustomMinorVersion; } + int firmwareCustomPatchVersion(void) const { return _firmwareCustomPatchVersion; } QString firmwareVersionTypeString(void) const; void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType = FIRMWARE_VERSION_TYPE_OFFICIAL); + void setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion); static const int versionNotSetValue = -1; QString gitHash(void) const { return _gitHash; } @@ -734,11 +741,8 @@ signals: void telemetryRNoiseChanged (int value); void autoDisarmChanged (void); - void firmwareMajorVersionChanged(int major); - void firmwareMinorVersionChanged(int minor); - void firmwarePatchVersionChanged(int patch); - void firmwareVersionTypeChanged(int type); - + void firmwareVersionChanged(void); + void firmwareCustomVersionChanged(void); void gitHashChanged(QString hash); /// New RC channel values @@ -986,6 +990,9 @@ private: int _firmwareMajorVersion; int _firmwareMinorVersion; int _firmwarePatchVersion; + int _firmwareCustomMajorVersion; + int _firmwareCustomMinorVersion; + int _firmwareCustomPatchVersion; FIRMWARE_VERSION_TYPE _firmwareVersionType; QString _gitHash;