From 4b3cddbcadfefdca21e8d25580ff9b36e7dd6884 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 30 Jul 2019 11:17:43 -0700 Subject: [PATCH 01/10] Remove unused modeCount --- src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h | 1 - src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h | 1 - src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h | 1 - 3 files changed, 3 deletions(-) diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h index cd0ce2e..0094a66 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h @@ -42,7 +42,6 @@ public: QLOITER = 19, QLAND = 20, QRTL = 21, - modeCount }; APMPlaneMode(uint32_t mode, bool settable); diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index 6dd7882..ad17a03 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -33,7 +33,6 @@ public: GUIDED = 15, INITIALIZING = 16, }; - static const int modeCount = 17; APMRoverMode(uint32_t mode, bool settable); }; diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h index b826c1e..7f6426b 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h @@ -96,7 +96,6 @@ public: RESERVED_18 = 18, MANUAL = 19 }; - static const int modeCount = 20; APMSubMode(uint32_t mode, bool settable); }; From 18bd5177cfad199bfc525f381c7cc28281e8cb77 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 30 Jul 2019 11:17:54 -0700 Subject: [PATCH 02/10] Bump GoTo limit to 10K. --- src/Vehicle/Vehicle.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index eaa6579..c56c477 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3041,7 +3041,7 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord) if (!coordinate().isValid()) { return; } - double maxDistance = 1000.0; + double maxDistance = 10000.0; if (coordinate().distanceTo(gotoCoord) > maxDistance) { qgcApp()->showMessage(QString("New location is too far. Must be less than %1 %2").arg(qRound(FactMetaData::metersToAppSettingsDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsDistanceUnitsString())); return; From 6422cc92626fa36ba2eebcf9a722e2f37756c67f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 30 Jul 2019 11:18:05 -0700 Subject: [PATCH 03/10] Add missing flight modes --- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 82 ++++++++++++---------- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h | 6 +- 2 files changed, 48 insertions(+), 40 deletions(-) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index e5ba76c..7275a91 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -23,50 +23,56 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : APMCustomMode(mode, settable) { setEnumToStringMapping({ - {STABILIZE, "Stabilize"}, - {ACRO, "Acro"}, - {ALT_HOLD, "Altitude Hold"}, - {AUTO, "Auto"}, - {GUIDED, "Guided"}, - {LOITER, "Loiter"}, - {RTL, "RTL"}, - {CIRCLE, "Circle"}, - {LAND, "Land"}, - {DRIFT, "Drift"}, - {SPORT, "Sport"}, - {FLIP, "Flip"}, - {AUTOTUNE, "Autotune"}, - {POS_HOLD, "Position Hold"}, - {BRAKE, "Brake"}, - {THROW, "Throw"}, - {AVOID_ADSB, "Avoid ADSB"}, - {GUIDED_NOGPS, "Guided No GPS"}, - {SAFE_RTL, "Smart RTL"}, + { STABILIZE, "Stabilize"}, + { ACRO, "Acro"}, + { ALT_HOLD, "Altitude Hold"}, + { AUTO, "Auto"}, + { GUIDED, "Guided"}, + { LOITER, "Loiter"}, + { RTL, "RTL"}, + { CIRCLE, "Circle"}, + { LAND, "Land"}, + { DRIFT, "Drift"}, + { SPORT, "Sport"}, + { FLIP, "Flip"}, + { AUTOTUNE, "Autotune"}, + { POS_HOLD, "Position Hold"}, + { BRAKE, "Brake"}, + { THROW, "Throw"}, + { AVOID_ADSB, "Avoid ADSB"}, + { GUIDED_NOGPS, "Guided No GPS"}, + { SMART_RTL, "Smart RTL"}, + { FLOWHOLD, "Flow Hold" }, + { FOLLOW, "Follow" }, + { ZIGZAG, "ZigZag" }, }); } ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void) { setSupportedModes({ - APMCopterMode(APMCopterMode::STABILIZE ,true), - APMCopterMode(APMCopterMode::ACRO ,true), - APMCopterMode(APMCopterMode::ALT_HOLD ,true), - APMCopterMode(APMCopterMode::AUTO ,true), - APMCopterMode(APMCopterMode::GUIDED ,true), - APMCopterMode(APMCopterMode::LOITER ,true), - APMCopterMode(APMCopterMode::RTL ,true), - APMCopterMode(APMCopterMode::CIRCLE ,true), - APMCopterMode(APMCopterMode::LAND ,true), - APMCopterMode(APMCopterMode::DRIFT ,true), - APMCopterMode(APMCopterMode::SPORT ,true), - APMCopterMode(APMCopterMode::FLIP ,true), - APMCopterMode(APMCopterMode::AUTOTUNE ,true), - APMCopterMode(APMCopterMode::POS_HOLD ,true), - APMCopterMode(APMCopterMode::BRAKE ,true), - APMCopterMode(APMCopterMode::THROW ,true), - APMCopterMode(APMCopterMode::AVOID_ADSB,true), - APMCopterMode(APMCopterMode::GUIDED_NOGPS,true), - APMCopterMode(APMCopterMode::SAFE_RTL,true), + APMCopterMode(APMCopterMode::STABILIZE, true), + APMCopterMode(APMCopterMode::ACRO, true), + APMCopterMode(APMCopterMode::ALT_HOLD, true), + APMCopterMode(APMCopterMode::AUTO, true), + APMCopterMode(APMCopterMode::GUIDED, true), + APMCopterMode(APMCopterMode::LOITER, true), + APMCopterMode(APMCopterMode::RTL, true), + APMCopterMode(APMCopterMode::CIRCLE, true), + APMCopterMode(APMCopterMode::LAND, true), + APMCopterMode(APMCopterMode::DRIFT, true), + APMCopterMode(APMCopterMode::SPORT, true), + APMCopterMode(APMCopterMode::FLIP, true), + APMCopterMode(APMCopterMode::AUTOTUNE, true), + APMCopterMode(APMCopterMode::POS_HOLD, true), + APMCopterMode(APMCopterMode::BRAKE, true), + APMCopterMode(APMCopterMode::THROW, true), + APMCopterMode(APMCopterMode::AVOID_ADSB, true), + APMCopterMode(APMCopterMode::GUIDED_NOGPS, true), + APMCopterMode(APMCopterMode::SMART_RTL, true), + APMCopterMode(APMCopterMode::FLOWHOLD, true), + APMCopterMode(APMCopterMode::FOLLOW, true), + APMCopterMode(APMCopterMode::ZIGZAG, true), }); if (!_remapParamNameIntialized) { diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 563baf7..a675e74 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -41,9 +41,11 @@ public: THROW = 18, AVOID_ADSB = 19, GUIDED_NOGPS= 20, - SAFE_RTL = 21, //Safe Return to Launch + SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps + FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder + FOLLOW = 23, // follow attempts to follow another vehicle or ground station + ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B }; - static const int modeCount = 22; APMCopterMode(uint32_t mode, bool settable); }; From da28a454feb42b84651963439c9e8b1479bd1169 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 30 Jul 2019 11:35:46 -0700 Subject: [PATCH 04/10] Change to Follow Vehicle since Follow Me is not supported --- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 7275a91..10e5abb 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -43,7 +43,7 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : { GUIDED_NOGPS, "Guided No GPS"}, { SMART_RTL, "Smart RTL"}, { FLOWHOLD, "Flow Hold" }, - { FOLLOW, "Follow" }, + { FOLLOW, "Follow Vehicle" }, { ZIGZAG, "ZigZag" }, }); } From 2647db0e7986cdceb7de0f61138025e82f727f9c Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Wed, 7 Aug 2019 13:32:30 -0400 Subject: [PATCH 05/10] Show how to implement custom video manager --- custom-example/custom.pri | 6 ++- custom-example/src/CustomPlugin.cc | 8 ++++ custom-example/src/CustomPlugin.h | 1 + custom-example/src/CustomVideoManager.cc | 39 +++++++++++++++++ custom-example/src/CustomVideoManager.h | 28 +++++++++++++ .../src/FirmwarePlugin/CustomCameraControl.cc | 8 ++++ .../src/FirmwarePlugin/CustomCameraControl.h | 3 ++ src/Camera/QGCCameraControl.cc | 24 ----------- src/Camera/QGCCameraControl.h | 10 ----- src/FlightDisplay/VideoManager.cc | 7 ---- src/FlightDisplay/VideoManager.h | 49 +++++++++++----------- src/VideoStreaming/VideoReceiver.cc | 5 ++- src/api/QGCCorePlugin.cc | 6 +++ src/api/QGCCorePlugin.h | 2 + 14 files changed, 126 insertions(+), 70 deletions(-) create mode 100644 custom-example/src/CustomVideoManager.cc create mode 100644 custom-example/src/CustomVideoManager.h diff --git a/custom-example/custom.pri b/custom-example/custom.pri index 03cd010..ab24fd8 100644 --- a/custom-example/custom.pri +++ b/custom-example/custom.pri @@ -58,11 +58,13 @@ QML_IMPORT_PATH += \ # Our own, custom sources SOURCES += \ $$PWD/src/CustomPlugin.cc \ - $$PWD/src/CustomQuickInterface.cc + $$PWD/src/CustomQuickInterface.cc \ + $$PWD/src/CustomVideoManager.cc HEADERS += \ $$PWD/src/CustomPlugin.h \ - $$PWD/src/CustomQuickInterface.h + $$PWD/src/CustomQuickInterface.h \ + $$PWD/src/CustomVideoManager.h INCLUDEPATH += \ $$PWD/src \ diff --git a/custom-example/src/CustomPlugin.cc b/custom-example/src/CustomPlugin.cc index 87fc0d5..f715a4e 100644 --- a/custom-example/src/CustomPlugin.cc +++ b/custom-example/src/CustomPlugin.cc @@ -17,6 +17,7 @@ #include "CustomPlugin.h" #include "CustomQuickInterface.h" +#include "CustomVideoManager.h" #include "MultiVehicleManager.h" #include "QGCApplication.h" @@ -192,6 +193,13 @@ CustomPlugin::overrideSettingsGroupVisibility(QString name) } //----------------------------------------------------------------------------- +VideoManager* +CustomPlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox) +{ + return new CustomVideoManager(app, toolbox); +} + +//----------------------------------------------------------------------------- VideoReceiver* CustomPlugin::createVideoReceiver(QObject* parent) { diff --git a/custom-example/src/CustomPlugin.h b/custom-example/src/CustomPlugin.h index b089c3e..d97746f 100644 --- a/custom-example/src/CustomPlugin.h +++ b/custom-example/src/CustomPlugin.h @@ -79,6 +79,7 @@ public: QString brandImageIndoor () const final; QString brandImageOutdoor () const final; bool overrideSettingsGroupVisibility (QString name) final; + VideoManager* createVideoManager (QGCApplication* app, QGCToolbox* toolbox) final; VideoReceiver* createVideoReceiver (QObject* parent) final; QQmlApplicationEngine* createRootWindow (QObject* parent) final; bool adjustSettingMetaData (const QString& settingsGroup, FactMetaData& metaData) final; diff --git a/custom-example/src/CustomVideoManager.cc b/custom-example/src/CustomVideoManager.cc new file mode 100644 index 0000000..7e72973 --- /dev/null +++ b/custom-example/src/CustomVideoManager.cc @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * (c) 2009-2019 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "CustomVideoManager.h" +#include "MultiVehicleManager.h" +#include "CustomCameraManager.h" +#include "CustomCameraControl.h" + +//----------------------------------------------------------------------------- +CustomVideoManager::CustomVideoManager(QGCApplication* app, QGCToolbox* toolbox) + : VideoManager(app, toolbox) +{ +} + +//----------------------------------------------------------------------------- +void +CustomVideoManager::_updateSettings() +{ + if(!_videoSettings || !_videoReceiver) + return; + //-- Check encoding + if(_activeVehicle && _activeVehicle->dynamicCameras()) { + CustomCameraControl* pCamera = dynamic_cast(_activeVehicle->dynamicCameras()->currentCameraInstance()); + if(pCamera) { + Fact *fact = pCamera->videoEncoding(); + if (fact) { + _videoReceiver->setVideoDecoder(static_cast(fact->rawValue().toInt())); + } + } + } + VideoManager::_updateSettings(); +} + diff --git a/custom-example/src/CustomVideoManager.h b/custom-example/src/CustomVideoManager.h new file mode 100644 index 0000000..58d7fe1 --- /dev/null +++ b/custom-example/src/CustomVideoManager.h @@ -0,0 +1,28 @@ +/**************************************************************************** + * + * (c) 2009-2019 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "VideoManager.h" + +class CustomVideoManager : public VideoManager +{ + Q_OBJECT +public: + CustomVideoManager (QGCApplication* app, QGCToolbox* toolbox); + +protected: + void _updateSettings (); + +}; diff --git a/custom-example/src/FirmwarePlugin/CustomCameraControl.cc b/custom-example/src/FirmwarePlugin/CustomCameraControl.cc index 7817a46..4311928 100644 --- a/custom-example/src/FirmwarePlugin/CustomCameraControl.cc +++ b/custom-example/src/FirmwarePlugin/CustomCameraControl.cc @@ -19,6 +19,7 @@ QGC_LOGGING_CATEGORY(CustomCameraVerboseLog, "CustomCameraVerboseLog") static const char* kCAM_IRPALETTE = "CAM_IRPALETTE"; static const char* kCAM_NEXTVISION_IRPALETTE = "IR_SENS_POL"; +static const char* kCAM_ENC = "CAM_ENC"; //----------------------------------------------------------------------------- CustomCameraControl::CustomCameraControl(const mavlink_camera_information_t *info, Vehicle* vehicle, int compID, QObject* parent) @@ -117,6 +118,13 @@ CustomCameraControl::irPalette() } //----------------------------------------------------------------------------- +Fact* +CustomCameraControl::videoEncoding() +{ + return _paramComplete ? getFact(kCAM_ENC) : nullptr; +} + +//----------------------------------------------------------------------------- void CustomCameraControl::setThermalMode(ThermalViewMode mode) { diff --git a/custom-example/src/FirmwarePlugin/CustomCameraControl.h b/custom-example/src/FirmwarePlugin/CustomCameraControl.h index 475ed33..a52dae9 100644 --- a/custom-example/src/FirmwarePlugin/CustomCameraControl.h +++ b/custom-example/src/FirmwarePlugin/CustomCameraControl.h @@ -29,8 +29,11 @@ public: CustomCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = nullptr); Q_PROPERTY(Fact* irPalette READ irPalette NOTIFY parametersReady) + Q_PROPERTY(Fact* videoEncoding READ videoEncoding NOTIFY parametersReady) Fact* irPalette (); + Fact* videoEncoding (); + bool takePhoto () override; bool stopTakePhoto () override; bool startVideo () override; diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index f50a6f5..ed69458 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -70,9 +70,6 @@ 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"; -const char* QGCCameraControl::kCAM_BITRATE = "CAM_BITRATE"; -const char* QGCCameraControl::kCAM_FPS = "CAM_FPS"; -const char* QGCCameraControl::kCAM_ENC = "CAM_ENC"; //----------------------------------------------------------------------------- QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) @@ -2117,27 +2114,6 @@ QGCCameraControl::mode() } //----------------------------------------------------------------------------- -Fact* -QGCCameraControl::bitRate() -{ - return _paramComplete ? getFact(kCAM_BITRATE) : nullptr; -} - -//----------------------------------------------------------------------------- -Fact* -QGCCameraControl::frameRate() -{ - return _paramComplete ? getFact(kCAM_FPS) : nullptr; -} - -//----------------------------------------------------------------------------- -Fact* -QGCCameraControl::videoEncoding() -{ - return _paramComplete ? getFact(kCAM_ENC) : nullptr; -} - -//----------------------------------------------------------------------------- QGCVideoStreamInfo::QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t *si) : QObject(parent) { diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h index d0a21df..95f0991 100644 --- a/src/Camera/QGCCameraControl.h +++ b/src/Camera/QGCCameraControl.h @@ -167,9 +167,6 @@ public: Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady) Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady) Q_PROPERTY(Fact* mode READ mode NOTIFY parametersReady) - Q_PROPERTY(Fact* bitRate READ bitRate NOTIFY parametersReady) - Q_PROPERTY(Fact* frameRate READ frameRate NOTIFY parametersReady) - Q_PROPERTY(Fact* videoEncoding READ videoEncoding NOTIFY parametersReady) Q_PROPERTY(QStringList activeSettings READ activeSettings NOTIFY activeSettingsChanged) Q_PROPERTY(VideoStatus videoStatus READ videoStatus NOTIFY videoStatusChanged) @@ -255,9 +252,6 @@ public: virtual Fact* aperture (); virtual Fact* wb (); virtual Fact* mode (); - virtual Fact* bitRate (); - virtual Fact* frameRate (); - virtual Fact* videoEncoding (); //-- Stream names to show the user (for selection) virtual QStringList streamLabels () { return _streamLabels; } @@ -289,7 +283,6 @@ 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; @@ -298,9 +291,6 @@ public: static const char* kCAM_APERTURE; static const char* kCAM_WBMODE; static const char* kCAM_MODE; - static const char* kCAM_BITRATE; - static const char* kCAM_FPS; - static const char* kCAM_ENC; signals: void infoChanged (); diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index f825927..23db603 100644 --- a/src/FlightDisplay/VideoManager.cc +++ b/src/FlightDisplay/VideoManager.cc @@ -279,13 +279,6 @@ VideoManager::_updateSettings() return; //-- Auto discovery if(_activeVehicle && _activeVehicle->dynamicCameras()) { - QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); - if(pCamera) { - Fact *fact = pCamera->videoEncoding(); - if (fact) { - _videoReceiver->setVideoDecoder(static_cast(fact->rawValue().toInt())); - } - } QGCVideoStreamInfo* pInfo = _activeVehicle->dynamicCameras()->currentStreamInstance(); if(pInfo) { qCDebug(VideoManagerLog) << "Configure primary stream: " << pInfo->uri(); diff --git a/src/FlightDisplay/VideoManager.h b/src/FlightDisplay/VideoManager.h index 7d9ad61..dfeafdc 100644 --- a/src/FlightDisplay/VideoManager.h +++ b/src/FlightDisplay/VideoManager.h @@ -34,7 +34,7 @@ class VideoManager : public QGCTool public: VideoManager (QGCApplication* app, QGCToolbox* toolbox); - ~VideoManager (); + virtual ~VideoManager (); Q_PROPERTY(bool hasVideo READ hasVideo NOTIFY hasVideoChanged) Q_PROPERTY(bool isGStreamer READ isGStreamer NOTIFY isGStreamerChanged) @@ -51,34 +51,33 @@ public: Q_PROPERTY(bool autoStreamConfigured READ autoStreamConfigured NOTIFY autoStreamConfiguredChanged) Q_PROPERTY(bool hasThermal READ hasThermal NOTIFY aspectRatioChanged) - bool hasVideo (); - bool isGStreamer (); - bool isAutoStream (); - bool isTaisync () { return _isTaisync; } - bool fullScreen () { return _fullScreen; } - QString videoSourceID () { return _videoSourceID; } - double aspectRatio (); - double thermalAspectRatio (); - double hfov (); - double thermalHfov (); - bool autoStreamConfigured(); - bool hasThermal (); - void restartVideo (); - - VideoReceiver* videoReceiver () { return _videoReceiver; } - VideoReceiver* thermalVideoReceiver () { return _thermalVideoReceiver; } + virtual bool hasVideo (); + virtual bool isGStreamer (); + virtual bool isTaisync () { return _isTaisync; } + virtual bool fullScreen () { return _fullScreen; } + virtual QString videoSourceID () { return _videoSourceID; } + virtual double aspectRatio (); + virtual double thermalAspectRatio (); + virtual double hfov (); + virtual double thermalHfov (); + virtual bool autoStreamConfigured(); + virtual bool hasThermal (); + virtual void restartVideo (); + + virtual VideoReceiver* videoReceiver () { return _videoReceiver; } + virtual VideoReceiver* thermalVideoReceiver () { return _thermalVideoReceiver; } #if defined(QGC_DISABLE_UVC) - bool uvcEnabled () { return false; } + virtual bool uvcEnabled () { return false; } #else - bool uvcEnabled (); + virtual bool uvcEnabled (); #endif - void setfullScreen (bool f) { _fullScreen = f; emit fullScreenChanged(); } - void setIsTaisync (bool t) { _isTaisync = t; emit isTaisyncChanged(); } + virtual void setfullScreen (bool f) { _fullScreen = f; emit fullScreenChanged(); } + virtual void setIsTaisync (bool t) { _isTaisync = t; emit isTaisyncChanged(); } // Override from QGCTool - void setToolbox (QGCToolbox *toolbox); + virtual void setToolbox (QGCToolbox *toolbox); Q_INVOKABLE void startVideo (); Q_INVOKABLE void stopVideo (); @@ -93,7 +92,7 @@ signals: void aspectRatioChanged (); void autoStreamConfiguredChanged(); -private slots: +protected slots: void _videoSourceChanged (); void _udpPortChanged (); void _rtspUrlChanged (); @@ -102,10 +101,10 @@ private slots: void _setActiveVehicle (Vehicle* vehicle); void _aspectRatioChanged (); -private: +protected: void _updateSettings (); -private: +protected: SubtitleWriter _subtitleWriter; bool _isTaisync = false; VideoReceiver* _videoReceiver = nullptr; diff --git a/src/VideoStreaming/VideoReceiver.cc b/src/VideoStreaming/VideoReceiver.cc index cd62f8d..dc96386 100644 --- a/src/VideoStreaming/VideoReceiver.cc +++ b/src/VideoStreaming/VideoReceiver.cc @@ -611,14 +611,14 @@ VideoReceiver::setVideoDecoder(VideoEncoding encoding) */ if (encoding == H265_HW || encoding == H265_SW) { - _depayName = "rtph265depay"; + _depayName = "rtph265depay"; _parserName = "h265parse"; #if defined(__android__) _hwDecoderName = "amcviddec-omxgooglehevcdecoder"; #endif _swDecoderName = "avdec_h265"; } else { - _depayName = "rtph264depay"; + _depayName = "rtph264depay"; _parserName = "h264parse"; #if defined(__android__) _hwDecoderName = "amcviddec-omxgoogleh264decoder"; @@ -630,6 +630,7 @@ VideoReceiver::setVideoDecoder(VideoEncoding encoding) _hwDecoderName = nullptr; } } + //----------------------------------------------------------------------------- // When we finish our pipeline will look like this: // diff --git a/src/api/QGCCorePlugin.cc b/src/api/QGCCorePlugin.cc index 91d95d2..9c22b19 100644 --- a/src/api/QGCCorePlugin.cc +++ b/src/api/QGCCorePlugin.cc @@ -15,6 +15,7 @@ #include "SettingsManager.h" #include "AppMessages.h" #include "QmlObjectListModel.h" +#include "VideoManager.h" #include "VideoReceiver.h" #include "QGCLoggingCategory.h" #include "QGCCameraManager.h" @@ -408,6 +409,11 @@ QmlObjectListModel* QGCCorePlugin::customMapItems() return &_p->_emptyCustomMapItems; } +VideoManager* QGCCorePlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox) +{ + return new VideoManager(app, toolbox); +} + VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent) { return new VideoReceiver(parent); diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h index a299944..3187789 100644 --- a/src/api/QGCCorePlugin.h +++ b/src/api/QGCCorePlugin.h @@ -104,6 +104,8 @@ public: /// Allows the plugin to override the creation of the root (native) window. virtual QQmlApplicationEngine* createRootWindow(QObject* parent); + /// Allows the plugin to override the creation of VideoManager. + virtual VideoManager* createVideoManager(QGCApplication* app, QGCToolbox* toolbox); /// Allows the plugin to override the creation of VideoReceiver. virtual VideoReceiver* createVideoReceiver(QObject* parent); From 29d7cd6c5b66e456b7ac68402e65cfb97d97070b Mon Sep 17 00:00:00 2001 From: PX4BuildBot Date: Fri, 9 Aug 2019 07:55:08 +0000 Subject: [PATCH 06/10] Update PX4 Firmware metadata Fri Aug 9 07:55:08 UTC 2019 --- src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index f5c8e75..0334257 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -537,6 +537,11 @@ Set to 2 to use heading from motion capture * the MSB bit is not used to avoid problems in the conversion between int and uint Default value: (10 << 0 | 1000 << 8 | 0 << 24) = 256010 - authorizer system id = 10 - authentication method parameter = 10000msec of timeout - authentication method = during arm + + Require all the ESCs to be detected to arm + This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param. + + Maximum value of EKF accelerometer delta velocity bias estimate that will allow arming. Note: ekf2 will limit the delta velocity bias estimate magnitude to be less than EKF2_ABL_LIM * FILTER_UPDATE_PERIOD_MS * 0.001 so this parameter must be less than that to be useful From 64a58f8f1393ab146960ecfc118600ad74cde74e Mon Sep 17 00:00:00 2001 From: PX4BuildBot Date: Fri, 9 Aug 2019 09:10:08 +0000 Subject: [PATCH 07/10] Update PX4 Firmware metadata Fri Aug 9 09:10:08 UTC 2019 --- .../PX4/PX4ParameterFactMetaData.xml | 99 ++++++++++++---------- 1 file changed, 55 insertions(+), 44 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index 0334257..0908ff6 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -137,6 +137,61 @@ 0 + + + Airspeed scale (scale from IAS to CAS/EAS) + Scale can either be entered manually, or estimated in-flight by setting ARSP_SCALE_EST to 1. + 0.5 + 1.5 + + + Airspeed Selector: Gate size for true sideslip fusion + Sets the number of standard deviations used by the innovation consistency test. + 1 + 5 + SD + + + Airspeed Selector: Wind estimator sideslip measurement noise + Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector. + 0 + 1 + rad + + + Automatic airspeed scale estimation on + Turns the automatic airspeed scale (scale from IAS to CAS/EAS) on or off. It is recommended level (keeping altitude) while performing the estimation. Set to 1 to start estimation (best when already flying). Set to 0 to end scale estimation. The estimated scale is then saved in the ARSP_ARSP_SCALE parameter. + + + + Airspeed Selector: Wind estimator true airspeed scale process noise + Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector. + 0 + 0.1 + 1/s + + + Airspeed Selector: Gate size for true airspeed fusion + Sets the number of standard deviations used by the innovation consistency test. + 1 + 5 + SD + + + Airspeed Selector: Wind estimator true airspeed measurement noise + True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector. + 0 + 4 + m/s + + + Airspeed Selector: Wind estimator wind process noise + Wind process noise of the internal wind estimator(s) of the airspeed selector. + 0 + 1 + m/s/s + + Acceleration compensation based on GPS @@ -10492,50 +10547,6 @@ to fixed wing mode. Zero or negative values will produce an instant throttle ris 0.01 - - - Gate size for true sideslip fusion - Sets the number of standard deviations used by the innovation consistency test. - 1 - 5 - SD - - - Wind estimator sideslip measurement noise - 0 - 1 - rad - - - Enable Wind estimator - - true - - - Wind estimator true airspeed scale process noise - 0 - 0.1 - - - Gate size for true airspeed fusion - Sets the number of standard deviations used by the innovation consistency test. - 1 - 5 - SD - - - Wind estimator true airspeed measurement noise - 0 - 4 - m/s - - - Wind estimator wind process noise - 0 - 1 - m/s/s - - EXFW_HDNG_P From 397c66ce28c4fe668f801c343c41db0edc9a6847 Mon Sep 17 00:00:00 2001 From: PX4BuildBot Date: Fri, 9 Aug 2019 10:20:27 +0000 Subject: [PATCH 08/10] Update PX4 Firmware metadata Fri Aug 9 10:20:27 UTC 2019 --- src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index 0908ff6..9b79852 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -4765,10 +4765,10 @@ the setpoint will be capped to MPC_XY_VEL_MAX 2.0 2 - + Proportional gain for horizontal trajectory position error 0.1 - 5.0 + 1.0 1 @@ -4822,7 +4822,7 @@ the setpoint will be capped to MPC_XY_VEL_MAX Proportional gain for vertical trajectory position error 0.1 - 5.0 + 1.0 1 From 8092647027b0251b9fedf3e27b4a2807828c0c7c Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Mon, 12 Aug 2019 14:20:06 -0400 Subject: [PATCH 09/10] Disable full screen video if no vehicle or connection lost --- src/FlightDisplay/VideoManager.cc | 29 +++++++++++++++++++++++++++++ src/FlightDisplay/VideoManager.h | 3 ++- 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index 23db603..ee1ac37 100644 --- a/src/FlightDisplay/VideoManager.cc +++ b/src/FlightDisplay/VideoManager.cc @@ -273,6 +273,20 @@ VideoManager::uvcEnabled() //----------------------------------------------------------------------------- void +VideoManager::setfullScreen(bool f) +{ + if(f) { + //-- No can do if no vehicle or connection lost + if(!_activeVehicle || _activeVehicle->connectionLost()) { + f = false; + } + } + _fullScreen = f; + emit fullScreenChanged(); +} + +//----------------------------------------------------------------------------- +void VideoManager::_updateSettings() { if(!_videoSettings || !_videoReceiver) @@ -357,6 +371,7 @@ void VideoManager::_setActiveVehicle(Vehicle* vehicle) { if(_activeVehicle) { + disconnect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged); if(_activeVehicle->dynamicCameras()) { QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); if(pCamera) { @@ -367,6 +382,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) } _activeVehicle = vehicle; if(_activeVehicle) { + connect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged); if(_activeVehicle->dynamicCameras()) { connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo); QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); @@ -374,6 +390,9 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) pCamera->resumeStream(); } } + } else { + //-- Disable full screen video if vehicle is gone + setfullScreen(false); } emit autoStreamConfiguredChanged(); restartVideo(); @@ -381,6 +400,16 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) //---------------------------------------------------------------------------------------- void +VideoManager::_connectionLostChanged(bool connectionLost) +{ + if(connectionLost) { + //-- Disable full screen video if connection is lost + setfullScreen(false); + } +} + +//---------------------------------------------------------------------------------------- +void VideoManager::_aspectRatioChanged() { emit aspectRatioChanged(); diff --git a/src/FlightDisplay/VideoManager.h b/src/FlightDisplay/VideoManager.h index dfeafdc..fcd8707 100644 --- a/src/FlightDisplay/VideoManager.h +++ b/src/FlightDisplay/VideoManager.h @@ -73,7 +73,7 @@ public: virtual bool uvcEnabled (); #endif - virtual void setfullScreen (bool f) { _fullScreen = f; emit fullScreenChanged(); } + virtual void setfullScreen (bool f); virtual void setIsTaisync (bool t) { _isTaisync = t; emit isTaisyncChanged(); } // Override from QGCTool @@ -100,6 +100,7 @@ protected slots: void _updateUVC (); void _setActiveVehicle (Vehicle* vehicle); void _aspectRatioChanged (); + void _connectionLostChanged (bool connectionLost); protected: void _updateSettings (); From f09948742d606167239e8a879b58d3832e7cc708 Mon Sep 17 00:00:00 2001 From: PX4BuildBot Date: Wed, 14 Aug 2019 12:36:25 +0000 Subject: [PATCH 10/10] Update PX4 Firmware metadata Wed Aug 14 12:36:25 UTC 2019 --- src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index 9b79852..795cd6c 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -714,9 +714,13 @@ Set -1 to disable the check Time-out for auto disarm after landing - A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be automatically disarmed in case a landing situation has been detected during this period. The vehicle will also auto-disarm right after arming if it has not even flown, however the time will always be 10 seconds such that the pilot has enough time to take off. A negative value means that automatic disarming triggered by landing detection is disabled. - -1 - 20 + A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be automatically disarmed in case a landing situation has been detected during this period. A zero or negative value means that automatic disarming triggered by landing detection is disabled. + s + 2 + + + Time-out for auto disarm if too slow to takeoff + A non-zero, positive value specifies the time after arming, in seconds, within which the vehicle must take off (after which it will automatically disarm). A zero or negative value means that automatic disarming triggered by a pre-takeoff timeout is disabled. s 2