Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
a9621d2b
Commit
a9621d2b
authored
Jul 18, 2017
by
Gus Grubba
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Handle Facts
Handle initial storage Handle image/video capture and storage.
parent
4362d4e5
Changes
8
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
8 changed files
with
445 additions
and
116 deletions
+445
-116
QGCCameraControl.cc
src/Camera/QGCCameraControl.cc
+292
-46
QGCCameraControl.h
src/Camera/QGCCameraControl.h
+75
-36
QGCCameraIO.cc
src/Camera/QGCCameraIO.cc
+13
-4
QGCCameraIO.h
src/Camera/QGCCameraIO.h
+1
-0
QGCCameraManager.cc
src/Camera/QGCCameraManager.cc
+48
-16
QGCCameraManager.h
src/Camera/QGCCameraManager.h
+9
-7
Fact.cc
src/FactSystem/Fact.cc
+3
-5
Fact.h
src/FactSystem/Fact.h
+4
-2
No files found.
src/Camera/QGCCameraControl.cc
View file @
a9621d2b
This diff is collapsed.
Click to expand it.
src/Camera/QGCCameraControl.h
View file @
a9621d2b
...
...
@@ -62,7 +62,7 @@ class QGCCameraControl : public FactGroup
Q_OBJECT
public:
QGCCameraControl
(
const
mavlink_camera_information_t
*
info
,
Vehicle
*
vehicle
,
int
compID
,
QObject
*
parent
=
NULL
);
~
QGCCameraControl
();
virtual
~
QGCCameraControl
();
//-- cam_mode
enum
CameraMode
{
...
...
@@ -71,7 +71,15 @@ public:
CAMERA_MODE_VIDEO
=
1
,
};
//-- Video Capture Status
enum
VideoStatus
{
VIDEO_CAPTURE_STATUS_STOPPED
=
0
,
VIDEO_CAPTURE_STATUS_RUNNING
,
VIDEO_CAPTURE_STATUS_UNDEFINED
};
Q_ENUMS
(
CameraMode
)
Q_ENUMS
(
VideoStatus
)
Q_PROPERTY
(
int
version
READ
version
NOTIFY
infoChanged
)
Q_PROPERTY
(
QString
modelName
READ
modelName
NOTIFY
infoChanged
)
...
...
@@ -86,57 +94,82 @@ public:
Q_PROPERTY
(
bool
photosInVideoMode
READ
photosInVideoMode
NOTIFY
infoChanged
)
Q_PROPERTY
(
bool
videoInPhotoMode
READ
videoInPhotoMode
NOTIFY
infoChanged
)
Q_PROPERTY
(
bool
isBasic
READ
isBasic
NOTIFY
infoChanged
)
Q_PROPERTY
(
quint32
storageFree
READ
storageFree
NOTIFY
storageFreeChanged
)
Q_PROPERTY
(
QString
storageFreeStr
READ
storageFreeStr
NOTIFY
storageFreeChanged
)
Q_PROPERTY
(
quint32
storageTotal
READ
storageTotal
NOTIFY
storageTotalChanged
)
Q_PROPERTY
(
QStringList
activeSettings
READ
activeSettings
NOTIFY
activeSettingsChanged
)
Q_PROPERTY
(
VideoStatus
videoStatus
READ
videoStatus
NOTIFY
videoStatusChanged
)
Q_PROPERTY
(
CameraMode
cameraMode
READ
cameraMode
WRITE
setCameraMode
NOTIFY
cameraModeChanged
)
Q_INVOKABLE
void
setVideoMode
();
Q_INVOKABLE
void
setPhotoMode
();
Q_INVOKABLE
void
toggleMode
();
Q_INVOKABLE
void
takePhoto
();
Q_INVOKABLE
void
startVideo
();
Q_INVOKABLE
void
stopVideo
();
Q_INVOKABLE
void
resetSettings
();
Q_INVOKABLE
void
formatCard
(
int
id
=
1
);
int
version
()
{
return
_version
;
}
QString
modelName
()
{
return
_modelName
;
}
QString
vendor
()
{
return
_vendor
;
}
QString
firmwareVersion
();
qreal
focalLength
()
{
return
(
qreal
)
_info
.
focal_length
;
}
QSizeF
sensorSize
()
{
return
QSizeF
(
_info
.
sensor_size_h
,
_info
.
sensor_size_v
);
}
QSize
resolution
()
{
return
QSize
(
_info
.
resolution_h
,
_info
.
resolution_v
);
}
bool
capturesVideo
()
{
return
true
/*_info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO*/
;
}
bool
capturesPhotos
()
{
return
true
/*_info.flags & CAMERA_CAP_FLAGS_CAPTURE_PHOTO*/
;
}
bool
hasModes
()
{
return
true
/*_info.flags & CAMERA_CAP_FLAGS_HAS_MODES*/
;
}
bool
photosInVideoMode
()
{
return
true
/*_info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_PHOTO_IN_VIDEO_MODE*/
;
}
bool
videoInPhotoMode
()
{
return
false
/*_info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_PHOTO_MODE*/
;
}
int
compID
()
{
return
_compID
;
}
bool
isBasic
()
{
return
_settings
.
size
()
==
0
;
}
CameraMode
cameraMode
()
{
return
_cameraMode
;
}
QStringList
activeSettings
()
{
return
_activeSettings
;
}
void
setCameraMode
(
CameraMode
mode
);
void
handleSettings
(
const
mavlink_camera_settings_t
&
settings
);
void
handleParamAck
(
const
mavlink_param_ext_ack_t
&
ack
);
void
handleParamValue
(
const
mavlink_param_ext_value_t
&
value
);
void
factChanged
(
Fact
*
pFact
);
Q_INVOKABLE
virtual
void
setVideoMode
();
Q_INVOKABLE
virtual
void
setPhotoMode
();
Q_INVOKABLE
virtual
void
toggleMode
();
Q_INVOKABLE
virtual
bool
takePhoto
();
Q_INVOKABLE
virtual
bool
startVideo
();
Q_INVOKABLE
virtual
bool
stopVideo
();
Q_INVOKABLE
virtual
bool
toggleVideo
();
Q_INVOKABLE
virtual
void
resetSettings
();
Q_INVOKABLE
virtual
void
formatCard
(
int
id
=
1
);
virtual
int
version
()
{
return
_version
;
}
virtual
QString
modelName
()
{
return
_modelName
;
}
virtual
QString
vendor
()
{
return
_vendor
;
}
virtual
QString
firmwareVersion
();
virtual
qreal
focalLength
()
{
return
(
qreal
)
_info
.
focal_length
;
}
virtual
QSizeF
sensorSize
()
{
return
QSizeF
(
_info
.
sensor_size_h
,
_info
.
sensor_size_v
);
}
virtual
QSize
resolution
()
{
return
QSize
(
_info
.
resolution_h
,
_info
.
resolution_v
);
}
virtual
bool
capturesVideo
()
{
return
_info
.
flags
&
CAMERA_CAP_FLAGS_CAPTURE_VIDEO
;
}
virtual
bool
capturesPhotos
()
{
return
_info
.
flags
&
CAMERA_CAP_FLAGS_CAPTURE_IMAGE
;
}
virtual
bool
hasModes
()
{
return
_info
.
flags
&
CAMERA_CAP_FLAGS_HAS_MODES
;
}
virtual
bool
photosInVideoMode
()
{
return
_info
.
flags
&
CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE
;
}
virtual
bool
videoInPhotoMode
()
{
return
_info
.
flags
&
CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE
;
}
virtual
int
compID
()
{
return
_compID
;
}
virtual
bool
isBasic
()
{
return
_settings
.
size
()
==
0
;
}
virtual
VideoStatus
videoStatus
();
virtual
CameraMode
cameraMode
()
{
return
_cameraMode
;
}
virtual
QStringList
activeSettings
()
{
return
_activeSettings
;
}
virtual
quint32
storageFree
()
{
return
_storageFree
;
}
virtual
QString
storageFreeStr
();
virtual
quint32
storageTotal
()
{
return
_storageTotal
;
}
virtual
void
setCameraMode
(
CameraMode
mode
);
virtual
void
handleSettings
(
const
mavlink_camera_settings_t
&
settings
);
virtual
void
handleCaptureStatus
(
const
mavlink_camera_capture_status_t
&
capStatus
);
virtual
void
handleParamAck
(
const
mavlink_param_ext_ack_t
&
ack
);
virtual
void
handleParamValue
(
const
mavlink_param_ext_value_t
&
value
);
virtual
void
handleStorageInfo
(
const
mavlink_storage_information_t
&
st
);
virtual
void
factChanged
(
Fact
*
pFact
);
signals:
void
infoChanged
();
void
videoStatusChanged
();
void
cameraModeChanged
();
void
activeSettingsChanged
();
void
storageFreeChanged
();
void
storageTotalChanged
();
void
dataReady
(
QByteArray
data
);
protected:
virtual
void
_setVideoStatus
(
VideoStatus
status
);
virtual
void
_setCameraMode
(
CameraMode
mode
);
private
slots
:
void
_initWhenReady
();
void
_requestCameraSettings
();
void
_requestAllParameters
();
void
_requestCaptureStatus
();
void
_requestStorageInfo
();
void
_downloadFinished
();
void
_mavCommandResult
(
int
vehicleId
,
int
component
,
int
command
,
int
result
,
bool
noReponseFromVehicle
);
void
_dataReady
(
QByteArray
data
);
private:
bool
_handleLocalization
(
QByteArray
&
bytes
);
bool
_replaceLocaleStrings
(
const
QDomNode
node
,
QByteArray
&
bytes
);
bool
_loadCameraDefinitionFile
(
const
QString
&
file
);
bool
_loadCameraDefinitionFile
(
QByteArray
&
bytes
);
bool
_loadConstants
(
const
QDomNodeList
nodeList
);
bool
_loadSettings
(
const
QDomNodeList
nodeList
);
void
_processRanges
();
...
...
@@ -146,20 +179,26 @@ private:
bool
_loadRanges
(
QDomNode
option
,
const
QString
factName
,
QString
paramValue
);
void
_updateActiveList
();
void
_updateRanges
(
Fact
*
pFact
);
void
_httpRequest
(
const
QString
&
url
);
QStringList
_loadExclusions
(
QDomNode
option
);
QString
_getParamName
(
const
char
*
param_id
);
pr
ivate
:
pr
otected
:
Vehicle
*
_vehicle
;
int
_compID
;
mavlink_camera_information_t
_info
;
int
_version
;
uint32_t
_storageFree
;
uint32_t
_storageTotal
;
QNetworkAccessManager
*
_netManager
;
QString
_modelName
;
QString
_vendor
;
CameraMode
_cameraMode
;
VideoStatus
_video_status
;
QStringList
_activeSettings
;
QStringList
_settings
;
QTimer
_captureStatusTimer
;
QList
<
QGCCameraOptionExclusion
*>
_valueExclusions
;
QList
<
QGCCameraOptionRange
*>
_optionRanges
;
QMap
<
QString
,
QStringList
>
_originalOptNames
;
...
...
src/Camera/QGCCameraIO.cc
View file @
a9621d2b
...
...
@@ -27,6 +27,7 @@ QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicl
connect
(
&
_paramWriteTimer
,
&
QTimer
::
timeout
,
this
,
&
QGCCameraParamIO
::
_paramWriteTimeout
);
connect
(
&
_paramRequestTimer
,
&
QTimer
::
timeout
,
this
,
&
QGCCameraParamIO
::
_paramRequestTimeout
);
connect
(
_fact
,
&
Fact
::
rawValueChanged
,
this
,
&
QGCCameraParamIO
::
_factChanged
);
connect
(
_fact
,
&
Fact
::
_containerRawValueChanged
,
this
,
&
QGCCameraParamIO
::
_containerRawValueChanged
);
}
//-----------------------------------------------------------------------------
...
...
@@ -43,12 +44,20 @@ void
QGCCameraParamIO
::
_factChanged
(
QVariant
value
)
{
Q_UNUSED
(
value
);
qCDebug
(
CameraIOLog
)
<<
"Fact"
<<
_fact
->
name
()
<<
"changed"
;
_sendParameter
();
qCDebug
(
CameraIOLog
)
<<
"UI Fact"
<<
_fact
->
name
()
<<
"changed"
;
//-- TODO: Do we really want to update the UI now or only when we receive the ACK?
_control
->
factChanged
(
_fact
);
}
//-----------------------------------------------------------------------------
void
QGCCameraParamIO
::
_containerRawValueChanged
(
const
QVariant
value
)
{
Q_UNUSED
(
value
);
qCDebug
(
CameraIOLog
)
<<
"Send Fact"
<<
_fact
->
name
()
<<
"changed"
;
_sendParameter
();
}
//-----------------------------------------------------------------------------
void
QGCCameraParamIO
::
_sendParameter
()
...
...
@@ -146,7 +155,7 @@ QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack)
{
_paramWriteTimer
.
stop
();
if
(
ack
.
param_result
==
PARAM_ACK_ACCEPTED
)
{
_fact
->
setRawValue
(
_valueFromMessage
(
ack
.
param_value
,
ack
.
param_type
),
true
);
_fact
->
_containerSetRawValue
(
_valueFromMessage
(
ack
.
param_value
,
ack
.
param_type
)
);
}
if
(
ack
.
param_result
==
PARAM_ACK_IN_PROGRESS
)
{
//-- Wait a bit longer for this one
...
...
@@ -166,7 +175,7 @@ void
QGCCameraParamIO
::
handleParamValue
(
const
mavlink_param_ext_value_t
&
value
)
{
_paramRequestTimer
.
stop
();
_fact
->
setRawValue
(
_valueFromMessage
(
value
.
param_value
,
value
.
param_type
),
true
);
_fact
->
_containerSetRawValue
(
_valueFromMessage
(
value
.
param_value
,
value
.
param_type
)
);
_paramRequestReceived
=
true
;
qCDebug
(
CameraIOLog
)
<<
QString
(
"handleParamValue() %1 %2"
).
arg
(
_fact
->
name
()).
arg
(
_fact
->
rawValueString
());
}
...
...
src/Camera/QGCCameraIO.h
View file @
a9621d2b
...
...
@@ -27,6 +27,7 @@ public:
private
slots
:
void
_factChanged
(
QVariant
value
);
void
_containerRawValueChanged
(
const
QVariant
value
);
void
_paramWriteTimeout
();
void
_paramRequestTimeout
();
...
...
src/Camera/QGCCameraManager.cc
View file @
a9621d2b
...
...
@@ -50,22 +50,30 @@ QGCCameraManager::_vehicleReady(bool ready)
void
QGCCameraManager
::
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
)
{
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_HEARTBEAT
:
_handleHeartbeat
(
message
);
break
;
case
MAVLINK_MSG_ID_CAMERA_INFORMATION
:
_handleCameraInfo
(
message
);
break
;
case
MAVLINK_MSG_ID_CAMERA_SETTINGS
:
_handleCameraSettings
(
message
);
break
;
case
MAVLINK_MSG_ID_PARAM_EXT_ACK
:
_handleParamAck
(
message
);
break
;
case
MAVLINK_MSG_ID_PARAM_EXT_VALUE
:
_handleParamValue
(
message
);
break
;
if
(
message
.
sysid
==
_vehicle
->
id
())
{
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS
:
_handleCaptureStatus
(
message
);
break
;
case
MAVLINK_MSG_ID_STORAGE_INFORMATION
:
_handleStorageInfo
(
message
);
break
;
case
MAVLINK_MSG_ID_HEARTBEAT
:
_handleHeartbeat
(
message
);
break
;
case
MAVLINK_MSG_ID_CAMERA_INFORMATION
:
_handleCameraInfo
(
message
);
break
;
case
MAVLINK_MSG_ID_CAMERA_SETTINGS
:
_handleCameraSettings
(
message
);
break
;
case
MAVLINK_MSG_ID_PARAM_EXT_ACK
:
_handleParamAck
(
message
);
break
;
case
MAVLINK_MSG_ID_PARAM_EXT_VALUE
:
_handleParamValue
(
message
);
break
;
}
}
}
...
...
@@ -120,6 +128,30 @@ QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message)
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager
::
_handleCaptureStatus
(
const
mavlink_message_t
&
message
)
{
QGCCameraControl
*
pCamera
=
_findCamera
(
message
.
compid
);
if
(
pCamera
)
{
mavlink_camera_capture_status_t
cap
;
mavlink_msg_camera_capture_status_decode
(
&
message
,
&
cap
);
pCamera
->
handleCaptureStatus
(
cap
);
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager
::
_handleStorageInfo
(
const
mavlink_message_t
&
message
)
{
QGCCameraControl
*
pCamera
=
_findCamera
(
message
.
compid
);
if
(
pCamera
)
{
mavlink_storage_information_t
st
;
mavlink_msg_storage_information_decode
(
&
message
,
&
st
);
pCamera
->
handleStorageInfo
(
st
);
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager
::
_handleCameraSettings
(
const
mavlink_message_t
&
message
)
...
...
src/Camera/QGCCameraManager.h
View file @
a9621d2b
...
...
@@ -23,7 +23,7 @@ class QGCCameraManager : public QObject
Q_OBJECT
public:
QGCCameraManager
(
Vehicle
*
vehicle
);
~
QGCCameraManager
();
virtual
~
QGCCameraManager
();
Q_PROPERTY
(
QmlObjectListModel
*
cameras
READ
cameras
NOTIFY
camerasChanged
)
Q_PROPERTY
(
QString
controllerSource
READ
controllerSource
NOTIFY
controllerSourceChanged
)
...
...
@@ -33,24 +33,26 @@ public:
//-- Camera controller source (QML)
virtual
QString
controllerSource
();
private
slots
:
void
_vehicleReady
(
bool
ready
);
void
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
);
signals:
void
camerasChanged
();
void
controllerSourceChanged
();
private:
protected
slots
:
void
_vehicleReady
(
bool
ready
);
void
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
);
protected:
QGCCameraControl
*
_findCamera
(
int
id
);
void
_requestCameraInfo
(
int
compID
);
void
_handleHeartbeat
(
const
mavlink_message_t
&
message
);
void
_handleCameraInfo
(
const
mavlink_message_t
&
message
);
void
_handleStorageInfo
(
const
mavlink_message_t
&
message
);
void
_handleCameraSettings
(
const
mavlink_message_t
&
message
);
void
_handleParamAck
(
const
mavlink_message_t
&
message
);
void
_handleParamValue
(
const
mavlink_message_t
&
message
);
void
_handleCaptureStatus
(
const
mavlink_message_t
&
message
);
pr
ivate
:
pr
otected
:
Vehicle
*
_vehicle
;
bool
_vehicleReadyState
;
int
_cameraCount
;
...
...
src/FactSystem/Fact.cc
View file @
a9621d2b
...
...
@@ -92,7 +92,7 @@ void Fact::forceSetRawValue(const QVariant& value)
}
}
void
Fact
::
setRawValue
(
const
QVariant
&
value
,
bool
skipSignal
)
void
Fact
::
setRawValue
(
const
QVariant
&
value
)
{
if
(
_metaData
)
{
QVariant
typedValue
;
...
...
@@ -102,10 +102,8 @@ void Fact::setRawValue(const QVariant& value, bool skipSignal)
if
(
typedValue
!=
_rawValue
)
{
_rawValue
.
setValue
(
typedValue
);
_sendValueChangedSignal
(
cookedValue
());
if
(
!
skipSignal
)
{
emit
_containerRawValueChanged
(
rawValue
());
emit
rawValueChanged
(
_rawValue
);
}
emit
_containerRawValueChanged
(
rawValue
());
emit
rawValueChanged
(
_rawValue
);
}
}
}
else
{
...
...
src/FactSystem/Fact.h
View file @
a9621d2b
...
...
@@ -112,7 +112,7 @@ public:
/// Returns the values as a string with full 18 digit precision if float/double.
QString
rawValueStringFullPrecision
(
void
)
const
;
void
setRawValue
(
const
QVariant
&
value
,
bool
skipSignal
=
false
);
void
setRawValue
(
const
QVariant
&
value
);
void
setCookedValue
(
const
QVariant
&
value
);
void
setEnumIndex
(
int
index
);
void
setEnumStringValue
(
const
QString
&
value
);
...
...
@@ -135,6 +135,8 @@ public:
void
setMetaData
(
FactMetaData
*
metaData
);
FactMetaData
*
metaData
()
{
return
_metaData
;
}
//-- Value coming from Vehicle. This does NOT send a _containerRawValueChanged signal.
void
_containerSetRawValue
(
const
QVariant
&
value
);
/// Generally you should not change the name of a fact. But if you know what you are doing, you can.
...
...
@@ -161,7 +163,7 @@ signals:
/// Signalled when property has been changed by a call to the property write accessor
///
/// This signal is meant for use by Fact container implementations.
/// This signal is meant for use by Fact container implementations.
Used to send changed values to vehicle.
void
_containerRawValueChanged
(
const
QVariant
&
value
);
protected:
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment