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
17313b9b
Commit
17313b9b
authored
Apr 14, 2020
by
DoinLakeFlyer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
parent
eaec5be5
Changes
20
Hide whitespace changes
Inline
Side-by-side
Showing
20 changed files
with
353 additions
and
90 deletions
+353
-90
qgroundcontrol.pro
qgroundcontrol.pro
+2
-0
qgroundcontrol.qrc
qgroundcontrol.qrc
+2
-0
QGCApplication.cc
src/QGCApplication.cc
+2
-0
ParameterEditor.qml
src/QmlControls/ParameterEditor.qml
+4
-3
ParameterEditorController.cc
src/QmlControls/ParameterEditorController.cc
+0
-7
ParameterEditorController.h
src/QmlControls/ParameterEditorController.h
+0
-1
ParameterEditorDialog.qml
src/QmlControls/ParameterEditorDialog.qml
+15
-6
qmldir
src/QmlControls/QGroundControl/Controls/qmldir
+1
-0
RCToParamDialog.FactMetaData.json
src/QmlControls/RCToParamDialog.FactMetaData.json
+31
-0
RCToParamDialog.qml
src/QmlControls/RCToParamDialog.qml
+104
-0
RCToParamDialogController.cc
src/QmlControls/RCToParamDialogController.cc
+54
-0
RCToParamDialogController.h
src/QmlControls/RCToParamDialogController.h
+59
-0
ScreenTools.qml
src/QmlControls/ScreenTools.qml
+3
-0
Vehicle.cc
src/Vehicle/Vehicle.cc
+48
-0
Vehicle.h
src/Vehicle/Vehicle.h
+7
-0
MockLink.cc
src/comm/MockLink.cc
+20
-0
MockLink.h
src/comm/MockLink.h
+1
-0
UAS.cc
src/uas/UAS.cc
+0
-62
UAS.h
src/uas/UAS.h
+0
-5
UASInterface.h
src/uas/UASInterface.h
+0
-6
No files found.
qgroundcontrol.pro
View file @
17313b9b
...
...
@@ -652,6 +652,7 @@ HEADERS += \
src
/
QmlControls
/
QmlObjectListModel
.
h
\
src
/
QmlControls
/
QGCGeoBoundingCube
.
h
\
src
/
QmlControls
/
RCChannelMonitorController
.
h
\
src
/
QmlControls
/
RCToParamDialogController
.
h
\
src
/
QmlControls
/
ScreenToolsController
.
h
\
src
/
QtLocationPlugin
/
QMLControl
/
QGCMapEngineManager
.
h
\
src
/
Settings
/
ADSBVehicleManagerSettings
.
h
\
...
...
@@ -857,6 +858,7 @@ SOURCES += \
src
/
QmlControls
/
QmlObjectListModel
.
cc
\
src
/
QmlControls
/
QGCGeoBoundingCube
.
cc
\
src
/
QmlControls
/
RCChannelMonitorController
.
cc
\
src
/
QmlControls
/
RCToParamDialogController
.
cc
\
src
/
QmlControls
/
ScreenToolsController
.
cc
\
src
/
QtLocationPlugin
/
QMLControl
/
QGCMapEngineManager
.
cc
\
src
/
Settings
/
ADSBVehicleManagerSettings
.
cc
\
...
...
qgroundcontrol.qrc
View file @
17313b9b
...
...
@@ -159,6 +159,7 @@
<file alias="QGroundControl/Controls/RallyPointItemEditor.qml">src/PlanView/RallyPointItemEditor.qml</file>
<file alias="QGroundControl/Controls/RallyPointMapVisuals.qml">src/PlanView/RallyPointMapVisuals.qml</file>
<file alias="QGroundControl/Controls/RCChannelMonitor.qml">src/QmlControls/RCChannelMonitor.qml</file>
<file alias="QGroundControl/Controls/RCToParamDialog.qml">src/QmlControls/RCToParamDialog.qml</file>
<file alias="QGroundControl/Controls/RoundButton.qml">src/QmlControls/RoundButton.qml</file>
<file alias="QGroundControl/Controls/SectionHeader.qml">src/QmlControls/SectionHeader.qml</file>
<file alias="QGroundControl/Controls/SetupPage.qml">src/AutoPilotPlugins/Common/SetupPage.qml</file>
...
...
@@ -278,6 +279,7 @@
<file alias="PlanView.SettingsGroup.json">src/Settings/PlanView.SettingsGroup.json</file>
<file alias="QGCMapCircle.Facts.json">src/MissionManager/QGCMapCircle.Facts.json</file>
<file alias="RallyPoint.FactMetaData.json">src/MissionManager/RallyPoint.FactMetaData.json</file>
<file alias="RCToParamDialog.FactMetaData.json">src/QmlControls/RCToParamDialog.FactMetaData.json</file>
<file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="SpeedSection.FactMetaData.json">src/MissionManager/SpeedSection.FactMetaData.json</file>
<file alias="StructureScan.SettingsGroup.json">src/MissionManager/StructureScan.SettingsGroup.json</file>
...
...
src/QGCApplication.cc
View file @
17313b9b
...
...
@@ -101,6 +101,7 @@
#include "VehicleObjectAvoidance.h"
#include "TrajectoryPoints.h"
#include "ValuesWidgetController.h"
#include "RCToParamDialogController.h"
#if defined(QGC_ENABLE_PAIRING)
#include "PairingManager.h"
...
...
@@ -540,6 +541,7 @@ void QGCApplication::_initCommon()
qmlRegisterType
<
LogDownloadController
>
(
kQGCControllers
,
1
,
0
,
"LogDownloadController"
);
qmlRegisterType
<
SyslinkComponentController
>
(
kQGCControllers
,
1
,
0
,
"SyslinkComponentController"
);
qmlRegisterType
<
EditPositionDialogController
>
(
kQGCControllers
,
1
,
0
,
"EditPositionDialogController"
);
qmlRegisterType
<
RCToParamDialogController
>
(
kQGCControllers
,
1
,
0
,
"RCToParamDialogController"
);
#ifndef __mobile__
#ifndef NO_SERIAL_LINK
...
...
src/QmlControls/ParameterEditor.qml
View file @
17313b9b
...
...
@@ -28,7 +28,8 @@ Item {
property
int
_rowWidth
:
10
// Dynamic adjusted at runtime
property
bool
_searchFilter
:
searchText
.
text
.
trim
()
!=
""
///< true: showing results of search
property
var
_searchResults
///< List of parameter names from search results
property
bool
_showRCToParam
:
!
ScreenTools
.
isMobile
&&
QGroundControl
.
multiVehicleManager
.
activeVehicle
.
px4Firmware
property
var
_activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
property
bool
_showRCToParam
:
_activeVehicle
.
px4Firmware
property
var
_appSettings
:
QGroundControl
.
settingsManager
.
appSettings
ParameterEditorController
{
...
...
@@ -133,8 +134,8 @@ Item {
}
QGCMenuSeparator
{
visible
:
_showRCToParam
}
QGCMenuItem
{
text
:
qsTr
(
"
Clear RC to Param
"
)
onTriggered
:
controller
.
clearRCToParam
()
text
:
qsTr
(
"
Clear
all
RC to Param
"
)
onTriggered
:
_activeVehicle
.
clearAllParamMapRC
()
visible
:
_showRCToParam
}
QGCMenuSeparator
{
}
...
...
src/QmlControls/ParameterEditorController.cc
View file @
17313b9b
...
...
@@ -86,13 +86,6 @@ QStringList ParameterEditorController::searchParameters(const QString& searchTex
return
list
;
}
void
ParameterEditorController
::
clearRCToParam
(
void
)
{
if
(
_uas
)
{
_uas
->
unsetRCToParameterMap
();
}
}
void
ParameterEditorController
::
saveToFile
(
const
QString
&
filename
)
{
if
(
!
filename
.
isEmpty
())
{
...
...
src/QmlControls/ParameterEditorController.h
View file @
17313b9b
...
...
@@ -41,7 +41,6 @@ public:
Q_INVOKABLE
QStringList
getGroupsForCategory
(
const
QString
&
category
);
Q_INVOKABLE
QStringList
searchParameters
(
const
QString
&
searchText
,
bool
searchInName
=
true
,
bool
searchInDescriptions
=
true
);
Q_INVOKABLE
void
clearRCToParam
(
void
);
Q_INVOKABLE
void
saveToFile
(
const
QString
&
filename
);
Q_INVOKABLE
void
loadFromFile
(
const
QString
&
filename
);
Q_INVOKABLE
void
refresh
(
void
);
...
...
src/QmlControls/ParameterEditorDialog.qml
View file @
17313b9b
...
...
@@ -10,6 +10,7 @@
import
QtQuick
2.3
import
QtQuick
.
Controls
1.2
import
QtQuick
.
Layouts
1.2
import
QtQuick
.
Dialogs
1.3
import
QGroundControl
1.0
import
QGroundControl
.
Controls
1.0
...
...
@@ -125,8 +126,8 @@ QGCViewDialog {
Layout.fillWidth
:
true
focus
:
setFocus
inputMethodHints
:
(
fact
.
typeIsString
||
ScreenTools
.
isiOS
)
?
Qt
.
ImhNone
:
// iOS numeric keyboard has no done button, we can't use it
Qt
.
ImhFormattedNumbersOnly
// Forces use of virtual numeric keyboard
Qt
.
ImhNone
:
// iOS numeric keyboard has no done button, we can't use it
Qt
.
ImhFormattedNumbersOnly
// Forces use of virtual numeric keyboard
}
QGCButton
{
...
...
@@ -286,11 +287,19 @@ QGCViewDialog {
}
QGCButton
{
text
:
qsTr
(
"
Set RC to Param...
"
)
width
:
_editFieldWidth
visible
:
_advanced
.
checked
&&
!
validate
&&
showRCToParam
onClicked
:
controller
.
setRCToParam
(
fact
.
name
)
text
:
qsTr
(
"
Set RC to Param
"
)
width
:
_editFieldWidth
visible
:
_advanced
.
checked
&&
!
validate
&&
showRCToParam
onClicked
:
mainWindow
.
showPopupDialog
(
rcToParamDialog
)
}
}
// Column
}
Component
{
id
:
rcToParamDialog
RCToParamDialog
{
tuningFact
:
fact
}
}
}
// QGCViewDialog
src/QmlControls/QGroundControl/Controls/qmldir
View file @
17313b9b
...
...
@@ -80,6 +80,7 @@ RallyPointEditorHeader 1.0 RallyPointEditorHeader.qml
RallyPointItemEditor 1.0 RallyPointItemEditor.qml
RallyPointMapVisuals 1.0 RallyPointMapVisuals.qml
RCChannelMonitor 1.0 RCChannelMonitor.qml
RCToParamDialog 1.0 RCToParamDialog.qml
RoundButton 1.0 RoundButton.qml
SectionHeader 1.0 SectionHeader.qml
SetupPage 1.0 SetupPage.qml
...
...
src/QmlControls/RCToParamDialog.FactMetaData.json
0 → 100644
View file @
17313b9b
[
{
"name"
:
"Scale"
,
"shortDescription"
:
"Scale the RC range"
,
"type"
:
"double"
,
"min"
:
-1
,
"max"
:
1
,
"defaultValue"
:
1
,
"decimalPlaces"
:
1
},
{
"name"
:
"CenterValue"
,
"shortDescription"
:
"Parameter value when RC output is 0"
,
"type"
:
"double"
,
"min"
:
-180.0
,
"max"
:
180.0
,
"decimalPlaces"
:
7
},
{
"name"
:
"MinValue"
,
"shortDescription"
:
"Minimum parameter value"
,
"type"
:
"double"
,
"decimalPlaces"
:
7
},
{
"name"
:
"MaxValue"
,
"shortDescription"
:
"Maximum parameter value"
,
"type"
:
"double"
,
"decimalPlaces"
:
7
}
]
src/QmlControls/RCToParamDialog.qml
0 → 100644
View file @
17313b9b
/****************************************************************************
*
* (c) 2009-2020 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.
*
****************************************************************************/
import
QtQuick
2.12
import
QtQuick
.
Layouts
1.2
import
QtQuick
.
Controls
2.5
import
QtQuick
.
Dialogs
1.3
import
QGroundControl
1.0
import
QGroundControl
.
Controls
1.0
import
QGroundControl
.
ScreenTools
1.0
import
QGroundControl
.
FactSystem
1.0
import
QGroundControl
.
FactControls
1.0
import
QGroundControl
.
Controllers
1.0
QGCPopupDialog
{
property
alias
tuningFact
:
controller
.
tuningFact
title
:
qsTr
(
"
RC To Param
"
)
buttons
:
StandardButton
.
Cancel
|
StandardButton
.
Ok
function
accept
()
{
QGroundControl
.
multiVehicleManager
.
activeVehicle
.
sendParamMapRC
(
tuningFact
.
name
,
scale
.
text
,
centerValue
.
text
,
tuningID
.
currentIndex
,
minValue
.
text
,
maxValue
.
text
);
hideDialog
()
}
RCToParamDialogController
{
id
:
controller
}
ColumnLayout
{
spacing
:
ScreenTools
.
defaultDialogControlSpacing
QGCLabel
{
Layout.preferredWidth
:
mainGrid
.
width
Layout.fillWidth
:
true
wrapMode
:
Text
.
WordWrap
text
:
qsTr
(
"
Bind an RC Channel to a parameter value. Tuning IDs can be mapped to an RC Channel from Radio Setup page.
"
)
}
QGCLabel
{
Layout.preferredWidth
:
mainGrid
.
width
Layout.fillWidth
:
true
text
:
qsTr
(
"
Waiting on parameter update from Vehicle.
"
)
visible
:
!
controller
.
ready
}
GridLayout
{
id
:
mainGrid
columns
:
2
rowSpacing
:
ScreenTools
.
defaultDialogControlSpacing
columnSpacing
:
ScreenTools
.
defaultDialogControlSpacing
enabled
:
controller
.
ready
QGCLabel
{
text
:
qsTr
(
"
Parameter
"
)
}
QGCLabel
{
text
:
tuningFact
.
name
}
QGCLabel
{
text
:
qsTr
(
"
Tuning ID
"
)
}
QGCComboBox
{
id
:
tuningID
Layout.fillWidth
:
true
currentIndex
:
0
model
:
[
1
,
2
,
3
]
}
QGCLabel
{
text
:
qsTr
(
"
Scale
"
)
}
QGCTextField
{
id
:
scale
text
:
controller
.
scale
.
valueString
}
QGCLabel
{
text
:
qsTr
(
"
Center Value
"
)
}
QGCTextField
{
id
:
centerValue
text
:
controller
.
center
.
valueString
}
QGCLabel
{
text
:
qsTr
(
"
Min Value
"
)
}
QGCTextField
{
id
:
minValue
text
:
controller
.
min
.
valueString
}
QGCLabel
{
text
:
qsTr
(
"
Max Value
"
)
}
QGCTextField
{
id
:
maxValue
text
:
controller
.
max
.
valueString
}
}
QGCLabel
{
Layout.preferredWidth
:
mainGrid
.
width
Layout.fillWidth
:
true
wrapMode
:
Text
.
WordWrap
text
:
qsTr
(
"
Double check that all values are correct prior to confirming dialog.
"
)
}
}
}
src/QmlControls/RCToParamDialogController.cc
0 → 100644
View file @
17313b9b
/****************************************************************************
*
* (c) 2009-2020 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 "RCToParamDialogController.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
const
char
*
RCToParamDialogController
::
_scaleFactName
=
"Scale"
;
const
char
*
RCToParamDialogController
::
_centerFactName
=
"CenterValue"
;
const
char
*
RCToParamDialogController
::
_minFactName
=
"MinValue"
;
const
char
*
RCToParamDialogController
::
_maxFactName
=
"MaxValue"
;
QMap
<
QString
,
FactMetaData
*>
RCToParamDialogController
::
_metaDataMap
;
RCToParamDialogController
::
RCToParamDialogController
(
void
)
:
_scaleFact
(
0
,
_scaleFactName
,
FactMetaData
::
valueTypeDouble
)
,
_centerFact
(
0
,
_centerFactName
,
FactMetaData
::
valueTypeDouble
)
,
_minFact
(
0
,
_minFactName
,
FactMetaData
::
valueTypeDouble
)
,
_maxFact
(
0
,
_maxFactName
,
FactMetaData
::
valueTypeDouble
)
{
if
(
_metaDataMap
.
isEmpty
())
{
_metaDataMap
=
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/RCToParamDialog.FactMetaData.json"
),
nullptr
/* QObject parent */
);
}
_scaleFact
.
setMetaData
(
_metaDataMap
[
_scaleFactName
],
true
/* setDefaultFromMetaData */
);
_centerFact
.
setMetaData
(
_metaDataMap
[
_centerFactName
]);
_minFact
.
setMetaData
(
_metaDataMap
[
_minFactName
]);
_maxFact
.
setMetaData
(
_metaDataMap
[
_maxFactName
]);
}
void
RCToParamDialogController
::
setTuningFact
(
Fact
*
tuningFact
)
{
_tuningFact
=
tuningFact
;
emit
tuningFactChanged
(
tuningFact
);
_centerFact
.
setRawValue
(
_tuningFact
->
rawValue
().
toDouble
());
_minFact
.
setRawValue
(
_tuningFact
->
rawMin
().
toDouble
());
_maxFact
.
setRawValue
(
_tuningFact
->
rawMax
().
toDouble
());
connect
(
_tuningFact
,
&
Fact
::
vehicleUpdated
,
this
,
&
RCToParamDialogController
::
_parameterUpdated
);
qgcApp
()
->
toolbox
()
->
multiVehicleManager
()
->
activeVehicle
()
->
parameterManager
()
->
refreshParameter
(
FactSystem
::
defaultComponentId
,
_tuningFact
->
name
());
}
void
RCToParamDialogController
::
_parameterUpdated
(
void
)
{
_ready
=
true
;
emit
readyChanged
(
true
);
}
src/QmlControls/RCToParamDialogController.h
0 → 100644
View file @
17313b9b
/****************************************************************************
*
* (c) 2009-2020 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 <QGeoCoordinate>
#include "FactSystem.h"
class
RCToParamDialogController
:
public
QObject
{
Q_OBJECT
public:
RCToParamDialogController
(
void
);
Q_PROPERTY
(
Fact
*
tuningFact
READ
tuningFact
WRITE
setTuningFact
NOTIFY
tuningFactChanged
)
Q_PROPERTY
(
bool
ready
MEMBER
_ready
NOTIFY
readyChanged
)
// true: editing can begin, false: still waiting for param update from vehicle
Q_PROPERTY
(
Fact
*
scale
READ
scale
CONSTANT
)
Q_PROPERTY
(
Fact
*
center
READ
center
CONSTANT
)
Q_PROPERTY
(
Fact
*
min
READ
min
CONSTANT
)
Q_PROPERTY
(
Fact
*
max
READ
max
CONSTANT
)
Fact
*
tuningFact
(
void
)
{
return
_tuningFact
;
}
Fact
*
scale
(
void
)
{
return
&
_scaleFact
;
}
Fact
*
center
(
void
)
{
return
&
_centerFact
;
}
Fact
*
min
(
void
)
{
return
&
_minFact
;
}
Fact
*
max
(
void
)
{
return
&
_maxFact
;
}
void
setTuningFact
(
Fact
*
tuningFact
);
signals:
void
tuningFactChanged
(
Fact
*
fact
);
void
readyChanged
(
bool
ready
);
private
slots
:
void
_parameterUpdated
(
void
);
private:
static
QMap
<
QString
,
FactMetaData
*>
_metaDataMap
;
Fact
*
_tuningFact
=
nullptr
;
bool
_ready
=
false
;
Fact
_scaleFact
;
Fact
_centerFact
;
Fact
_minFact
;
Fact
_maxFact
;
static
const
char
*
_scaleFactName
;
static
const
char
*
_centerFactName
;
static
const
char
*
_minFactName
;
static
const
char
*
_maxFactName
;
};
src/QmlControls/ScreenTools.qml
View file @
17313b9b
...
...
@@ -46,6 +46,9 @@ Item {
/// QFontMetrics::descent for default font at default point size
property
real
defaultFontDescent
:
0
/// The default amount of space in between controls in a dialog
property
real
defaultDialogControlSpacing
:
defaultFontPixelHeight
/
2
property
real
smallFontPointSize
:
10
property
real
mediumFontPointSize
:
10
property
real
largeFontPointSize
:
10
...
...
src/Vehicle/Vehicle.cc
View file @
17313b9b
...
...
@@ -4337,6 +4337,54 @@ void Vehicle::updateFlightDistance(double distance)
_flightDistanceFact
.
setRawValue
(
_flightDistanceFact
.
rawValue
().
toDouble
()
+
distance
);
}
void
Vehicle
::
sendParamMapRC
(
const
QString
&
paramName
,
double
scale
,
double
centerValue
,
int
tuningID
,
double
minValue
,
double
maxValue
)
{
mavlink_message_t
message
;
char
param_id_cstr
[
MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN
]
=
{};
// Copy string into buffer, ensuring not to exceed the buffer size
for
(
unsigned
int
i
=
0
;
i
<
sizeof
(
param_id_cstr
);
i
++
)
{
if
((
int
)
i
<
paramName
.
length
())
{
param_id_cstr
[
i
]
=
paramName
.
toLatin1
()[
i
];
}
}
mavlink_msg_param_map_rc_pack_chan
(
static_cast
<
uint8_t
>
(
_mavlink
->
getSystemId
()),
static_cast
<
uint8_t
>
(
_mavlink
->
getComponentId
()),
priorityLink
()
->
mavlinkChannel
(),
&
message
,
_id
,
MAV_COMP_ID_AUTOPILOT1
,
param_id_cstr
,
-
1
,
// parameter name specified as string in previous argument
static_cast
<
uint8_t
>
(
tuningID
),
static_cast
<
float
>
(
scale
),
static_cast
<
float
>
(
centerValue
),
static_cast
<
float
>
(
minValue
),
static_cast
<
float
>
(
maxValue
));
sendMessageOnLink
(
priorityLink
(),
message
);
}
void
Vehicle
::
clearAllParamMapRC
(
void
)
{
char
param_id_cstr
[
MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN
]
=
{};
for
(
int
i
=
0
;
i
<
3
;
i
++
)
{
mavlink_message_t
message
;
mavlink_msg_param_map_rc_pack_chan
(
static_cast
<
uint8_t
>
(
_mavlink
->
getSystemId
()),
static_cast
<
uint8_t
>
(
_mavlink
->
getComponentId
()),
priorityLink
()
->
mavlinkChannel
(),
&
message
,
_id
,
MAV_COMP_ID_AUTOPILOT1
,
param_id_cstr
,
-
2
,
// Disable map for specified tuning id
i
,
// tuning id
0
,
0
,
0
,
0
);
// unused
sendMessageOnLink
(
priorityLink
(),
message
);
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
...
...
src/Vehicle/Vehicle.h
View file @
17313b9b
...
...
@@ -781,6 +781,13 @@ public:
Q_INVOKABLE
void
gimbalYawStep
(
int
direction
);
Q_INVOKABLE
void
centerGimbal
();
/// Sends PARAM_MAP_RC message to vehicle
Q_INVOKABLE
void
sendParamMapRC
(
const
QString
&
paramName
,
double
scale
,
double
centerValue
,
int
tuningID
,
double
minValue
,
double
maxValue
);
/// Clears all PARAM_MAP_RC settings from vehicle
Q_INVOKABLE
void
clearAllParamMapRC
(
void
);
#if !defined(NO_ARDUPILOT_DIALECT)
Q_INVOKABLE
void
flashBootloader
();
#endif
...
...
src/comm/MockLink.cc
View file @
17313b9b
...
...
@@ -505,6 +505,10 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
_handleLogRequestData
(
msg
);
break
;
case
MAVLINK_MSG_ID_PARAM_MAP_RC
:
_handleParamMapRC
(
msg
);
break
;
default:
break
;
}
...
...
@@ -517,6 +521,22 @@ void MockLink::_handleHeartBeat(const mavlink_message_t& msg)
qCDebug
(
MockLinkLog
)
<<
"Heartbeat"
;
}
void
MockLink
::
_handleParamMapRC
(
const
mavlink_message_t
&
msg
)
{
mavlink_param_map_rc_t
paramMapRC
;
mavlink_msg_param_map_rc_decode
(
&
msg
,
&
paramMapRC
);
const
QString
paramName
(
QString
::
fromLocal8Bit
(
paramMapRC
.
param_id
,
static_cast
<
int
>
(
strnlen
(
paramMapRC
.
param_id
,
MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN
))));
if
(
paramMapRC
.
param_index
==
-
1
)
{
qDebug
()
<<
QStringLiteral
(
"MockLink - PARAM_MAP_RC: param(%1) tuningID(%2) centerValue(%3) scale(%4) min(%5) max(%6)"
).
arg
(
paramName
).
arg
(
paramMapRC
.
parameter_rc_channel_index
).
arg
(
paramMapRC
.
param_value0
).
arg
(
paramMapRC
.
scale
).
arg
(
paramMapRC
.
param_value_min
).
arg
(
paramMapRC
.
param_value_max
);
}
else
if
(
paramMapRC
.
param_index
==
-
2
)
{
qDebug
()
<<
QStringLiteral
(
"MockLink - PARAM_MAP_RC: Clear tuningID(%1)"
).
arg
(
paramMapRC
.
parameter_rc_channel_index
);
}
else
{
qWarning
()
<<
QStringLiteral
(
"MockLink - PARAM_MAP_RC: Unsupported param_index(%1)"
).
arg
(
paramMapRC
.
param_index
);
}
}
void
MockLink
::
_handleSetMode
(
const
mavlink_message_t
&
msg
)
{
mavlink_set_mode_t
request
;
...
...
src/comm/MockLink.h
View file @
17313b9b
...
...
@@ -192,6 +192,7 @@ private:
void
_handlePreFlightCalibration
(
const
mavlink_command_long_t
&
request
);
void
_handleLogRequestList
(
const
mavlink_message_t
&
msg
);
void
_handleLogRequestData
(
const
mavlink_message_t
&
msg
);
void
_handleParamMapRC
(
const
mavlink_message_t
&
msg
);
float
_floatUnionForParam
(
int
componentId
,
const
QString
&
paramName
);
void
_setParamFloatUnionIntoMap
(
int
componentId
,
const
QString
&
paramName
,
float
paramFloat
);
void
_sendHomePosition
(
void
);
...
...
src/uas/UAS.cc
View file @
17313b9b
...
...
@@ -986,68 +986,6 @@ void UAS::pairRX(int rxType, int rxSubType)
}
}
void
UAS
::
sendMapRCToParam
(
QString
param_id
,
float
scale
,
float
value0
,
quint8
param_rc_channel_index
,
float
valueMin
,
float
valueMax
)
{
if
(
!
_vehicle
)
{
return
;
}
mavlink_message_t
message
;
char
param_id_cstr
[
MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN
]
=
{};
// Copy string into buffer, ensuring not to exceed the buffer size
for
(
unsigned
int
i
=
0
;
i
<
sizeof
(
param_id_cstr
);
i
++
)
{
if
((
int
)
i
<
param_id
.
length
())
{
param_id_cstr
[
i
]
=
param_id
.
toLatin1
()[
i
];
}
}
mavlink_msg_param_map_rc_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
message
,
this
->
uasId
,
_vehicle
->
defaultComponentId
(),
param_id_cstr
,
-
1
,
param_rc_channel_index
,
value0
,
scale
,
valueMin
,
valueMax
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
message
);
//qDebug() << "Mavlink message sent";
}
void
UAS
::
unsetRCToParameterMap
()
{
if
(
!
_vehicle
)
{
return
;
}
char
param_id_cstr
[
MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN
]
=
{};
for
(
int
i
=
0
;
i
<
3
;
i
++
)
{
mavlink_message_t
message
;
mavlink_msg_param_map_rc_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
message
,
this
->
uasId
,
_vehicle
->
defaultComponentId
(),
param_id_cstr
,
-
2
,
i
,
0.0
f
,
0.0
f
,
0.0
f
,
0.0
f
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
message
);
}
}
void
UAS
::
shutdownVehicle
(
void
)
{
_vehicle
=
nullptr
;
...
...
src/uas/UAS.h
View file @
17313b9b
...
...
@@ -210,11 +210,6 @@ public slots:
void
startBusConfig
(
StartBusConfigType
calType
);
void
stopBusConfig
(
void
);
/** @brief Send command to map a RC channel to a parameter */
void
sendMapRCToParam
(
QString
param_id
,
float
scale
,
float
value0
,
quint8
param_rc_channel_index
,
float
valueMin
,
float
valueMax
);
/** @brief Send command to disable all bindings/maps between RC and parameters */
void
unsetRCToParameterMap
();
signals:
void
imageStarted
(
quint64
timestamp
);
/** @brief A new camera image has arrived */
...
...
src/uas/UASInterface.h
View file @
17313b9b
...
...
@@ -82,12 +82,6 @@ public slots:
/** @brief Order the robot to pair its receiver **/
virtual
void
pairRX
(
int
rxType
,
int
rxSubType
)
=
0
;
/** @brief Send command to map a RC channel to a parameter */
virtual
void
sendMapRCToParam
(
QString
param_id
,
float
scale
,
float
value0
,
quint8
param_rc_channel_index
,
float
valueMin
,
float
valueMax
)
=
0
;
/** @brief Send command to disable all bindings/maps between RC and parameters */
virtual
void
unsetRCToParameterMap
()
=
0
;
signals:
/** @brief The robot is connected **/
void
connected
();
...
...
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