Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
d4d26f03
Commit
d4d26f03
authored
Nov 24, 2015
by
Don Gagne
Browse files
Explicit raw/cooked values
parent
c33a4a83
Changes
16
Expand all
Hide whitespace changes
Inline
Side-by-side
src/AutoPilotPlugins/APM/APMAirframeComponent.cc
View file @
d4d26f03
...
...
@@ -59,7 +59,7 @@ bool APMAirframeComponent::setupComplete(void) const
{
// You'll need to figure out which parameters trigger setup complete
#if 0
return _autopilot->getParameterFact(FactSystem::defaultComponentId, "SYS_AUTOSTART")->
v
alue().toInt() != 0;
return _autopilot->getParameterFact(FactSystem::defaultComponentId, "SYS_AUTOSTART")->
rawV
alue().toInt() != 0;
#else
return
true
;
#endif
...
...
src/AutoPilotPlugins/PX4/AirframeComponent.cc
View file @
d4d26f03
...
...
@@ -142,7 +142,7 @@ bool AirframeComponent::requiresSetup(void) const
bool
AirframeComponent
::
setupComplete
(
void
)
const
{
return
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"SYS_AUTOSTART"
)
->
v
alue
().
toInt
()
!=
0
;
return
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"SYS_AUTOSTART"
)
->
rawV
alue
().
toInt
()
!=
0
;
}
QString
AirframeComponent
::
setupStateDescription
(
void
)
const
...
...
src/AutoPilotPlugins/PX4/AirframeComponentController.cc
View file @
d4d26f03
...
...
@@ -57,8 +57,7 @@ AirframeComponentController::AirframeComponentController(void) :
// Load up member variables
bool
autostartFound
=
false
;
_autostartId
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
"SYS_AUTOSTART"
)
->
value
().
toInt
();
_autostartId
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
"SYS_AUTOSTART"
)
->
rawValue
().
toInt
();
for
(
int
tindex
=
0
;
tindex
<
AirframeComponentAirframes
::
get
().
count
();
tindex
++
)
{
...
...
@@ -114,8 +113,8 @@ void AirframeComponentController::changeAutostart(void)
connect
(
sysAutoConfigFact
,
&
Fact
::
vehicleUpdated
,
this
,
&
AirframeComponentController
::
_waitParamWriteSignal
);
// We use forceSetValue to params are sent even if the previous value is that same as the new value
sysAutoStartFact
->
forceSetValue
(
_autostartId
);
sysAutoConfigFact
->
forceSetValue
(
1
);
sysAutoStartFact
->
forceSet
Raw
Value
(
_autostartId
);
sysAutoConfigFact
->
forceSet
Raw
Value
(
1
);
}
void
AirframeComponentController
::
_waitParamWriteSignal
(
QVariant
value
)
...
...
src/AutoPilotPlugins/PX4/FlightModesComponent.cc
View file @
d4d26f03
...
...
@@ -68,13 +68,13 @@ QString FlightModesComponent::iconResource(void) const
bool
FlightModesComponent
::
requiresSetup
(
void
)
const
{
return
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
v
alue
().
toInt
()
==
1
?
false
:
true
;
return
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
rawV
alue
().
toInt
()
==
1
?
false
:
true
;
}
bool
FlightModesComponent
::
setupComplete
(
void
)
const
{
return
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
v
alue
().
toInt
()
==
1
||
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"RC_MAP_MODE_SW"
)
->
v
alue
().
toInt
()
!=
0
;
return
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
rawV
alue
().
toInt
()
==
1
||
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"RC_MAP_MODE_SW"
)
->
rawV
alue
().
toInt
()
!=
0
;
}
QString
FlightModesComponent
::
setupStateDescription
(
void
)
const
...
...
@@ -117,7 +117,7 @@ QUrl FlightModesComponent::summaryQmlSource(void) const
QString
FlightModesComponent
::
prerequisiteSetup
(
void
)
const
{
if
(
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
v
alue
().
toInt
()
==
1
)
{
if
(
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
rawV
alue
().
toInt
()
==
1
)
{
// No RC input
return
QString
();
}
else
{
...
...
src/AutoPilotPlugins/PX4/FlightModesComponentController.cc
View file @
d4d26f03
This diff is collapsed.
Click to expand it.
src/AutoPilotPlugins/PX4/PowerComponent.cc
View file @
d4d26f03
...
...
@@ -57,9 +57,9 @@ bool PowerComponent::requiresSetup(void) const
bool
PowerComponent
::
setupComplete
(
void
)
const
{
QVariant
cvalue
,
evalue
,
nvalue
;
return
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"BAT_V_CHARGED"
)
->
v
alue
().
toFloat
()
!=
0.0
f
&&
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"BAT_V_EMPTY"
)
->
v
alue
().
toFloat
()
!=
0.0
f
&&
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"BAT_N_CELLS"
)
->
v
alue
().
toInt
()
!=
0
;
return
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"BAT_V_CHARGED"
)
->
rawV
alue
().
toFloat
()
!=
0.0
f
&&
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"BAT_V_EMPTY"
)
->
rawV
alue
().
toFloat
()
!=
0.0
f
&&
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"BAT_N_CELLS"
)
->
rawV
alue
().
toInt
()
!=
0
;
}
QString
PowerComponent
::
setupStateDescription
(
void
)
const
...
...
src/AutoPilotPlugins/PX4/RadioComponent.cc
View file @
d4d26f03
...
...
@@ -53,18 +53,18 @@ QString RadioComponent::iconResource(void) const
bool
RadioComponent
::
requiresSetup
(
void
)
const
{
return
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
v
alue
().
toInt
()
==
1
?
false
:
true
;
return
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
rawV
alue
().
toInt
()
==
1
?
false
:
true
;
}
bool
RadioComponent
::
setupComplete
(
void
)
const
{
if
(
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
v
alue
().
toInt
()
!=
1
)
{
if
(
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
rawV
alue
().
toInt
()
!=
1
)
{
// The best we can do to detect the need for a radio calibration is look for attitude
// controls to be mapped.
QStringList
attitudeMappings
;
attitudeMappings
<<
"RC_MAP_ROLL"
<<
"RC_MAP_PITCH"
<<
"RC_MAP_YAW"
<<
"RC_MAP_THROTTLE"
;
foreach
(
QString
mapParam
,
attitudeMappings
)
{
if
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
mapParam
)
->
v
alue
().
toInt
()
==
0
)
{
if
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
mapParam
)
->
rawV
alue
().
toInt
()
==
0
)
{
return
false
;
}
}
...
...
@@ -115,7 +115,7 @@ QUrl RadioComponent::summaryQmlSource(void) const
QString
RadioComponent
::
prerequisiteSetup
(
void
)
const
{
if
(
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
v
alue
().
toInt
()
!=
1
)
{
if
(
_autopilot
->
getParameterFact
(
-
1
,
"COM_RC_IN_MODE"
)
->
rawV
alue
().
toInt
()
!=
1
)
{
PX4AutoPilotPlugin
*
plugin
=
dynamic_cast
<
PX4AutoPilotPlugin
*>
(
_autopilot
);
Q_ASSERT
(
plugin
);
...
...
src/AutoPilotPlugins/PX4/RadioComponentController.cc
View file @
d4d26f03
...
...
@@ -652,7 +652,7 @@ void RadioComponentController::_resetInternalCalibrationValues(void)
enum
rcCalFunctions
curFunction
=
rgFlightModeFunctions
[
i
];
bool
ok
;
int
switchChannel
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
_rgFunctionInfo
[
curFunction
].
parameterName
)
->
v
alue
().
toInt
(
&
ok
);
int
switchChannel
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
_rgFunctionInfo
[
curFunction
].
parameterName
)
->
rawV
alue
().
toInt
(
&
ok
);
Q_ASSERT
(
ok
);
// Parameter: 1-based channel, 0=not mapped
...
...
@@ -694,16 +694,16 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
for
(
int
i
=
0
;
i
<
_chanMax
;
++
i
)
{
struct
ChannelInfo
*
info
=
&
_rgChannelInfo
[
i
];
info
->
rcTrim
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
trimTpl
.
arg
(
i
+
1
))
->
v
alue
().
toInt
(
&
convertOk
);
info
->
rcTrim
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
trimTpl
.
arg
(
i
+
1
))
->
rawV
alue
().
toInt
(
&
convertOk
);
Q_ASSERT
(
convertOk
);
info
->
rcMin
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
minTpl
.
arg
(
i
+
1
))
->
v
alue
().
toInt
(
&
convertOk
);
info
->
rcMin
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
minTpl
.
arg
(
i
+
1
))
->
rawV
alue
().
toInt
(
&
convertOk
);
Q_ASSERT
(
convertOk
);
info
->
rcMax
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
maxTpl
.
arg
(
i
+
1
))
->
v
alue
().
toInt
(
&
convertOk
);
info
->
rcMax
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
maxTpl
.
arg
(
i
+
1
))
->
rawV
alue
().
toInt
(
&
convertOk
);
Q_ASSERT
(
convertOk
);
float
floatReversed
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
revTpl
.
arg
(
i
+
1
))
->
v
alue
().
toFloat
(
&
convertOk
);
float
floatReversed
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
revTpl
.
arg
(
i
+
1
))
->
rawV
alue
().
toFloat
(
&
convertOk
);
Q_ASSERT
(
convertOk
);
Q_ASSERT
(
floatReversed
==
1.0
f
||
floatReversed
==
-
1.0
f
);
info
->
reversed
=
floatReversed
==
-
1.0
f
;
...
...
@@ -712,7 +712,7 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
for
(
int
i
=
0
;
i
<
rcCalFunctionMax
;
i
++
)
{
int32_t
paramChannel
;
paramChannel
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
_rgFunctionInfo
[
i
].
parameterName
)
->
v
alue
().
toInt
(
&
convertOk
);
paramChannel
=
getParameterFact
(
FactSystem
::
defaultComponentId
,
_rgFunctionInfo
[
i
].
parameterName
)
->
rawV
alue
().
toInt
(
&
convertOk
);
Q_ASSERT
(
convertOk
);
if
(
paramChannel
!=
0
)
{
...
...
@@ -794,10 +794,10 @@ void RadioComponentController::_writeCalibration(void)
struct
ChannelInfo
*
info
=
&
_rgChannelInfo
[
chan
];
int
oneBasedChannel
=
chan
+
1
;
getParameterFact
(
FactSystem
::
defaultComponentId
,
trimTpl
.
arg
(
oneBasedChannel
))
->
setValue
((
float
)
info
->
rcTrim
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
minTpl
.
arg
(
oneBasedChannel
))
->
setValue
((
float
)
info
->
rcMin
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
maxTpl
.
arg
(
oneBasedChannel
))
->
setValue
((
float
)
info
->
rcMax
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
revTpl
.
arg
(
oneBasedChannel
))
->
setValue
(
info
->
reversed
?
-
1.0
f
:
1.0
f
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
trimTpl
.
arg
(
oneBasedChannel
))
->
set
Raw
Value
((
float
)
info
->
rcTrim
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
minTpl
.
arg
(
oneBasedChannel
))
->
set
Raw
Value
((
float
)
info
->
rcMin
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
maxTpl
.
arg
(
oneBasedChannel
))
->
set
Raw
Value
((
float
)
info
->
rcMax
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
revTpl
.
arg
(
oneBasedChannel
))
->
set
Raw
Value
(
info
->
reversed
?
-
1.0
f
:
1.0
f
);
}
// Write function mapping parameters
...
...
@@ -810,12 +810,12 @@ void RadioComponentController::_writeCalibration(void)
// Note that the channel value is 1-based
paramChannel
=
_rgFunctionChannelMapping
[
i
]
+
1
;
}
getParameterFact
(
FactSystem
::
defaultComponentId
,
_rgFunctionInfo
[
i
].
parameterName
)
->
setValue
(
paramChannel
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
_rgFunctionInfo
[
i
].
parameterName
)
->
set
Raw
Value
(
paramChannel
);
}
// If the RC_CHAN_COUNT parameter is available write the channel count
if
(
parameterExists
(
FactSystem
::
defaultComponentId
,
"RC_CHAN_CNT"
))
{
getParameterFact
(
FactSystem
::
defaultComponentId
,
"RC_CHAN_CNT"
)
->
setValue
(
_chanCount
);
getParameterFact
(
FactSystem
::
defaultComponentId
,
"RC_CHAN_CNT"
)
->
set
Raw
Value
(
_chanCount
);
}
_stopCalibration
();
...
...
src/AutoPilotPlugins/PX4/SensorsComponent.cc
View file @
d4d26f03
...
...
@@ -62,7 +62,7 @@ bool SensorsComponent::requiresSetup(void) const
bool
SensorsComponent
::
setupComplete
(
void
)
const
{
foreach
(
QString
triggerParam
,
setupCompleteChangedTriggerList
())
{
if
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
triggerParam
)
->
v
alue
().
toFloat
()
==
0.0
f
)
{
if
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
triggerParam
)
->
rawV
alue
().
toFloat
()
==
0.0
f
)
{
return
false
;
}
}
...
...
src/FactSystem/Fact.cc
View file @
d4d26f03
...
...
@@ -73,16 +73,16 @@ const Fact& Fact::operator=(const Fact& other)
return
*
this
;
}
void
Fact
::
forceSetValue
(
const
QVariant
&
value
)
void
Fact
::
forceSet
Raw
Value
(
const
QVariant
&
value
)
{
if
(
_metaData
)
{
QVariant
typedValue
;
QString
errorString
;
if
(
_metaData
->
convertAndValidate
(
value
,
true
/* convertOnly */
,
typedValue
,
errorString
))
{
_rawValue
.
setValue
(
_metaData
->
cookedTranslator
()(
typedValue
)
)
;
emit
valueChanged
(
typ
edValue
);
emit
_containerValueChanged
(
typed
Value
);
_rawValue
.
setValue
(
typedValue
);
emit
valueChanged
(
cook
edValue
()
);
emit
_container
Raw
ValueChanged
(
raw
Value
()
);
}
}
else
{
qWarning
()
<<
"Meta data pointer missing"
;
...
...
@@ -98,8 +98,8 @@ void Fact::setRawValue(const QVariant& value)
if
(
_metaData
->
convertAndValidate
(
value
,
true
/* convertOnly */
,
typedValue
,
errorString
))
{
if
(
typedValue
!=
_rawValue
)
{
_rawValue
.
setValue
(
typedValue
);
emit
valueChanged
(
this
->
v
alue
());
emit
_containerValueChanged
(
this
->
v
alue
());
emit
valueChanged
(
cookedV
alue
());
emit
_container
Raw
ValueChanged
(
rawV
alue
());
}
}
}
else
{
...
...
@@ -107,7 +107,7 @@ void Fact::setRawValue(const QVariant& value)
}
}
void
Fact
::
setValue
(
const
QVariant
&
value
)
void
Fact
::
set
Cooked
Value
(
const
QVariant
&
value
)
{
if
(
_metaData
)
{
setRawValue
(
_metaData
->
cookedTranslator
()(
value
));
...
...
@@ -121,7 +121,7 @@ void Fact::setEnumStringValue(const QString& value)
if
(
_metaData
)
{
int
index
=
_metaData
->
enumStrings
().
indexOf
(
value
);
if
(
index
!=
-
1
)
{
setValue
(
_metaData
->
enumValues
()[
index
]);
set
Cooked
Value
(
_metaData
->
enumValues
()[
index
]);
}
}
else
{
qWarning
()
<<
"Meta data pointer missing"
;
...
...
@@ -131,16 +131,16 @@ void Fact::setEnumStringValue(const QString& value)
void
Fact
::
setEnumIndex
(
int
index
)
{
if
(
_metaData
)
{
setValue
(
_metaData
->
enumValues
()[
index
]);
set
Cooked
Value
(
_metaData
->
enumValues
()[
index
]);
}
else
{
qWarning
()
<<
"Meta data pointer missing"
;
}
}
void
Fact
::
_containerSetValue
(
const
QVariant
&
value
)
void
Fact
::
_containerSet
Raw
Value
(
const
QVariant
&
value
)
{
_rawValue
=
value
;
emit
valueChanged
(
_raw
Value
);
emit
valueChanged
(
cooked
Value
()
);
emit
vehicleUpdated
(
_rawValue
);
}
...
...
@@ -154,7 +154,7 @@ int Fact::componentId(void) const
return
_componentId
;
}
QVariant
Fact
::
v
alue
(
void
)
const
QVariant
Fact
::
cookedV
alue
(
void
)
const
{
if
(
_metaData
)
{
return
_metaData
->
rawTranslator
()(
_rawValue
);
...
...
@@ -183,7 +183,7 @@ int Fact::enumIndex(void) const
if
(
_metaData
)
{
int
index
=
0
;
foreach
(
QVariant
enumValue
,
_metaData
->
enumValues
())
{
if
(
enumValue
==
v
alue
())
{
if
(
enumValue
==
rawV
alue
())
{
return
index
;
}
index
++
;
...
...
@@ -236,7 +236,7 @@ QString Fact::_variantToString(const QVariant& variant) const
QString
Fact
::
valueString
(
void
)
const
{
return
_variantToString
(
v
alue
());
return
_variantToString
(
cookedV
alue
());
}
QVariant
Fact
::
defaultValue
(
void
)
const
...
...
@@ -365,14 +365,14 @@ QString Fact::group(void) const
void
Fact
::
setMetaData
(
FactMetaData
*
metaData
)
{
_metaData
=
metaData
;
emit
valueChanged
(
v
alue
());
emit
valueChanged
(
cookedV
alue
());
}
bool
Fact
::
valueEqualsDefault
(
void
)
const
{
if
(
_metaData
)
{
if
(
_metaData
->
defaultValueAvailable
())
{
return
_metaData
->
defaultValue
()
==
v
alue
();
return
_metaData
->
defaultValue
()
==
rawV
alue
();
}
else
{
return
false
;
}
...
...
src/FactSystem/Fact.h
View file @
d4d26f03
...
...
@@ -67,7 +67,7 @@ public:
Q_PROPERTY
(
QString
shortDescription
READ
shortDescription
CONSTANT
)
Q_PROPERTY
(
FactMetaData
::
ValueType_t
type
READ
type
CONSTANT
)
Q_PROPERTY
(
QString
units
READ
units
CONSTANT
)
Q_PROPERTY
(
QVariant
value
READ
value
WRITE
set
Value
NOTIFY
valueChanged
)
Q_PROPERTY
(
QVariant
value
READ
cookedValue
WRITE
set
CookedValue
NOTIFY
valueChanged
)
Q_PROPERTY
(
bool
valueEqualsDefault
READ
valueEqualsDefault
NOTIFY
valueChanged
)
Q_PROPERTY
(
QVariant
valueString
READ
valueString
NOTIFY
valueChanged
)
...
...
@@ -75,6 +75,7 @@ public:
/// @param convertOnly true: validate type conversion only, false: validate against meta data as well
Q_INVOKABLE
QString
validate
(
const
QString
&
value
,
bool
convertOnly
);
QVariant
cookedValue
(
void
)
const
;
/// Value after translation
int
componentId
(
void
)
const
;
int
decimalPlaces
(
void
)
const
;
QVariant
defaultValue
(
void
)
const
;
...
...
@@ -97,24 +98,23 @@ public:
QString
shortDescription
(
void
)
const
;
FactMetaData
::
ValueType_t
type
(
void
)
const
;
QString
units
(
void
)
const
;
QVariant
value
(
void
)
const
;
QString
valueString
(
void
)
const
;
bool
valueEqualsDefault
(
void
)
const
;
void
setRawValue
(
const
QVariant
&
value
);
void
set
Value
(
const
QVariant
&
value
);
void
set
CookedValue
(
const
QVariant
&
value
);
void
setEnumIndex
(
int
index
);
void
setEnumStringValue
(
const
QString
&
value
);
// C++ methods
/// Sets and sends new value to vehicle even if value is the same
void
forceSetValue
(
const
QVariant
&
value
);
void
forceSet
Raw
Value
(
const
QVariant
&
value
);
/// Sets the meta data associated with the Fact.
void
setMetaData
(
FactMetaData
*
metaData
);
void
_containerSetValue
(
const
QVariant
&
value
);
void
_containerSet
Raw
Value
(
const
QVariant
&
value
);
/// Generally you should not change the name of a fact. But if you know what you are doing, you can.
void
_setName
(
const
QString
&
name
)
{
_name
=
name
;
}
...
...
@@ -131,7 +131,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.
void
_containerValueChanged
(
const
QVariant
&
value
);
void
_container
Raw
ValueChanged
(
const
QVariant
&
value
);
private:
QString
_variantToString
(
const
QVariant
&
variant
)
const
;
...
...
src/FactSystem/FactSystemTestBase.cc
View file @
d4d26f03
...
...
@@ -61,7 +61,7 @@ void FactSystemTestBase::_parameter_default_component_id_test(void)
QVERIFY
(
_plugin
->
factExists
(
FactSystem
::
ParameterProvider
,
FactSystem
::
defaultComponentId
,
"RC_MAP_THROTTLE"
));
Fact
*
fact
=
_plugin
->
getFact
(
FactSystem
::
ParameterProvider
,
FactSystem
::
defaultComponentId
,
"RC_MAP_THROTTLE"
);
QVERIFY
(
fact
!=
NULL
);
QVariant
factValue
=
fact
->
v
alue
();
QVariant
factValue
=
fact
->
rawV
alue
();
QCOMPARE
(
factValue
.
isValid
(),
true
);
QCOMPARE
(
factValue
.
toInt
(),
3
);
...
...
@@ -72,7 +72,7 @@ void FactSystemTestBase::_parameter_specific_component_id_test(void)
QVERIFY
(
_plugin
->
factExists
(
FactSystem
::
ParameterProvider
,
50
,
"RC_MAP_THROTTLE"
));
Fact
*
fact
=
_plugin
->
getFact
(
FactSystem
::
ParameterProvider
,
50
,
"RC_MAP_THROTTLE"
);
QVERIFY
(
fact
!=
NULL
);
QVariant
factValue
=
fact
->
v
alue
();
QVariant
factValue
=
fact
->
rawV
alue
();
QCOMPARE
(
factValue
.
isValid
(),
true
);
...
...
@@ -82,7 +82,7 @@ void FactSystemTestBase::_parameter_specific_component_id_test(void)
QVERIFY
(
_plugin
->
factExists
(
FactSystem
::
ParameterProvider
,
51
,
"COMPONENT_51"
));
fact
=
_plugin
->
getFact
(
FactSystem
::
ParameterProvider
,
51
,
"COMPONENT_51"
);
QVERIFY
(
fact
!=
NULL
);
factValue
=
fact
->
v
alue
();
factValue
=
fact
->
rawV
alue
();
QCOMPARE
(
factValue
.
isValid
(),
true
);
QCOMPARE
(
factValue
.
toInt
(),
51
);
...
...
@@ -119,7 +119,7 @@ void FactSystemTestBase::_qmlUpdate_test(void)
// Change the value
QVariant
paramValue
=
12
;
_plugin
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"RC_MAP_THROTTLE"
)
->
setValue
(
paramValue
);
_plugin
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"RC_MAP_THROTTLE"
)
->
set
Raw
Value
(
paramValue
);
QTest
::
qWait
(
500
);
// Let the signals flow through
...
...
src/FactSystem/ParameterLoader.cc
View file @
d4d26f03
...
...
@@ -245,14 +245,14 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
_mapParameterName2Variant
[
componentId
][
parameterName
]
=
QVariant
::
fromValue
(
fact
);
// We need to know when the fact changes from QML so that we can send the new value to the parameter manager
connect
(
fact
,
&
Fact
::
_containerValueChanged
,
this
,
&
ParameterLoader
::
_valueUpdated
);
connect
(
fact
,
&
Fact
::
_container
Raw
ValueChanged
,
this
,
&
ParameterLoader
::
_valueUpdated
);
}
Q_ASSERT
(
_mapParameterName2Variant
[
componentId
].
contains
(
parameterName
));
Fact
*
fact
=
_mapParameterName2Variant
[
componentId
][
parameterName
].
value
<
Fact
*>
();
Q_ASSERT
(
fact
);
fact
->
_containerSetValue
(
value
);
fact
->
_containerSet
Raw
Value
(
value
);
if
(
setMetaData
)
{
_vehicle
->
firmwarePlugin
()
->
addMetaDataToFact
(
fact
);
...
...
@@ -465,7 +465,7 @@ void ParameterLoader::_waitingParamTimeout(void)
foreach
(
QString
paramName
,
_waitingWriteParamNameMap
[
componentId
].
keys
())
{
paramsRequested
=
true
;
_waitingWriteParamNameMap
[
componentId
][
paramName
]
++
;
// Bump retry count
_writeParameterRaw
(
componentId
,
paramName
,
_autopilot
->
getFact
(
FactSystem
::
ParameterProvider
,
componentId
,
paramName
)
->
v
alue
());
_writeParameterRaw
(
componentId
,
paramName
,
_autopilot
->
getFact
(
FactSystem
::
ParameterProvider
,
componentId
,
paramName
)
->
rawV
alue
());
qCDebug
(
ParameterLoaderLog
)
<<
"Write resend for (componentId:"
<<
componentId
<<
"paramName:"
<<
paramName
<<
"retryCount:"
<<
_waitingWriteParamNameMap
[
componentId
][
paramName
]
<<
")"
;
if
(
++
batchCount
>
maxBatchSize
)
{
...
...
@@ -586,7 +586,7 @@ void ParameterLoader::_writeLocalParamCache()
foreach
(
int
id
,
_mapParameterId2Name
[
component
].
keys
())
{
const
QString
name
(
_mapParameterId2Name
[
component
][
id
]);
const
Fact
*
fact
=
_mapParameterName2Variant
[
component
][
name
].
value
<
Fact
*>
();
cache_map
[
component
][
id
]
=
NamedParam
(
name
,
ParamTypeVal
(
fact
->
type
(),
fact
->
v
alue
()));
cache_map
[
component
][
id
]
=
NamedParam
(
name
,
ParamTypeVal
(
fact
->
type
(),
fact
->
rawV
alue
()));
}
}
...
...
@@ -700,7 +700,7 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream)
}
qCDebug
(
ParameterLoaderLog
)
<<
"Updating parameter"
<<
componentId
<<
paramName
<<
valStr
;
fact
->
setValue
(
valStr
);
fact
->
set
Raw
Value
(
valStr
);
}
}
}
...
...
src/MissionManager/MissionItem.cc
View file @
d4d26f03
...
...
@@ -193,9 +193,9 @@ MissionItem::MissionItem(int sequenceNumber,
_param2Fact
.
setRawValue
(
param2
);
_param3Fact
.
setRawValue
(
param3
);
_param4Fact
.
setRawValue
(
param4
);
_param5Fact
.
setValue
(
param5
);
_param6Fact
.
setValue
(
param6
);
_param7Fact
.
setValue
(
param7
);
_param5Fact
.
set
Raw
Value
(
param5
);
_param6Fact
.
set
Raw
Value
(
param6
);
_param7Fact
.
set
Raw
Value
(
param7
);
}
MissionItem
::
MissionItem
(
const
MissionItem
&
other
,
QObject
*
parent
)
...
...
@@ -226,13 +226,13 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
_syncFrameToAltitudeRelativeToHome
();
_syncCommandToSupportedCommand
(
QVariant
(
this
->
command
()));
_param1Fact
.
setValue
(
other
.
_param1Fact
.
v
alue
());
_param2Fact
.
setValue
(
other
.
_param2Fact
.
v
alue
());
_param3Fact
.
setValue
(
other
.
_param3Fact
.
v
alue
());
_param4Fact
.
setValue
(
other
.
_param4Fact
.
v
alue
());
_param5Fact
.
setValue
(
other
.
_param5Fact
.
v
alue
());
_param6Fact
.
setValue
(
other
.
_param6Fact
.
v
alue
());
_param7Fact
.
setValue
(
other
.
_param7Fact
.
v
alue
());
_param1Fact
.
set
Raw
Value
(
other
.
_param1Fact
.
rawV
alue
());
_param2Fact
.
set
Raw
Value
(
other
.
_param2Fact
.
rawV
alue
());
_param3Fact
.
set
Raw
Value
(
other
.
_param3Fact
.
rawV
alue
());
_param4Fact
.
set
Raw
Value
(
other
.
_param4Fact
.
rawV
alue
());
_param5Fact
.
set
Raw
Value
(
other
.
_param5Fact
.
rawV
alue
());
_param6Fact
.
set
Raw
Value
(
other
.
_param6Fact
.
rawV
alue
());
_param7Fact
.
set
Raw
Value
(
other
.
_param7Fact
.
rawV
alue
());
return
*
this
;
}
...
...
@@ -525,7 +525,7 @@ void MissionItem::setSequenceNumber(int sequenceNumber)
void
MissionItem
::
setCommand
(
MAV_CMD
command
)
{
if
((
MAV_CMD
)
this
->
command
()
!=
command
)
{
_commandFact
.
setValue
(
command
);
_commandFact
.
set
Raw
Value
(
command
);
setDefaultsForCommand
();
emit
commandChanged
(
this
->
command
());
}
...
...
@@ -540,7 +540,7 @@ void MissionItem::setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command)
void
MissionItem
::
setFrame
(
MAV_FRAME
frame
)
{
if
(
this
->
frame
()
!=
frame
)
{
_frameFact
.
setValue
(
frame
);
_frameFact
.
set
Raw
Value
(
frame
);
frameChanged
(
frame
);
}
}
...
...
@@ -548,7 +548,7 @@ void MissionItem::setFrame(MAV_FRAME frame)
void
MissionItem
::
setAutoContinue
(
bool
autoContinue
)
{
if
(
this
->
autoContinue
()
!=
autoContinue
)
{
_autoContinueFact
.
setValue
(
autoContinue
);
_autoContinueFact
.
set
Raw
Value
(
autoContinue
);
}
}
...
...
@@ -748,7 +748,7 @@ QmlObjectListModel* MissionItem::comboboxFacts(void)
QGeoCoordinate
MissionItem
::
coordinate
(
void
)
const
{
return
QGeoCoordinate
(
_param5Fact
.
v
alue
().
toDouble
(),
_param6Fact
.
v
alue
().
toDouble
(),
_param7Fact
.
v
alue
().
toDouble
());
return
QGeoCoordinate
(
_param5Fact
.
rawV
alue
().
toDouble
(),
_param6Fact
.
rawV
alue
().
toDouble
(),
_param7Fact
.
rawV
alue
().
toDouble
());
}
void
MissionItem
::
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
...
...
@@ -835,7 +835,7 @@ void MissionItem::_syncFrameToAltitudeRelativeToHome(void)
{
if
(
!
_syncingAltitudeRelativeToHomeAndFrame
)
{
_syncingAltitudeRelativeToHomeAndFrame
=
true
;
_altitudeRelativeToHomeFact
.
setValue
(
relativeAltitude
());
_altitudeRelativeToHomeFact
.
set
Raw
Value
(
relativeAltitude
());
_syncingAltitudeRelativeToHomeAndFrame
=
false
;
}
}
...
...
@@ -844,7 +844,7 @@ void MissionItem::_syncSupportedCommandToCommand(const QVariant& value)
{
if
(
!
_syncingSupportedCommandAndCommand
)
{
_syncingSupportedCommandAndCommand
=
true
;
_commandFact
.
setValue
(
value
.
toInt
());
_commandFact
.
set
Raw
Value
(
value
.
toInt
());
_syncingSupportedCommandAndCommand
=
false
;
}
}
...
...
@@ -853,7 +853,7 @@ void MissionItem::_syncCommandToSupportedCommand(const QVariant& value)
{
if
(
!
_syncingSupportedCommandAndCommand
)
{
_syncingSupportedCommandAndCommand
=
true
;
_supportedCommandFact
.
setValue
(
value
.
toInt
());
_supportedCommandFact
.
set
Raw
Value
(
value
.
toInt
());
_syncingSupportedCommandAndCommand
=
false
;
}
}
...
...
src/MissionManager/MissionItem.h
View file @
d4d26f03
...
...
@@ -95,7 +95,7 @@ public:
// Property accesors
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_commandFact
.
v
alue
().
toInt
();
};
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_commandFact
.
cookedV
alue
().
toInt
();
};
QString
commandDescription
(
void
)
const
;
QString
commandName
(
void
)
const
;
QGeoCoordinate
coordinate
(
void
)
const
;
...
...
@@ -135,8 +135,8 @@ public:
// C++ only methods
MAV_FRAME
frame
(
void
)
const
{
return
(
MAV_FRAME
)
_frameFact
.
v
alue
().
toInt
();
}
bool
autoContinue
(
void
)
const
{
return
_autoContinueFact
.
v
alue
().
toBool
();
}
MAV_FRAME
frame
(
void
)
const
{
return
(
MAV_FRAME
)
_frameFact
.
rawV
alue
().
toInt
();
}
bool
autoContinue
(
void
)
const
{
return
_autoContinueFact
.
rawV
alue
().
toBool
();
}
double
param1
(
void
)
const
{
return
_param1Fact
.
rawValue
().
toDouble
();
}
double
param2
(
void
)
const
{
return
_param2Fact
.
rawValue
().
toDouble
();
}
double
param3
(
void
)
const
{
return
_param3Fact
.
rawValue
().
toDouble
();
}
...
...
src/qgcunittest/PX4RCCalibrationTest.cc
View file @
d4d26f03
...
...
@@ -389,7 +389,7 @@ void RadioConfigTest::_fullCalibration_test(void)
switchList
<<
"RC_MAP_MODE_SW"
<<
"RC_MAP_LOITER_SW"
<<
"RC_MAP_RETURN_SW"
<<
"RC_MAP_POSCTL_SW"
<<
"RC_MAP_ACRO_SW"
;
foreach
(
QString
switchParam
,
switchList
)
{
Q_ASSERT
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
switchParam
)
->
v
alue
().
toInt
()
!=
channel
+
1
);
Q_ASSERT
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
switchParam
)
->
rawV
alue
().
toInt
()
!=
channel
+
1
);
}
_rgFunctionChannelMap
[
function
]
=
channel
;
...
...
@@ -401,7 +401,7 @@ void RadioConfigTest::_fullCalibration_test(void)
// If we aren't mapping this function during calibration, set it to the previous setting
if
(
!
found
)
{
_rgFunctionChannelMap
[
function
]
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
function
].
parameterName
)
->
v
alue
().
toInt
();
_rgFunctionChannelMap
[
function
]
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
function
].
parameterName
)
->
rawV
alue
().
toInt
();
qCDebug
(
RadioConfigTestLog
)
<<
"Assigning switch"
<<
function
<<
_rgFunctionChannelMap
[
function
];
if
(
_rgFunctionChannelMap
[
function
]
==
0
)
{
_rgFunctionChannelMap
[
function
]
=
-
1
;
// -1 signals no mapping
...
...
@@ -465,8 +465,8 @@ void RadioConfigTest::_validateParameters(void)
expectedParameterValue
=
chanIndex
+
1
;
// 1-based parameter value
}
qCDebug
(
RadioConfigTestLog
)
<<
"Validate"
<<
chanFunction
<<
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
chanFunction
].
parameterName
)
->
v
alue
().
toInt
();
QCOMPARE
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
chanFunction
].
parameterName
)
->
v
alue
().
toInt
(),
expectedParameterValue
);
qCDebug
(
RadioConfigTestLog
)
<<
"Validate"
<<
chanFunction
<<
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
chanFunction
].
parameterName
)
->
rawV
alue
().
toInt
();
QCOMPARE
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
chanFunction
].
parameterName
)
->
rawV
alue
().
toInt
(),
expectedParameterValue
);
}
// Validate the channel settings. Note the channels are 1-based in parameter names.
...
...
@@ -480,13 +480,13 @@ void RadioConfigTest::_validateParameters(void)
int
rcTrimExpected
=
_rgChannelSettingsValidate
[
chan
].
rcTrim
;
bool
rcReversedExpected
=
_rgChannelSettingsValidate
[
chan
].
reversed
;
int
rcMinActual
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
minTpl
.
arg
(
oneBasedChannel
))
->
v
alue
().
toInt
(
&
convertOk
);
int
rcMinActual
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
minTpl
.
arg
(
oneBasedChannel
))
->
rawV
alue
().
toInt
(
&
convertOk
);
QCOMPARE
(
convertOk
,
true
);
int
rcMaxActual
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
maxTpl
.
arg
(
oneBasedChannel
))
->
v
alue
().
toInt
(
&
convertOk
);
int
rcMaxActual
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
maxTpl
.
arg
(
oneBasedChannel
))
->
rawV
alue
().
toInt
(
&
convertOk
);
QCOMPARE
(
convertOk
,
true
);
int
rcTrimActual
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
trimTpl
.
arg
(
oneBasedChannel
))
->
v
alue
().
toInt
(
&
convertOk
);
int
rcTrimActual
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
trimTpl
.
arg
(
oneBasedChannel
))
->
rawV
alue
().
toInt
(
&
convertOk
);
QCOMPARE
(
convertOk
,
true
);
float
rcReversedFloat
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
revTpl
.
arg
(
oneBasedChannel
))
->
v
alue
().
toFloat
(
&
convertOk
);
float
rcReversedFloat
=
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
revTpl
.
arg
(
oneBasedChannel
))
->
rawV
alue
().
toFloat
(
&
convertOk
);
QCOMPARE
(
convertOk
,
true
);
bool
rcReversedActual
=
(
rcReversedFloat
==
-
1.0
f
);
...
...
@@ -508,6 +508,6 @@ void RadioConfigTest::_validateParameters(void)
expectedValue
=
_rgFunctionChannelMap
[
chanFunction
]
+
1
;
// 1-based
}
// qCDebug(RadioConfigTestLog) << chanFunction << expectedValue << mapParamsSet[RadioComponentController::_rgFunctionInfo[chanFunction].parameterName].toInt();
QCOMPARE
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
chanFunction
].
parameterName
)
->
v
alue
().
toInt
(),
expectedValue
);
QCOMPARE
(
_autopilot
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
RadioComponentController
::
_rgFunctionInfo
[
chanFunction
].
parameterName
)
->
rawV
alue
().
toInt
(),
expectedValue
);
}
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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