Use AUTOPILOT_VERSION to get firmware version

alsaibie · May 24, 2016 · c571636 · c571636
1 parent b8d2596
commit c571636
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 2 deletions.
diff --git 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
@@ -316,6 +316,12 @@ class Vehicle : public FactGroup
     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 @@ class Vehicle : public FactGroup
     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 @@ public slots:
     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 slots:
     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 slots:
     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;