diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 3eb7219b27296d1248667d7d737bac2e533d429e..49600b339bb21e519bcffcaf378757d882646df9 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -61,13 +61,13 @@ static const char* kPhotoLapseCount = "PhotoLapseCount"; //----------------------------------------------------------------------------- // Known Parameters -static const char *kCAM_EV = "CAM_EV"; -static const char *kCAM_EXPMODE = "CAM_EXPMODE"; -static const char *kCAM_ISO = "CAM_ISO"; -static const char* kCAM_SHUTTER = "CAM_SHUTTER"; -static const char* kCAM_APERTURE = "CAM_APERTURE"; -static const char* kCAM_WBMODE = "CAM_WBMODE"; -static const char* kCAM_MODE = "CAM_MODE"; +const char* QGCCameraControl::kCAM_EV = "CAM_EV"; +const char* QGCCameraControl::kCAM_EXPMODE = "CAM_EXPMODE"; +const char* QGCCameraControl::kCAM_ISO = "CAM_ISO"; +const char* QGCCameraControl::kCAM_SHUTTERSPD = "CAM_SHUTTERSPD"; +const char* QGCCameraControl::kCAM_APERTURE = "CAM_APERTURE"; +const char* QGCCameraControl::kCAM_WBMODE = "CAM_WBMODE"; +const char* QGCCameraControl::kCAM_MODE = "CAM_MODE"; //----------------------------------------------------------------------------- QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) @@ -1990,9 +1990,9 @@ QGCCameraControl::iso() //----------------------------------------------------------------------------- Fact* -QGCCameraControl::shutter() +QGCCameraControl::shutterSpeed() { - return (_paramComplete && _activeSettings.contains(kCAM_SHUTTER)) ? getFact(kCAM_SHUTTER) : nullptr; + return (_paramComplete && _activeSettings.contains(kCAM_SHUTTERSPD)) ? getFact(kCAM_SHUTTERSPD) : nullptr; } //----------------------------------------------------------------------------- diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h index 7255e1974bc58a6f61bc837a7d2c967308c7a103..8e06b63ef56bfb720d9c9dbee74cf844b6e45d95 100644 --- a/src/Camera/QGCCameraControl.h +++ b/src/Camera/QGCCameraControl.h @@ -144,7 +144,7 @@ public: Q_PROPERTY(Fact* exposureMode READ exposureMode NOTIFY parametersReady) Q_PROPERTY(Fact* ev READ ev NOTIFY parametersReady) Q_PROPERTY(Fact* iso READ iso NOTIFY parametersReady) - Q_PROPERTY(Fact* shutter READ shutter NOTIFY parametersReady) + Q_PROPERTY(Fact* shutterSpeed READ shutterSpeed NOTIFY parametersReady) Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady) Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady) Q_PROPERTY(Fact* mode READ mode NOTIFY parametersReady) @@ -222,7 +222,7 @@ public: virtual Fact* exposureMode (); virtual Fact* ev (); virtual Fact* iso (); - virtual Fact* shutter (); + virtual Fact* shutterSpeed (); virtual Fact* aperture (); virtual Fact* wb (); virtual Fact* mode (); @@ -249,6 +249,16 @@ public: //-- Allow controller to modify or invalidate parameter change virtual bool validateParameter (Fact* pFact, QVariant& newValue); + + // Known Parameters + static const char* kCAM_EV; + static const char* kCAM_EXPMODE; + static const char* kCAM_ISO; + static const char* kCAM_SHUTTERSPD; + static const char* kCAM_APERTURE; + static const char* kCAM_WBMODE; + static const char* kCAM_MODE; + signals: void infoChanged (); void videoStatusChanged (); diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 20e027b1d69711258a70b2cc7d288fcf3d17bce5..bcf3cce4be8bd63b55f584c217c83fa2dc3ba6d2 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -109,35 +109,39 @@ QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) //-- If this heartbeat is from a different component within the vehicle if(_vehicleReadyState && _vehicle->id() == message.sysid && _vehicle->defaultComponentId() != message.compid) { //-- 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; CameraStruct* pInfo = new CameraStruct(this); pInfo->lastHeartbeat.start(); - _cameraInfoRequest[message.compid] = pInfo; + _cameraInfoRequest[sCompID] = pInfo; //-- Request camera info _requestCameraInfo(message.compid); } else { - if(_cameraInfoRequest[message.compid]) { + if(_cameraInfoRequest[sCompID]) { + CameraStruct* pInfo = _cameraInfoRequest[sCompID]; //-- 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 - _cameraInfoRequest[message.compid]->lastHeartbeat.start(); + pInfo->lastHeartbeat.start(); } else { //-- Try again. Maybe. - if(_cameraInfoRequest[message.compid]->lastHeartbeat.elapsed() > 2000) { - if(_cameraInfoRequest[message.compid]->tryCount > 3) { - if(!_cameraInfoRequest[message.compid]->gaveUp) { - _cameraInfoRequest[message.compid]->gaveUp = true; + 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 { - _cameraInfoRequest[message.compid]->tryCount++; + pInfo->tryCount++; //-- Request camera info. Again. It could be something other than a camera, in which // case, we won't ever receive it. _requestCameraInfo(message.compid); } } } + } else { + qWarning() << "_cameraInfoRequest[" << sCompID << "] is null"; } } } @@ -191,9 +195,10 @@ void QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) { //-- 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 - _cameraInfoRequest[message.compid]->infoReceived = true; + _cameraInfoRequest[sCompID]->infoReceived = true; mavlink_camera_information_t info; mavlink_msg_camera_information_decode(&message, &info); qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast(info.model_name) << reinterpret_cast(info.vendor_name) << "Comp ID:" << message.compid; @@ -213,14 +218,15 @@ void QGCCameraManager::_cameraTimeout() { //-- Iterate cameras - for(int i = 0; i < _cameraInfoRequest.count(); i++) { - if(_cameraInfoRequest[i]) { + foreach(QString sCompID, _cameraInfoRequest.keys()) { + if(_cameraInfoRequest[sCompID]) { + CameraStruct* pInfo = _cameraInfoRequest[sCompID]; //-- Have we received a camera info message? - if(_cameraInfoRequest[i]->infoReceived) { + if(pInfo->infoReceived) { //-- Has the camera stopped talking to us? - if(_cameraInfoRequest[i]->lastHeartbeat.elapsed() > 5000) { + if(pInfo->lastHeartbeat.elapsed() > 5000) { //-- Camera is gone. Remove it. - QGCCameraControl* pCamera = _findCamera(i); + QGCCameraControl* pCamera = _findCamera(pInfo->compID); qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list."; int idx = _cameraLabels.indexOf(pCamera->modelName()); if(idx >= 0) { @@ -232,8 +238,8 @@ QGCCameraManager::_cameraTimeout() } bool autoStream = pCamera->autoStream(); pCamera->deleteLater(); - delete _cameraInfoRequest[i]; - _cameraInfoRequest.remove(i); + delete pInfo; + _cameraInfoRequest.remove(sCompID); emit cameraLabelsChanged(); //-- If we have another camera, switch current camera. if(_cameras.count()) { diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 6b882480cfc9ed0e34c11d88e72ed5eb7e921afc..8d52976d7a2359360401b2b1f0a0d48fce7c05ae 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -81,6 +81,7 @@ protected: bool infoReceived = false; bool gaveUp = false; int tryCount = 0; + uint8_t compID = 0; }; Vehicle* _vehicle = nullptr; @@ -93,5 +94,5 @@ protected: QTime _lastZoomChange; QTime _lastCameraChange; QTimer _cameraTimer; - QMap _cameraInfoRequest; + QMap _cameraInfoRequest; };