Unverified Commit ef07aecf authored by Gus Grubba's avatar Gus Grubba Committed by GitHub

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

We can only filter by component ID
parents aa7a4582 e8618672
......@@ -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";
}
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment