Commit 50e585c9 authored by Gus Grubba's avatar Gus Grubba

Merge branch 'master' of https://github.com/mavlink/qgroundcontrol into objectAvoidance

parents edf814c1 f0994874
...@@ -58,11 +58,13 @@ QML_IMPORT_PATH += \ ...@@ -58,11 +58,13 @@ QML_IMPORT_PATH += \
# Our own, custom sources # Our own, custom sources
SOURCES += \ SOURCES += \
$$PWD/src/CustomPlugin.cc \ $$PWD/src/CustomPlugin.cc \
$$PWD/src/CustomQuickInterface.cc $$PWD/src/CustomQuickInterface.cc \
$$PWD/src/CustomVideoManager.cc
HEADERS += \ HEADERS += \
$$PWD/src/CustomPlugin.h \ $$PWD/src/CustomPlugin.h \
$$PWD/src/CustomQuickInterface.h $$PWD/src/CustomQuickInterface.h \
$$PWD/src/CustomVideoManager.h
INCLUDEPATH += \ INCLUDEPATH += \
$$PWD/src \ $$PWD/src \
......
...@@ -17,6 +17,7 @@ ...@@ -17,6 +17,7 @@
#include "CustomPlugin.h" #include "CustomPlugin.h"
#include "CustomQuickInterface.h" #include "CustomQuickInterface.h"
#include "CustomVideoManager.h"
#include "MultiVehicleManager.h" #include "MultiVehicleManager.h"
#include "QGCApplication.h" #include "QGCApplication.h"
...@@ -191,6 +192,13 @@ CustomPlugin::overrideSettingsGroupVisibility(QString name) ...@@ -191,6 +192,13 @@ CustomPlugin::overrideSettingsGroupVisibility(QString name)
return true; return true;
} }
//-----------------------------------------------------------------------------
VideoManager*
CustomPlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox)
{
return new CustomVideoManager(app, toolbox);
}
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
VideoReceiver* VideoReceiver*
CustomPlugin::createVideoReceiver(QObject* parent) CustomPlugin::createVideoReceiver(QObject* parent)
......
...@@ -79,6 +79,7 @@ public: ...@@ -79,6 +79,7 @@ public:
QString brandImageIndoor () const final; QString brandImageIndoor () const final;
QString brandImageOutdoor () const final; QString brandImageOutdoor () const final;
bool overrideSettingsGroupVisibility (QString name) final; bool overrideSettingsGroupVisibility (QString name) final;
VideoManager* createVideoManager (QGCApplication* app, QGCToolbox* toolbox) final;
VideoReceiver* createVideoReceiver (QObject* parent) final; VideoReceiver* createVideoReceiver (QObject* parent) final;
QQmlApplicationEngine* createRootWindow (QObject* parent) final; QQmlApplicationEngine* createRootWindow (QObject* parent) final;
bool adjustSettingMetaData (const QString& settingsGroup, FactMetaData& metaData) final; bool adjustSettingMetaData (const QString& settingsGroup, FactMetaData& metaData) final;
......
/****************************************************************************
*
* (c) 2009-2019 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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<CustomCameraControl*>(_activeVehicle->dynamicCameras()->currentCameraInstance());
if(pCamera) {
Fact *fact = pCamera->videoEncoding();
if (fact) {
_videoReceiver->setVideoDecoder(static_cast<VideoReceiver::VideoEncoding>(fact->rawValue().toInt()));
}
}
}
VideoManager::_updateSettings();
}
/****************************************************************************
*
* (c) 2009-2019 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include <QObject>
#include <QTimer>
#include <QTime>
#include <QUrl>
#include "VideoManager.h"
class CustomVideoManager : public VideoManager
{
Q_OBJECT
public:
CustomVideoManager (QGCApplication* app, QGCToolbox* toolbox);
protected:
void _updateSettings ();
};
...@@ -19,6 +19,7 @@ QGC_LOGGING_CATEGORY(CustomCameraVerboseLog, "CustomCameraVerboseLog") ...@@ -19,6 +19,7 @@ QGC_LOGGING_CATEGORY(CustomCameraVerboseLog, "CustomCameraVerboseLog")
static const char* kCAM_IRPALETTE = "CAM_IRPALETTE"; static const char* kCAM_IRPALETTE = "CAM_IRPALETTE";
static const char* kCAM_NEXTVISION_IRPALETTE = "IR_SENS_POL"; 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) CustomCameraControl::CustomCameraControl(const mavlink_camera_information_t *info, Vehicle* vehicle, int compID, QObject* parent)
...@@ -116,6 +117,13 @@ CustomCameraControl::irPalette() ...@@ -116,6 +117,13 @@ CustomCameraControl::irPalette()
return nullptr; return nullptr;
} }
//-----------------------------------------------------------------------------
Fact*
CustomCameraControl::videoEncoding()
{
return _paramComplete ? getFact(kCAM_ENC) : nullptr;
}
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void void
CustomCameraControl::setThermalMode(ThermalViewMode mode) CustomCameraControl::setThermalMode(ThermalViewMode mode)
......
...@@ -29,8 +29,11 @@ public: ...@@ -29,8 +29,11 @@ public:
CustomCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = nullptr); 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* irPalette READ irPalette NOTIFY parametersReady)
Q_PROPERTY(Fact* videoEncoding READ videoEncoding NOTIFY parametersReady)
Fact* irPalette (); Fact* irPalette ();
Fact* videoEncoding ();
bool takePhoto () override; bool takePhoto () override;
bool stopTakePhoto () override; bool stopTakePhoto () override;
bool startVideo () override; bool startVideo () override;
......
...@@ -70,9 +70,6 @@ const char* QGCCameraControl::kCAM_SHUTTERSPD = "CAM_SHUTTERSPD"; ...@@ -70,9 +70,6 @@ const char* QGCCameraControl::kCAM_SHUTTERSPD = "CAM_SHUTTERSPD";
const char* QGCCameraControl::kCAM_APERTURE = "CAM_APERTURE"; const char* QGCCameraControl::kCAM_APERTURE = "CAM_APERTURE";
const char* QGCCameraControl::kCAM_WBMODE = "CAM_WBMODE"; const char* QGCCameraControl::kCAM_WBMODE = "CAM_WBMODE";
const char* QGCCameraControl::kCAM_MODE = "CAM_MODE"; 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_) QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_)
...@@ -2116,27 +2113,6 @@ QGCCameraControl::mode() ...@@ -2116,27 +2113,6 @@ QGCCameraControl::mode()
return _paramComplete ? getFact(kCAM_MODE) : nullptr; return _paramComplete ? getFact(kCAM_MODE) : nullptr;
} }
//-----------------------------------------------------------------------------
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) QGCVideoStreamInfo::QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t *si)
: QObject(parent) : QObject(parent)
......
...@@ -167,9 +167,6 @@ public: ...@@ -167,9 +167,6 @@ public:
Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady) Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady)
Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady) Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady)
Q_PROPERTY(Fact* mode READ mode 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(QStringList activeSettings READ activeSettings NOTIFY activeSettingsChanged)
Q_PROPERTY(VideoStatus videoStatus READ videoStatus NOTIFY videoStatusChanged) Q_PROPERTY(VideoStatus videoStatus READ videoStatus NOTIFY videoStatusChanged)
...@@ -255,9 +252,6 @@ public: ...@@ -255,9 +252,6 @@ public:
virtual Fact* aperture (); virtual Fact* aperture ();
virtual Fact* wb (); virtual Fact* wb ();
virtual Fact* mode (); virtual Fact* mode ();
virtual Fact* bitRate ();
virtual Fact* frameRate ();
virtual Fact* videoEncoding ();
//-- Stream names to show the user (for selection) //-- Stream names to show the user (for selection)
virtual QStringList streamLabels () { return _streamLabels; } virtual QStringList streamLabels () { return _streamLabels; }
...@@ -289,7 +283,6 @@ public: ...@@ -289,7 +283,6 @@ public:
//-- Allow controller to modify or invalidate parameter change //-- Allow controller to modify or invalidate parameter change
virtual bool validateParameter (Fact* pFact, QVariant& newValue); virtual bool validateParameter (Fact* pFact, QVariant& newValue);
// Known Parameters // Known Parameters
static const char* kCAM_EV; static const char* kCAM_EV;
static const char* kCAM_EXPMODE; static const char* kCAM_EXPMODE;
...@@ -298,9 +291,6 @@ public: ...@@ -298,9 +291,6 @@ public:
static const char* kCAM_APERTURE; static const char* kCAM_APERTURE;
static const char* kCAM_WBMODE; static const char* kCAM_WBMODE;
static const char* kCAM_MODE; static const char* kCAM_MODE;
static const char* kCAM_BITRATE;
static const char* kCAM_FPS;
static const char* kCAM_ENC;
signals: signals:
void infoChanged (); void infoChanged ();
......
...@@ -23,50 +23,56 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : ...@@ -23,50 +23,56 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
APMCustomMode(mode, settable) APMCustomMode(mode, settable)
{ {
setEnumToStringMapping({ setEnumToStringMapping({
{STABILIZE, "Stabilize"}, { STABILIZE, "Stabilize"},
{ACRO, "Acro"}, { ACRO, "Acro"},
{ALT_HOLD, "Altitude Hold"}, { ALT_HOLD, "Altitude Hold"},
{AUTO, "Auto"}, { AUTO, "Auto"},
{GUIDED, "Guided"}, { GUIDED, "Guided"},
{LOITER, "Loiter"}, { LOITER, "Loiter"},
{RTL, "RTL"}, { RTL, "RTL"},
{CIRCLE, "Circle"}, { CIRCLE, "Circle"},
{LAND, "Land"}, { LAND, "Land"},
{DRIFT, "Drift"}, { DRIFT, "Drift"},
{SPORT, "Sport"}, { SPORT, "Sport"},
{FLIP, "Flip"}, { FLIP, "Flip"},
{AUTOTUNE, "Autotune"}, { AUTOTUNE, "Autotune"},
{POS_HOLD, "Position Hold"}, { POS_HOLD, "Position Hold"},
{BRAKE, "Brake"}, { BRAKE, "Brake"},
{THROW, "Throw"}, { THROW, "Throw"},
{AVOID_ADSB, "Avoid ADSB"}, { AVOID_ADSB, "Avoid ADSB"},
{GUIDED_NOGPS, "Guided No GPS"}, { GUIDED_NOGPS, "Guided No GPS"},
{SAFE_RTL, "Smart RTL"}, { SMART_RTL, "Smart RTL"},
{ FLOWHOLD, "Flow Hold" },
{ FOLLOW, "Follow Vehicle" },
{ ZIGZAG, "ZigZag" },
}); });
} }
ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void) ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
{ {
setSupportedModes({ setSupportedModes({
APMCopterMode(APMCopterMode::STABILIZE ,true), APMCopterMode(APMCopterMode::STABILIZE, true),
APMCopterMode(APMCopterMode::ACRO ,true), APMCopterMode(APMCopterMode::ACRO, true),
APMCopterMode(APMCopterMode::ALT_HOLD ,true), APMCopterMode(APMCopterMode::ALT_HOLD, true),
APMCopterMode(APMCopterMode::AUTO ,true), APMCopterMode(APMCopterMode::AUTO, true),
APMCopterMode(APMCopterMode::GUIDED ,true), APMCopterMode(APMCopterMode::GUIDED, true),
APMCopterMode(APMCopterMode::LOITER ,true), APMCopterMode(APMCopterMode::LOITER, true),
APMCopterMode(APMCopterMode::RTL ,true), APMCopterMode(APMCopterMode::RTL, true),
APMCopterMode(APMCopterMode::CIRCLE ,true), APMCopterMode(APMCopterMode::CIRCLE, true),
APMCopterMode(APMCopterMode::LAND ,true), APMCopterMode(APMCopterMode::LAND, true),
APMCopterMode(APMCopterMode::DRIFT ,true), APMCopterMode(APMCopterMode::DRIFT, true),
APMCopterMode(APMCopterMode::SPORT ,true), APMCopterMode(APMCopterMode::SPORT, true),
APMCopterMode(APMCopterMode::FLIP ,true), APMCopterMode(APMCopterMode::FLIP, true),
APMCopterMode(APMCopterMode::AUTOTUNE ,true), APMCopterMode(APMCopterMode::AUTOTUNE, true),
APMCopterMode(APMCopterMode::POS_HOLD ,true), APMCopterMode(APMCopterMode::POS_HOLD, true),
APMCopterMode(APMCopterMode::BRAKE ,true), APMCopterMode(APMCopterMode::BRAKE, true),
APMCopterMode(APMCopterMode::THROW ,true), APMCopterMode(APMCopterMode::THROW, true),
APMCopterMode(APMCopterMode::AVOID_ADSB,true), APMCopterMode(APMCopterMode::AVOID_ADSB, true),
APMCopterMode(APMCopterMode::GUIDED_NOGPS,true), APMCopterMode(APMCopterMode::GUIDED_NOGPS, true),
APMCopterMode(APMCopterMode::SAFE_RTL,true), APMCopterMode(APMCopterMode::SMART_RTL, true),
APMCopterMode(APMCopterMode::FLOWHOLD, true),
APMCopterMode(APMCopterMode::FOLLOW, true),
APMCopterMode(APMCopterMode::ZIGZAG, true),
}); });
if (!_remapParamNameIntialized) { if (!_remapParamNameIntialized) {
......
...@@ -41,9 +41,11 @@ public: ...@@ -41,9 +41,11 @@ public:
THROW = 18, THROW = 18,
AVOID_ADSB = 19, AVOID_ADSB = 19,
GUIDED_NOGPS= 20, 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); APMCopterMode(uint32_t mode, bool settable);
}; };
......
...@@ -42,7 +42,6 @@ public: ...@@ -42,7 +42,6 @@ public:
QLOITER = 19, QLOITER = 19,
QLAND = 20, QLAND = 20,
QRTL = 21, QRTL = 21,
modeCount
}; };
APMPlaneMode(uint32_t mode, bool settable); APMPlaneMode(uint32_t mode, bool settable);
......
...@@ -33,7 +33,6 @@ public: ...@@ -33,7 +33,6 @@ public:
GUIDED = 15, GUIDED = 15,
INITIALIZING = 16, INITIALIZING = 16,
}; };
static const int modeCount = 17;
APMRoverMode(uint32_t mode, bool settable); APMRoverMode(uint32_t mode, bool settable);
}; };
......
...@@ -96,7 +96,6 @@ public: ...@@ -96,7 +96,6 @@ public:
RESERVED_18 = 18, RESERVED_18 = 18,
MANUAL = 19 MANUAL = 19
}; };
static const int modeCount = 20;
APMSubMode(uint32_t mode, bool settable); APMSubMode(uint32_t mode, bool settable);
}; };
......
...@@ -137,6 +137,61 @@ ...@@ -137,6 +137,61 @@
<min>0</min> <min>0</min>
</parameter> </parameter>
</group> </group>
<group name="Airspeed Validator">
<parameter default="1.0" name="ARSP_ARSP_SCALE" type="FLOAT">
<short_desc>Airspeed scale (scale from IAS to CAS/EAS)</short_desc>
<long_desc>Scale can either be entered manually, or estimated in-flight by setting ARSP_SCALE_EST to 1.</long_desc>
<min>0.5</min>
<max>1.5</max>
</parameter>
<parameter default="1" name="ARSP_BETA_GATE" type="INT32">
<short_desc>Airspeed Selector: Gate size for true sideslip fusion</short_desc>
<long_desc>Sets the number of standard deviations used by the innovation consistency test.</long_desc>
<min>1</min>
<max>5</max>
<unit>SD</unit>
</parameter>
<parameter default="0.3" name="ARSP_BETA_NOISE" type="FLOAT">
<short_desc>Airspeed Selector: Wind estimator sideslip measurement noise</short_desc>
<long_desc>Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector.</long_desc>
<min>0</min>
<max>1</max>
<unit>rad</unit>
</parameter>
<parameter default="0" name="ARSP_SCALE_EST" type="INT32">
<short_desc>Automatic airspeed scale estimation on</short_desc>
<long_desc>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.</long_desc>
<boolean />
</parameter>
<parameter default="0.0001" name="ARSP_SC_P_NOISE" type="FLOAT">
<short_desc>Airspeed Selector: Wind estimator true airspeed scale process noise</short_desc>
<long_desc>Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.</long_desc>
<min>0</min>
<max>0.1</max>
<unit>1/s</unit>
</parameter>
<parameter default="3" name="ARSP_TAS_GATE" type="INT32">
<short_desc>Airspeed Selector: Gate size for true airspeed fusion</short_desc>
<long_desc>Sets the number of standard deviations used by the innovation consistency test.</long_desc>
<min>1</min>
<max>5</max>
<unit>SD</unit>
</parameter>
<parameter default="1.4" name="ARSP_TAS_NOISE" type="FLOAT">
<short_desc>Airspeed Selector: Wind estimator true airspeed measurement noise</short_desc>
<long_desc>True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector.</long_desc>
<min>0</min>
<max>4</max>
<unit>m/s</unit>
</parameter>
<parameter default="0.1" name="ARSP_W_P_NOISE" type="FLOAT">
<short_desc>Airspeed Selector: Wind estimator wind process noise</short_desc>
<long_desc>Wind process noise of the internal wind estimator(s) of the airspeed selector.</long_desc>
<min>0</min>
<max>1</max>
<unit>m/s/s</unit>
</parameter>
</group>
<group name="Attitude Q estimator"> <group name="Attitude Q estimator">
<parameter default="1" name="ATT_ACC_COMP" type="INT32"> <parameter default="1" name="ATT_ACC_COMP" type="INT32">
<short_desc>Acceleration compensation based on GPS <short_desc>Acceleration compensation based on GPS
...@@ -537,6 +592,11 @@ Set to 2 to use heading from motion capture</short_desc> ...@@ -537,6 +592,11 @@ Set to 2 to use heading from motion capture</short_desc>
* the MSB bit is not used to avoid problems in the conversion between int and uint</short_desc> * the MSB bit is not used to avoid problems in the conversion between int and uint</short_desc>
<long_desc>Default value: (10 &lt;&lt; 0 | 1000 &lt;&lt; 8 | 0 &lt;&lt; 24) = 256010 - authorizer system id = 10 - authentication method parameter = 10000msec of timeout - authentication method = during arm</long_desc> <long_desc>Default value: (10 &lt;&lt; 0 | 1000 &lt;&lt; 8 | 0 &lt;&lt; 24) = 256010 - authorizer system id = 10 - authentication method parameter = 10000msec of timeout - authentication method = during arm</long_desc>
</parameter> </parameter>
<parameter default="1" name="COM_ARM_CHK_ESCS" type="INT32">
<short_desc>Require all the ESCs to be detected to arm</short_desc>
<long_desc>This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param.</long_desc>
<boolean />
</parameter>
<parameter default="1.73e-3" name="COM_ARM_EKF_AB" type="FLOAT"> <parameter default="1.73e-3" name="COM_ARM_EKF_AB" type="FLOAT">
<short_desc>Maximum value of EKF accelerometer delta velocity bias estimate that will allow arming. <short_desc>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</short_desc> 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</short_desc>
...@@ -654,9 +714,13 @@ Set -1 to disable the check</short_desc> ...@@ -654,9 +714,13 @@ Set -1 to disable the check</short_desc>
</parameter> </parameter>
<parameter default="2.0" name="COM_DISARM_LAND" type="FLOAT"> <parameter default="2.0" name="COM_DISARM_LAND" type="FLOAT">
<short_desc>Time-out for auto disarm after landing</short_desc> <short_desc>Time-out for auto disarm after landing</short_desc>
<long_desc>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.</long_desc> <long_desc>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.</long_desc>
<min>-1</min> <unit>s</unit>
<max>20</max> <decimal>2</decimal>
</parameter>
<parameter default="10.0" name="COM_DISARM_PRFLT" type="FLOAT">
<short_desc>Time-out for auto disarm if too slow to takeoff</short_desc>
<long_desc>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.</long_desc>
<unit>s</unit> <unit>s</unit>
<decimal>2</decimal> <decimal>2</decimal>
</parameter> </parameter>
...@@ -4705,10 +4769,10 @@ the setpoint will be capped to MPC_XY_VEL_MAX</short_desc> ...@@ -4705,10 +4769,10 @@ the setpoint will be capped to MPC_XY_VEL_MAX</short_desc>
<max>2.0</max> <max>2.0</max>
<decimal>2</decimal> <decimal>2</decimal>
</parameter> </parameter>
<parameter default="0.3" name="MPC_XY_TRAJ_P" type="FLOAT"> <parameter default="0.5" name="MPC_XY_TRAJ_P" type="FLOAT">
<short_desc>Proportional gain for horizontal trajectory position error</short_desc> <short_desc>Proportional gain for horizontal trajectory position error</short_desc>
<min>0.1</min> <min>0.1</min>
<max>5.0</max> <max>1.0</max>
<decimal>1</decimal> <decimal>1</decimal>
</parameter> </parameter>
<parameter default="0.01" name="MPC_XY_VEL_D" type="FLOAT"> <parameter default="0.01" name="MPC_XY_VEL_D" type="FLOAT">
...@@ -4762,7 +4826,7 @@ the setpoint will be capped to MPC_XY_VEL_MAX</short_desc> ...@@ -4762,7 +4826,7 @@ the setpoint will be capped to MPC_XY_VEL_MAX</short_desc>
<parameter default="0.3" name="MPC_Z_TRAJ_P" type="FLOAT"> <parameter default="0.3" name="MPC_Z_TRAJ_P" type="FLOAT">
<short_desc>Proportional gain for vertical trajectory position error</short_desc> <short_desc>Proportional gain for vertical trajectory position error</short_desc>
<min>0.1</min> <min>0.1</min>
<max>5.0</max> <max>1.0</max>
<decimal>1</decimal> <decimal>1</decimal>
</parameter> </parameter>
<parameter default="0.0" name="MPC_Z_VEL_D" type="FLOAT"> <parameter default="0.0" name="MPC_Z_VEL_D" type="FLOAT">
...@@ -10487,50 +10551,6 @@ to fixed wing mode. Zero or negative values will produce an instant throttle ris ...@@ -10487,50 +10551,6 @@ to fixed wing mode. Zero or negative values will produce an instant throttle ris
<increment>0.01</increment> <increment>0.01</increment>
</parameter> </parameter>
</group> </group>
<group name="Wind Estimator">
<parameter default="1" name="WEST_BETA_GATE" type="INT32">
<short_desc>Gate size for true sideslip fusion</short_desc>
<long_desc>Sets the number of standard deviations used by the innovation consistency test.</long_desc>
<min>1</min>
<max>5</max>
<unit>SD</unit>
</parameter>
<parameter default="0.3" name="WEST_BETA_NOISE" type="FLOAT">
<short_desc>Wind estimator sideslip measurement noise</short_desc>
<min>0</min>
<max>1</max>
<unit>rad</unit>
</parameter>
<parameter default="0" name="WEST_EN" type="INT32">
<short_desc>Enable Wind estimator</short_desc>
<boolean />
<reboot_required>true</reboot_required>
</parameter>
<parameter default="0.0001" name="WEST_SC_P_NOISE" type="FLOAT">
<short_desc>Wind estimator true airspeed scale process noise</short_desc>
<min>0</min>
<max>0.1</max>
</parameter>
<parameter default="3" name="WEST_TAS_GATE" type="INT32">
<short_desc>Gate size for true airspeed fusion</short_desc>
<long_desc>Sets the number of standard deviations used by the innovation consistency test.</long_desc>
<min>1</min>
<max>5</max>
<unit>SD</unit>
</parameter>
<parameter default="1.4" name="WEST_TAS_NOISE" type="FLOAT">
<short_desc>Wind estimator true airspeed measurement noise</short_desc>
<min>0</min>
<max>4</max>
<unit>m/s</unit>
</parameter>
<parameter default="0.1" name="WEST_W_P_NOISE" type="FLOAT">
<short_desc>Wind estimator wind process noise</short_desc>
<min>0</min>
<max>1</max>
<unit>m/s/s</unit>
</parameter>
</group>
<group name="Miscellaneous"> <group name="Miscellaneous">
<parameter default="0.1" name="EXFW_HDNG_P" type="FLOAT"> <parameter default="0.1" name="EXFW_HDNG_P" type="FLOAT">
<short_desc>EXFW_HDNG_P</short_desc> <short_desc>EXFW_HDNG_P</short_desc>
......
...@@ -271,6 +271,20 @@ VideoManager::uvcEnabled() ...@@ -271,6 +271,20 @@ VideoManager::uvcEnabled()
} }
#endif #endif
//-----------------------------------------------------------------------------
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 void
VideoManager::_updateSettings() VideoManager::_updateSettings()
...@@ -279,13 +293,6 @@ VideoManager::_updateSettings() ...@@ -279,13 +293,6 @@ VideoManager::_updateSettings()
return; return;
//-- Auto discovery //-- Auto discovery
if(_activeVehicle && _activeVehicle->dynamicCameras()) { if(_activeVehicle && _activeVehicle->dynamicCameras()) {
QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance();
if(pCamera) {
Fact *fact = pCamera->videoEncoding();
if (fact) {
_videoReceiver->setVideoDecoder(static_cast<VideoReceiver::VideoEncoding>(fact->rawValue().toInt()));
}
}
QGCVideoStreamInfo* pInfo = _activeVehicle->dynamicCameras()->currentStreamInstance(); QGCVideoStreamInfo* pInfo = _activeVehicle->dynamicCameras()->currentStreamInstance();
if(pInfo) { if(pInfo) {
qCDebug(VideoManagerLog) << "Configure primary stream: " << pInfo->uri(); qCDebug(VideoManagerLog) << "Configure primary stream: " << pInfo->uri();
...@@ -364,6 +371,7 @@ void ...@@ -364,6 +371,7 @@ void
VideoManager::_setActiveVehicle(Vehicle* vehicle) VideoManager::_setActiveVehicle(Vehicle* vehicle)
{ {
if(_activeVehicle) { if(_activeVehicle) {
disconnect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged);
if(_activeVehicle->dynamicCameras()) { if(_activeVehicle->dynamicCameras()) {
QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance();
if(pCamera) { if(pCamera) {
...@@ -374,6 +382,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) ...@@ -374,6 +382,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle)
} }
_activeVehicle = vehicle; _activeVehicle = vehicle;
if(_activeVehicle) { if(_activeVehicle) {
connect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged);
if(_activeVehicle->dynamicCameras()) { if(_activeVehicle->dynamicCameras()) {
connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo); connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo);
QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance();
...@@ -381,11 +390,24 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) ...@@ -381,11 +390,24 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle)
pCamera->resumeStream(); pCamera->resumeStream();
} }
} }
} else {
//-- Disable full screen video if vehicle is gone
setfullScreen(false);
} }
emit autoStreamConfiguredChanged(); emit autoStreamConfiguredChanged();
restartVideo(); restartVideo();
} }
//----------------------------------------------------------------------------------------
void
VideoManager::_connectionLostChanged(bool connectionLost)
{
if(connectionLost) {
//-- Disable full screen video if connection is lost
setfullScreen(false);
}
}
//---------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------
void void
VideoManager::_aspectRatioChanged() VideoManager::_aspectRatioChanged()
......
...@@ -34,7 +34,7 @@ class VideoManager : public QGCTool ...@@ -34,7 +34,7 @@ class VideoManager : public QGCTool
public: public:
VideoManager (QGCApplication* app, QGCToolbox* toolbox); VideoManager (QGCApplication* app, QGCToolbox* toolbox);
~VideoManager (); virtual ~VideoManager ();
Q_PROPERTY(bool hasVideo READ hasVideo NOTIFY hasVideoChanged) Q_PROPERTY(bool hasVideo READ hasVideo NOTIFY hasVideoChanged)
Q_PROPERTY(bool isGStreamer READ isGStreamer NOTIFY isGStreamerChanged) Q_PROPERTY(bool isGStreamer READ isGStreamer NOTIFY isGStreamerChanged)
...@@ -51,34 +51,33 @@ public: ...@@ -51,34 +51,33 @@ public:
Q_PROPERTY(bool autoStreamConfigured READ autoStreamConfigured NOTIFY autoStreamConfiguredChanged) Q_PROPERTY(bool autoStreamConfigured READ autoStreamConfigured NOTIFY autoStreamConfiguredChanged)
Q_PROPERTY(bool hasThermal READ hasThermal NOTIFY aspectRatioChanged) Q_PROPERTY(bool hasThermal READ hasThermal NOTIFY aspectRatioChanged)
bool hasVideo (); virtual bool hasVideo ();
bool isGStreamer (); virtual bool isGStreamer ();
bool isAutoStream (); virtual bool isTaisync () { return _isTaisync; }
bool isTaisync () { return _isTaisync; } virtual bool fullScreen () { return _fullScreen; }
bool fullScreen () { return _fullScreen; } virtual QString videoSourceID () { return _videoSourceID; }
QString videoSourceID () { return _videoSourceID; } virtual double aspectRatio ();
double aspectRatio (); virtual double thermalAspectRatio ();
double thermalAspectRatio (); virtual double hfov ();
double hfov (); virtual double thermalHfov ();
double thermalHfov (); virtual bool autoStreamConfigured();
bool autoStreamConfigured(); virtual bool hasThermal ();
bool hasThermal (); virtual void restartVideo ();
void restartVideo ();
virtual VideoReceiver* videoReceiver () { return _videoReceiver; }
VideoReceiver* videoReceiver () { return _videoReceiver; } virtual VideoReceiver* thermalVideoReceiver () { return _thermalVideoReceiver; }
VideoReceiver* thermalVideoReceiver () { return _thermalVideoReceiver; }
#if defined(QGC_DISABLE_UVC) #if defined(QGC_DISABLE_UVC)
bool uvcEnabled () { return false; } virtual bool uvcEnabled () { return false; }
#else #else
bool uvcEnabled (); virtual bool uvcEnabled ();
#endif #endif
void setfullScreen (bool f) { _fullScreen = f; emit fullScreenChanged(); } virtual void setfullScreen (bool f);
void setIsTaisync (bool t) { _isTaisync = t; emit isTaisyncChanged(); } virtual void setIsTaisync (bool t) { _isTaisync = t; emit isTaisyncChanged(); }
// Override from QGCTool // Override from QGCTool
void setToolbox (QGCToolbox *toolbox); virtual void setToolbox (QGCToolbox *toolbox);
Q_INVOKABLE void startVideo (); Q_INVOKABLE void startVideo ();
Q_INVOKABLE void stopVideo (); Q_INVOKABLE void stopVideo ();
...@@ -93,7 +92,7 @@ signals: ...@@ -93,7 +92,7 @@ signals:
void aspectRatioChanged (); void aspectRatioChanged ();
void autoStreamConfiguredChanged(); void autoStreamConfiguredChanged();
private slots: protected slots:
void _videoSourceChanged (); void _videoSourceChanged ();
void _udpPortChanged (); void _udpPortChanged ();
void _rtspUrlChanged (); void _rtspUrlChanged ();
...@@ -101,11 +100,12 @@ private slots: ...@@ -101,11 +100,12 @@ private slots:
void _updateUVC (); void _updateUVC ();
void _setActiveVehicle (Vehicle* vehicle); void _setActiveVehicle (Vehicle* vehicle);
void _aspectRatioChanged (); void _aspectRatioChanged ();
void _connectionLostChanged (bool connectionLost);
private: protected:
void _updateSettings (); void _updateSettings ();
private: protected:
SubtitleWriter _subtitleWriter; SubtitleWriter _subtitleWriter;
bool _isTaisync = false; bool _isTaisync = false;
VideoReceiver* _videoReceiver = nullptr; VideoReceiver* _videoReceiver = nullptr;
......
...@@ -3046,7 +3046,7 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord) ...@@ -3046,7 +3046,7 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
if (!coordinate().isValid()) { if (!coordinate().isValid()) {
return; return;
} }
double maxDistance = 1000.0; double maxDistance = 10000.0;
if (coordinate().distanceTo(gotoCoord) > maxDistance) { 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())); qgcApp()->showMessage(QString("New location is too far. Must be less than %1 %2").arg(qRound(FactMetaData::metersToAppSettingsDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsDistanceUnitsString()));
return; return;
......
...@@ -611,14 +611,14 @@ VideoReceiver::setVideoDecoder(VideoEncoding encoding) ...@@ -611,14 +611,14 @@ VideoReceiver::setVideoDecoder(VideoEncoding encoding)
*/ */
if (encoding == H265_HW || encoding == H265_SW) { if (encoding == H265_HW || encoding == H265_SW) {
_depayName = "rtph265depay"; _depayName = "rtph265depay";
_parserName = "h265parse"; _parserName = "h265parse";
#if defined(__android__) #if defined(__android__)
_hwDecoderName = "amcviddec-omxgooglehevcdecoder"; _hwDecoderName = "amcviddec-omxgooglehevcdecoder";
#endif #endif
_swDecoderName = "avdec_h265"; _swDecoderName = "avdec_h265";
} else { } else {
_depayName = "rtph264depay"; _depayName = "rtph264depay";
_parserName = "h264parse"; _parserName = "h264parse";
#if defined(__android__) #if defined(__android__)
_hwDecoderName = "amcviddec-omxgoogleh264decoder"; _hwDecoderName = "amcviddec-omxgoogleh264decoder";
...@@ -630,6 +630,7 @@ VideoReceiver::setVideoDecoder(VideoEncoding encoding) ...@@ -630,6 +630,7 @@ VideoReceiver::setVideoDecoder(VideoEncoding encoding)
_hwDecoderName = nullptr; _hwDecoderName = nullptr;
} }
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
// When we finish our pipeline will look like this: // When we finish our pipeline will look like this:
// //
......
...@@ -15,6 +15,7 @@ ...@@ -15,6 +15,7 @@
#include "SettingsManager.h" #include "SettingsManager.h"
#include "AppMessages.h" #include "AppMessages.h"
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
#include "VideoManager.h"
#include "VideoReceiver.h" #include "VideoReceiver.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "QGCCameraManager.h" #include "QGCCameraManager.h"
...@@ -408,6 +409,11 @@ QmlObjectListModel* QGCCorePlugin::customMapItems() ...@@ -408,6 +409,11 @@ QmlObjectListModel* QGCCorePlugin::customMapItems()
return &_p->_emptyCustomMapItems; return &_p->_emptyCustomMapItems;
} }
VideoManager* QGCCorePlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox)
{
return new VideoManager(app, toolbox);
}
VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent) VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent)
{ {
return new VideoReceiver(parent); return new VideoReceiver(parent);
......
...@@ -104,6 +104,8 @@ public: ...@@ -104,6 +104,8 @@ public:
/// Allows the plugin to override the creation of the root (native) window. /// Allows the plugin to override the creation of the root (native) window.
virtual QQmlApplicationEngine* createRootWindow(QObject* parent); 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. /// Allows the plugin to override the creation of VideoReceiver.
virtual VideoReceiver* createVideoReceiver(QObject* parent); virtual VideoReceiver* createVideoReceiver(QObject* parent);
......
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