Merge pull request #7998 from mavlink/pr-fix-camera-battery

We can only filter by component ID

src/Camera/QGCCameraManager.cc

@@ -68,7 +68,8 @@ QGCCameraManager::_vehicleReady(bool ready)
 void
 QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message)
 {
-    if(message.sysid == _vehicle->id()) {
+    //-- Only pay attention to camera components, as identified by their compId
+    if(message.sysid == _vehicle->id() && (message.compid >= MAV_COMP_ID_CAMERA && message.compid <= MAV_COMP_ID_CAMERA6)) {
         switch (message.msgid) {
             case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
                 _handleCaptureStatus(message);
@@ -110,44 +111,39 @@ QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message)
 {
     mavlink_heartbeat_t heartbeat;
     mavlink_msg_heartbeat_decode(&message, &heartbeat);
-    //-- Only pay attention to camera components, as identified by their MAV_TYPE, and as fallback by their compId
-    if(_vehicleReadyState && _vehicle->id() == message.sysid &&
-            ((heartbeat.autopilot == MAV_AUTOPILOT_INVALID && heartbeat.type == MAV_TYPE_CAMERA) ||
-             (message.compid >= MAV_COMP_ID_CAMERA && message.compid <= MAV_COMP_ID_CAMERA6))) {
-        //-- First time hearing from this one?
-        QString sCompID = QString::number(message.compid);
-        if(!_cameraInfoRequest.contains(sCompID)) {
-            qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid;
-            CameraStruct* pInfo = new CameraStruct(this, message.compid);
-            pInfo->lastHeartbeat.start();
-            _cameraInfoRequest[sCompID] = pInfo;
-            //-- Request camera info
-            _requestCameraInfo(message.compid);
-        } else {
-            if(_cameraInfoRequest[sCompID]) {
-                CameraStruct* pInfo = _cameraInfoRequest[sCompID];
-                //-- Check if we have indeed received the camera info
-                if(pInfo->infoReceived) {
-                    //-- We have it. Just update the heartbeat timeout
-                    pInfo->lastHeartbeat.start();
-                } else {
-                    //-- Try again. Maybe.
-                    if(pInfo->lastHeartbeat.elapsed() > 2000) {
-                        if(pInfo->tryCount > 3) {
-                            if(!pInfo->gaveUp) {
-                                pInfo->gaveUp = true;
-                                qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid;
-                            }
-                        } else {
-                            pInfo->tryCount++;
-                            //-- Request camera info again.
-                            _requestCameraInfo(message.compid);
+    //-- First time hearing from this one?
+    QString sCompID = QString::number(message.compid);
+    if(!_cameraInfoRequest.contains(sCompID)) {
+        qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid;
+        CameraStruct* pInfo = new CameraStruct(this, message.compid);
+        pInfo->lastHeartbeat.start();
+        _cameraInfoRequest[sCompID] = pInfo;
+        //-- Request camera info
+        _requestCameraInfo(message.compid);
+    } else {
+        if(_cameraInfoRequest[sCompID]) {
+            CameraStruct* pInfo = _cameraInfoRequest[sCompID];
+            //-- Check if we have indeed received the camera info
+            if(pInfo->infoReceived) {
+                //-- We have it. Just update the heartbeat timeout
+                pInfo->lastHeartbeat.start();
+            } else {
+                //-- Try again. Maybe.
+                if(pInfo->lastHeartbeat.elapsed() > 2000) {
+                    if(pInfo->tryCount > 3) {
+                        if(!pInfo->gaveUp) {
+                            pInfo->gaveUp = true;
+                            qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid;
                         }
+                    } else {
+                        pInfo->tryCount++;
+                        //-- Request camera info again.
+                        _requestCameraInfo(message.compid);
                     }
                 }
-            } else {
-                qWarning() << "_cameraInfoRequest[" << sCompID << "] is null";
             }
+        } else {
+            qWarning() << "_cameraInfoRequest[" << sCompID << "] is null";
         }
     }
 }