Commit 3df6b0af authored by Gus Grubba's avatar Gus Grubba Committed by Gus Grubba

CP - Handle camera info QMap

parent 0a4fe0e4
...@@ -109,35 +109,39 @@ QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) ...@@ -109,35 +109,39 @@ QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message)
//-- If this heartbeat is from a different component within the vehicle //-- If this heartbeat is from a different component within the vehicle
if(_vehicleReadyState && _vehicle->id() == message.sysid && _vehicle->defaultComponentId() != message.compid) { if(_vehicleReadyState && _vehicle->id() == message.sysid && _vehicle->defaultComponentId() != message.compid) {
//-- First time hearing from this one? //-- First time hearing from this one?
if(!_cameraInfoRequest.contains(message.compid)) { QString sCompID = QString::number(message.compid);
if(!_cameraInfoRequest.contains(sCompID)) {
qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid; qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid;
CameraStruct* pInfo = new CameraStruct(this); CameraStruct* pInfo = new CameraStruct(this);
pInfo->lastHeartbeat.start(); pInfo->lastHeartbeat.start();
_cameraInfoRequest[message.compid] = pInfo; _cameraInfoRequest[sCompID] = pInfo;
//-- Request camera info //-- Request camera info
_requestCameraInfo(message.compid); _requestCameraInfo(message.compid);
} else { } else {
if(_cameraInfoRequest[message.compid]) { if(_cameraInfoRequest[sCompID]) {
CameraStruct* pInfo = _cameraInfoRequest[sCompID];
//-- Check if we have indeed received the camera info //-- Check if we have indeed received the camera info
if(_cameraInfoRequest[message.compid]->infoReceived) { if(pInfo->infoReceived) {
//-- We have it. Just update the heartbeat timeout //-- We have it. Just update the heartbeat timeout
_cameraInfoRequest[message.compid]->lastHeartbeat.start(); pInfo->lastHeartbeat.start();
} else { } else {
//-- Try again. Maybe. //-- Try again. Maybe.
if(_cameraInfoRequest[message.compid]->lastHeartbeat.elapsed() > 2000) { if(pInfo->lastHeartbeat.elapsed() > 2000) {
if(_cameraInfoRequest[message.compid]->tryCount > 3) { if(pInfo->tryCount > 3) {
if(!_cameraInfoRequest[message.compid]->gaveUp) { if(!pInfo->gaveUp) {
_cameraInfoRequest[message.compid]->gaveUp = true; pInfo->gaveUp = true;
qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid; qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid;
} }
} else { } else {
_cameraInfoRequest[message.compid]->tryCount++; pInfo->tryCount++;
//-- Request camera info. Again. It could be something other than a camera, in which //-- Request camera info. Again. It could be something other than a camera, in which
// case, we won't ever receive it. // case, we won't ever receive it.
_requestCameraInfo(message.compid); _requestCameraInfo(message.compid);
} }
} }
} }
} else {
qWarning() << "_cameraInfoRequest[" << sCompID << "] is null";
} }
} }
} }
...@@ -191,9 +195,10 @@ void ...@@ -191,9 +195,10 @@ void
QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message)
{ {
//-- Have we requested it? //-- Have we requested it?
if(_cameraInfoRequest.contains(message.compid) && !_cameraInfoRequest[message.compid]->infoReceived) { QString sCompID = QString::number(message.compid);
if(_cameraInfoRequest.contains(sCompID) && !_cameraInfoRequest[sCompID]->infoReceived) {
//-- Flag it as done //-- Flag it as done
_cameraInfoRequest[message.compid]->infoReceived = true; _cameraInfoRequest[sCompID]->infoReceived = true;
mavlink_camera_information_t info; mavlink_camera_information_t info;
mavlink_msg_camera_information_decode(&message, &info); mavlink_msg_camera_information_decode(&message, &info);
qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast<const char*>(info.model_name) << reinterpret_cast<const char*>(info.vendor_name) << "Comp ID:" << message.compid; qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast<const char*>(info.model_name) << reinterpret_cast<const char*>(info.vendor_name) << "Comp ID:" << message.compid;
...@@ -213,14 +218,15 @@ void ...@@ -213,14 +218,15 @@ void
QGCCameraManager::_cameraTimeout() QGCCameraManager::_cameraTimeout()
{ {
//-- Iterate cameras //-- Iterate cameras
for(int i = 0; i < _cameraInfoRequest.count(); i++) { foreach(QString sCompID, _cameraInfoRequest.keys()) {
if(_cameraInfoRequest[i]) { if(_cameraInfoRequest[sCompID]) {
CameraStruct* pInfo = _cameraInfoRequest[sCompID];
//-- Have we received a camera info message? //-- Have we received a camera info message?
if(_cameraInfoRequest[i]->infoReceived) { if(pInfo->infoReceived) {
//-- Has the camera stopped talking to us? //-- Has the camera stopped talking to us?
if(_cameraInfoRequest[i]->lastHeartbeat.elapsed() > 5000) { if(pInfo->lastHeartbeat.elapsed() > 5000) {
//-- Camera is gone. Remove it. //-- Camera is gone. Remove it.
QGCCameraControl* pCamera = _findCamera(i); QGCCameraControl* pCamera = _findCamera(pInfo->compID);
qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list."; qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list.";
int idx = _cameraLabels.indexOf(pCamera->modelName()); int idx = _cameraLabels.indexOf(pCamera->modelName());
if(idx >= 0) { if(idx >= 0) {
...@@ -232,8 +238,8 @@ QGCCameraManager::_cameraTimeout() ...@@ -232,8 +238,8 @@ QGCCameraManager::_cameraTimeout()
} }
bool autoStream = pCamera->autoStream(); bool autoStream = pCamera->autoStream();
pCamera->deleteLater(); pCamera->deleteLater();
delete _cameraInfoRequest[i]; delete pInfo;
_cameraInfoRequest.remove(i); _cameraInfoRequest.remove(sCompID);
emit cameraLabelsChanged(); emit cameraLabelsChanged();
//-- If we have another camera, switch current camera. //-- If we have another camera, switch current camera.
if(_cameras.count()) { if(_cameras.count()) {
......
...@@ -81,6 +81,7 @@ protected: ...@@ -81,6 +81,7 @@ protected:
bool infoReceived = false; bool infoReceived = false;
bool gaveUp = false; bool gaveUp = false;
int tryCount = 0; int tryCount = 0;
uint8_t compID = 0;
}; };
Vehicle* _vehicle = nullptr; Vehicle* _vehicle = nullptr;
...@@ -93,5 +94,5 @@ protected: ...@@ -93,5 +94,5 @@ protected:
QTime _lastZoomChange; QTime _lastZoomChange;
QTime _lastCameraChange; QTime _lastCameraChange;
QTimer _cameraTimer; QTimer _cameraTimer;
QMap<int, CameraStruct*> _cameraInfoRequest; QMap<QString, CameraStruct*> _cameraInfoRequest;
}; };
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