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
3ff23278
Commit
3ff23278
authored
Mar 03, 2016
by
Don Gagne
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #2921 from DonLakeFlyer/TalkingBattery
Talking battery, plus other stuff
parents
859f08c9
7285dfb5
Changes
20
Hide whitespace changes
Inline
Side-by-side
Showing
20 changed files
with
311 additions
and
292 deletions
+311
-292
qgroundcontrol.pro
qgroundcontrol.pro
+0
-4
FactTextField.qml
src/FactSystem/FactControls/FactTextField.qml
+1
-1
FactGroup.cc
src/FactSystem/FactGroup.cc
+15
-2
FactGroup.h
src/FactSystem/FactGroup.h
+3
-0
QGCInstrumentWidget.qml
src/FlightMap/Widgets/QGCInstrumentWidget.qml
+1
-2
ValuesWidget.qml
src/FlightMap/Widgets/ValuesWidget.qml
+1
-2
VibrationWidget.qml
src/FlightMap/Widgets/VibrationWidget.qml
+1
-2
GAudioOutput.cc
src/GAudioOutput.cc
+3
-4
GAudioOutput.h
src/GAudioOutput.h
+3
-3
QGroundControlQmlGlobal.h
src/QmlControls/QGroundControlQmlGlobal.h
+1
-1
BatteryFact.json
src/Vehicle/BatteryFact.json
+10
-0
MultiVehicleManager.cc
src/Vehicle/MultiVehicleManager.cc
+3
-0
MultiVehicleManager.h
src/Vehicle/MultiVehicleManager.h
+4
-0
Vehicle.cc
src/Vehicle/Vehicle.cc
+176
-49
Vehicle.h
src/Vehicle/Vehicle.h
+53
-29
QGCAudioWorker.cpp
src/audio/QGCAudioWorker.cpp
+1
-31
QGCAudioWorker.h
src/audio/QGCAudioWorker.h
+2
-11
UAS.cc
src/uas/UAS.cc
+5
-129
UAS.h
src/uas/UAS.h
+0
-22
GeneralSettings.qml
src/ui/preferences/GeneralSettings.qml
+28
-0
No files found.
qgroundcontrol.pro
View file @
3ff23278
...
@@ -92,10 +92,6 @@ QT += \
...
@@ -92,10 +92,6 @@ QT += \
bluetooth
\
bluetooth
\
}
}
contains
(
DEFINES
,
QGC_NOTIFY_TUNES_ENABLED
)
{
QT
+=
multimedia
}
#
testlib
is
needed
even
in
release
flavor
for
QSignalSpy
support
#
testlib
is
needed
even
in
release
flavor
for
QSignalSpy
support
QT
+=
testlib
QT
+=
testlib
...
...
src/FactSystem/FactControls/FactTextField.qml
View file @
3ff23278
...
@@ -23,7 +23,7 @@ QGCTextField {
...
@@ -23,7 +23,7 @@ QGCTextField {
inputMethodHints
:
Qt
.
ImhFormattedNumbersOnly
inputMethodHints
:
Qt
.
ImhFormattedNumbersOnly
onEditingFinished
:
{
onEditingFinished
:
{
if
(
qgcView
)
{
if
(
typeof
qgcView
!==
'
undefined
'
&&
qgcView
)
{
var
errorString
=
fact
.
validate
(
text
,
false
/* convertOnly */
)
var
errorString
=
fact
.
validate
(
text
,
false
/* convertOnly */
)
if
(
errorString
==
""
)
{
if
(
errorString
==
""
)
{
fact
.
value
=
text
fact
.
value
=
text
...
...
src/FactSystem/FactGroup.cc
View file @
3ff23278
...
@@ -40,6 +40,9 @@ const char* FactGroup::_versionJsonKey = "version";
...
@@ -40,6 +40,9 @@ const char* FactGroup::_versionJsonKey = "version";
const
char
*
FactGroup
::
_typeJsonKey
=
"type"
;
const
char
*
FactGroup
::
_typeJsonKey
=
"type"
;
const
char
*
FactGroup
::
_shortDescriptionJsonKey
=
"shortDescription"
;
const
char
*
FactGroup
::
_shortDescriptionJsonKey
=
"shortDescription"
;
const
char
*
FactGroup
::
_unitsJsonKey
=
"units"
;
const
char
*
FactGroup
::
_unitsJsonKey
=
"units"
;
const
char
*
FactGroup
::
_defaultValueJsonKey
=
"defaultValue"
;
const
char
*
FactGroup
::
_minJsonKey
=
"min"
;
const
char
*
FactGroup
::
_maxJsonKey
=
"max"
;
FactGroup
::
FactGroup
(
int
updateRateMsecs
,
const
QString
&
metaDataFile
,
QObject
*
parent
)
FactGroup
::
FactGroup
(
int
updateRateMsecs
,
const
QString
&
metaDataFile
,
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
)
...
@@ -187,8 +190,8 @@ void FactGroup::_loadMetaData(const QString& jsonFilename)
...
@@ -187,8 +190,8 @@ void FactGroup::_loadMetaData(const QString& jsonFilename)
QStringList
keys
;
QStringList
keys
;
QList
<
QJsonValue
::
Type
>
types
;
QList
<
QJsonValue
::
Type
>
types
;
keys
<<
_nameJsonKey
<<
_decimalPlacesJsonKey
;
keys
<<
_nameJsonKey
<<
_decimalPlacesJsonKey
<<
_typeJsonKey
<<
_shortDescriptionJsonKey
<<
_unitsJsonKey
<<
_defaultValueJsonKey
<<
_minJsonKey
<<
_maxJsonKey
;
types
<<
QJsonValue
::
String
<<
QJsonValue
::
Double
;
types
<<
QJsonValue
::
String
<<
QJsonValue
::
Double
<<
QJsonValue
::
String
<<
QJsonValue
::
String
<<
QJsonValue
::
String
<<
QJsonValue
::
Double
<<
QJsonValue
::
Double
<<
QJsonValue
::
Double
;
if
(
!
JsonHelper
::
validateKeyTypes
(
jsonObject
,
keys
,
types
,
errorString
))
{
if
(
!
JsonHelper
::
validateKeyTypes
(
jsonObject
,
keys
,
types
,
errorString
))
{
qWarning
()
<<
errorString
;
qWarning
()
<<
errorString
;
return
;
return
;
...
@@ -219,6 +222,16 @@ void FactGroup::_loadMetaData(const QString& jsonFilename)
...
@@ -219,6 +222,16 @@ void FactGroup::_loadMetaData(const QString& jsonFilename)
metaData
->
setShortDescription
(
jsonObject
.
value
(
_shortDescriptionJsonKey
).
toString
());
metaData
->
setShortDescription
(
jsonObject
.
value
(
_shortDescriptionJsonKey
).
toString
());
metaData
->
setRawUnits
(
jsonObject
.
value
(
_unitsJsonKey
).
toString
());
metaData
->
setRawUnits
(
jsonObject
.
value
(
_unitsJsonKey
).
toString
());
if
(
jsonObject
.
contains
(
_defaultValueJsonKey
))
{
metaData
->
setRawDefaultValue
(
jsonObject
.
value
(
_defaultValueJsonKey
).
toDouble
());
}
if
(
jsonObject
.
contains
(
_minJsonKey
))
{
metaData
->
setRawMin
(
jsonObject
.
value
(
_minJsonKey
).
toDouble
());
}
if
(
jsonObject
.
contains
(
_maxJsonKey
))
{
metaData
->
setRawMax
(
jsonObject
.
value
(
_maxJsonKey
).
toDouble
());
}
for
(
int
i
=
0
;
i
<
enumValues
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
enumValues
.
count
();
i
++
)
{
QVariant
enumVariant
;
QVariant
enumVariant
;
QString
errorString
;
QString
errorString
;
...
...
src/FactSystem/FactGroup.h
View file @
3ff23278
...
@@ -78,6 +78,9 @@ private:
...
@@ -78,6 +78,9 @@ private:
static
const
char
*
_versionJsonKey
;
static
const
char
*
_versionJsonKey
;
static
const
char
*
_shortDescriptionJsonKey
;
static
const
char
*
_shortDescriptionJsonKey
;
static
const
char
*
_unitsJsonKey
;
static
const
char
*
_unitsJsonKey
;
static
const
char
*
_defaultValueJsonKey
;
static
const
char
*
_minJsonKey
;
static
const
char
*
_maxJsonKey
;
};
};
#endif
#endif
src/FlightMap/Widgets/QGCInstrumentWidget.qml
View file @
3ff23278
...
@@ -131,7 +131,7 @@ Item {
...
@@ -131,7 +131,7 @@ Item {
Rectangle
{
Rectangle
{
anchors.fill
:
_valuesWidget
anchors.fill
:
_valuesWidget
color
:
_backgroundColor
color
:
_backgroundColor
visible
:
!
_showCompass
&&
_activeVehicle
visible
:
!
_showCompass
radius
:
_spacing
radius
:
_spacing
}
}
...
@@ -145,7 +145,6 @@ Item {
...
@@ -145,7 +145,6 @@ Item {
}
}
}
}
Rectangle
{
Rectangle
{
id
:
_spacer2
id
:
_spacer2
height
:
1
height
:
1
...
...
src/FlightMap/Widgets/ValuesWidget.qml
View file @
3ff23278
...
@@ -33,7 +33,6 @@ import QGroundControl 1.0
...
@@ -33,7 +33,6 @@ import QGroundControl 1.0
QGCFlickable
{
QGCFlickable
{
id
:
_root
id
:
_root
visible
:
_activeVehicle
height
:
Math
.
min
(
maxHeight
,
_smallFlow
.
y
+
_smallFlow
.
height
)
height
:
Math
.
min
(
maxHeight
,
_smallFlow
.
y
+
_smallFlow
.
height
)
contentHeight
:
_smallFlow
.
y
+
_smallFlow
.
height
contentHeight
:
_smallFlow
.
y
+
_smallFlow
.
height
flickableDirection
:
Flickable
.
VerticalFlick
flickableDirection
:
Flickable
.
VerticalFlick
...
@@ -43,7 +42,7 @@ QGCFlickable {
...
@@ -43,7 +42,7 @@ QGCFlickable {
property
color
textColor
property
color
textColor
property
var
maxHeight
property
var
maxHeight
property
var
_activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
property
var
_activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
?
QGroundControl
.
multiVehicleManager
.
activeVehicle
:
QGroundControl
.
multiVehicleManager
.
disconnectedVehicle
property
real
_margins
:
ScreenTools
.
defaultFontPixelWidth
/
2
property
real
_margins
:
ScreenTools
.
defaultFontPixelWidth
/
2
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
...
...
src/FlightMap/Widgets/VibrationWidget.qml
View file @
3ff23278
...
@@ -33,7 +33,6 @@ import QGroundControl 1.0
...
@@ -33,7 +33,6 @@ import QGroundControl 1.0
QGCFlickable
{
QGCFlickable
{
id
:
_root
id
:
_root
visible
:
_activeVehicle
height
:
Math
.
min
(
maxHeight
,
innerItem
.
height
)
height
:
Math
.
min
(
maxHeight
,
innerItem
.
height
)
contentHeight
:
innerItem
.
height
contentHeight
:
innerItem
.
height
flickableDirection
:
Flickable
.
VerticalFlick
flickableDirection
:
Flickable
.
VerticalFlick
...
@@ -43,7 +42,7 @@ QGCFlickable {
...
@@ -43,7 +42,7 @@ QGCFlickable {
property
color
backgroundColor
property
color
backgroundColor
property
var
maxHeight
property
var
maxHeight
property
var
_activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
property
var
_activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
?
QGroundControl
.
multiVehicleManager
.
activeVehicle
:
QGroundControl
.
multiVehicleManager
.
disconnectedVehicle
property
real
_margins
:
ScreenTools
.
defaultFontPixelWidth
/
2
property
real
_margins
:
ScreenTools
.
defaultFontPixelWidth
/
2
property
real
_barWidth
:
Math
.
round
(
ScreenTools
.
defaultFontPixelWidth
*
3
)
property
real
_barWidth
:
Math
.
round
(
ScreenTools
.
defaultFontPixelWidth
*
3
)
...
...
src/GAudioOutput.cc
View file @
3ff23278
...
@@ -88,12 +88,11 @@ bool GAudioOutput::isMuted()
...
@@ -88,12 +88,11 @@ bool GAudioOutput::isMuted()
return
muted
;
return
muted
;
}
}
bool
GAudioOutput
::
say
(
const
QString
&
inText
,
int
severity
)
bool
GAudioOutput
::
say
(
const
QString
&
inText
)
{
{
if
(
!
muted
)
{
if
(
!
muted
&&
!
qgcApp
()
->
runningUnitTests
()
)
{
#if defined __android__
#if defined __android__
#if defined QGC_SPEECH_ENABLED
#if defined QGC_SPEECH_ENABLED
Q_UNUSED
(
severity
);
static
const
char
V_jniClassName
[]
{
"org/qgroundcontrol/qgchelper/UsbDeviceJNI"
};
static
const
char
V_jniClassName
[]
{
"org/qgroundcontrol/qgchelper/UsbDeviceJNI"
};
QAndroidJniEnvironment
env
;
QAndroidJniEnvironment
env
;
if
(
env
->
ExceptionCheck
())
{
if
(
env
->
ExceptionCheck
())
{
...
@@ -105,7 +104,7 @@ bool GAudioOutput::say(const QString& inText, int severity)
...
@@ -105,7 +104,7 @@ bool GAudioOutput::say(const QString& inText, int severity)
QAndroidJniObject
::
callStaticMethod
<
void
>
(
V_jniClassName
,
"say"
,
"(Ljava/lang/String;)V"
,
javaMessage
.
object
<
jstring
>
());
QAndroidJniObject
::
callStaticMethod
<
void
>
(
V_jniClassName
,
"say"
,
"(Ljava/lang/String;)V"
,
javaMessage
.
object
<
jstring
>
());
#endif
#endif
#else
#else
emit
textToSpeak
(
inText
,
severity
);
emit
textToSpeak
(
inText
);
#endif
#endif
}
}
return
true
;
return
true
;
...
...
src/GAudioOutput.h
View file @
3ff23278
...
@@ -79,14 +79,14 @@ public:
...
@@ -79,14 +79,14 @@ public:
bool
isMuted
();
bool
isMuted
();
public
slots
:
public
slots
:
/** @brief Say this text
if current output priority matches
*/
/** @brief Say this text */
bool
say
(
const
QString
&
text
,
int
severity
=
6
);
bool
say
(
const
QString
&
text
);
/** @brief Mute/unmute sound */
/** @brief Mute/unmute sound */
void
mute
(
bool
mute
);
void
mute
(
bool
mute
);
signals:
signals:
void
mutedChanged
(
bool
);
void
mutedChanged
(
bool
);
bool
textToSpeak
(
QString
text
,
int
severity
=
1
);
bool
textToSpeak
(
QString
text
);
void
beepOnce
();
void
beepOnce
();
protected:
protected:
...
...
src/QmlControls/QGroundControlQmlGlobal.h
View file @
3ff23278
...
@@ -55,7 +55,7 @@ public:
...
@@ -55,7 +55,7 @@ public:
Q_PROPERTY
(
LinkManager
*
linkManager
READ
linkManager
CONSTANT
)
Q_PROPERTY
(
LinkManager
*
linkManager
READ
linkManager
CONSTANT
)
Q_PROPERTY
(
MissionCommands
*
missionCommands
READ
missionCommands
CONSTANT
)
Q_PROPERTY
(
MissionCommands
*
missionCommands
READ
missionCommands
CONSTANT
)
Q_PROPERTY
(
MultiVehicleManager
*
multiVehicleManager
READ
multiVehicleManager
CONSTANT
)
Q_PROPERTY
(
MultiVehicleManager
*
multiVehicleManager
READ
multiVehicleManager
CONSTANT
)
Q_PROPERTY
(
QGCMapEngineManager
*
mapEngineManager
READ
mapEngineManager
CONSTANT
)
Q_PROPERTY
(
QGCMapEngineManager
*
mapEngineManager
READ
mapEngineManager
CONSTANT
)
Q_PROPERTY
(
qreal
zOrderTopMost
READ
zOrderTopMost
CONSTANT
)
///< z order for top most items, toolbar, main window sub view
Q_PROPERTY
(
qreal
zOrderTopMost
READ
zOrderTopMost
CONSTANT
)
///< z order for top most items, toolbar, main window sub view
Q_PROPERTY
(
qreal
zOrderWidgets
READ
zOrderWidgets
CONSTANT
)
///< z order value to widgets, for example: zoom controls, hud widgetss
Q_PROPERTY
(
qreal
zOrderWidgets
READ
zOrderWidgets
CONSTANT
)
///< z order value to widgets, for example: zoom controls, hud widgetss
...
...
src/Vehicle/BatteryFact.json
View file @
3ff23278
...
@@ -16,6 +16,16 @@
...
@@ -16,6 +16,16 @@
"decimalPlaces"
:
0
,
"decimalPlaces"
:
0
,
"units"
:
"%"
"units"
:
"%"
},
},
{
"name"
:
"percentRemainingAnnounce"
,
"shortDescription"
:
"Percent"
,
"type"
:
"int32"
,
"decimalPlaces"
:
0
,
"units"
:
"%"
,
"defaultValue"
:
30
,
"min"
:
0
,
"max"
:
100
},
{
{
"name"
:
"mahConsumed"
,
"name"
:
"mahConsumed"
,
"shortDescription"
:
"Consumed"
,
"shortDescription"
:
"Consumed"
,
...
...
src/Vehicle/MultiVehicleManager.cc
View file @
3ff23278
...
@@ -50,6 +50,7 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app)
...
@@ -50,6 +50,7 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app)
,
_activeVehicleAvailable
(
false
)
,
_activeVehicleAvailable
(
false
)
,
_parameterReadyVehicleAvailable
(
false
)
,
_parameterReadyVehicleAvailable
(
false
)
,
_activeVehicle
(
NULL
)
,
_activeVehicle
(
NULL
)
,
_disconnectedVehicle
(
NULL
)
,
_firmwarePluginManager
(
NULL
)
,
_firmwarePluginManager
(
NULL
)
,
_autopilotPluginManager
(
NULL
)
,
_autopilotPluginManager
(
NULL
)
,
_joystickManager
(
NULL
)
,
_joystickManager
(
NULL
)
...
@@ -66,6 +67,8 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app)
...
@@ -66,6 +67,8 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app)
if
(
_gcsHeartbeatEnabled
)
{
if
(
_gcsHeartbeatEnabled
)
{
_gcsHeartbeatTimer
.
start
();
_gcsHeartbeatTimer
.
start
();
}
}
_disconnectedVehicle
=
new
Vehicle
(
this
);
}
}
void
MultiVehicleManager
::
setToolbox
(
QGCToolbox
*
toolbox
)
void
MultiVehicleManager
::
setToolbox
(
QGCToolbox
*
toolbox
)
...
...
src/Vehicle/MultiVehicleManager.h
View file @
3ff23278
...
@@ -57,6 +57,9 @@ public:
...
@@ -57,6 +57,9 @@ public:
Q_PROPERTY
(
QmlObjectListModel
*
vehicles
READ
vehicles
CONSTANT
)
Q_PROPERTY
(
QmlObjectListModel
*
vehicles
READ
vehicles
CONSTANT
)
Q_PROPERTY
(
bool
gcsHeartBeatEnabled
READ
gcsHeartbeatEnabled
WRITE
setGcsHeartbeatEnabled
NOTIFY
gcsHeartBeatEnabledChanged
)
Q_PROPERTY
(
bool
gcsHeartBeatEnabled
READ
gcsHeartbeatEnabled
WRITE
setGcsHeartbeatEnabled
NOTIFY
gcsHeartBeatEnabledChanged
)
/// A disconnected vehicle is used to access FactGroup information for the Vehicle object when no active vehicle is available
Q_PROPERTY
(
Vehicle
*
disconnectedVehicle
MEMBER
_disconnectedVehicle
CONSTANT
)
// Methods
// Methods
Q_INVOKABLE
Vehicle
*
getVehicleById
(
int
vehicleId
);
Q_INVOKABLE
Vehicle
*
getVehicleById
(
int
vehicleId
);
...
@@ -104,6 +107,7 @@ private:
...
@@ -104,6 +107,7 @@ private:
bool
_activeVehicleAvailable
;
///< true: An active vehicle is available
bool
_activeVehicleAvailable
;
///< true: An active vehicle is available
bool
_parameterReadyVehicleAvailable
;
///< true: An active vehicle with ready parameters is available
bool
_parameterReadyVehicleAvailable
;
///< true: An active vehicle with ready parameters is available
Vehicle
*
_activeVehicle
;
///< Currently active vehicle from a ui perspective
Vehicle
*
_activeVehicle
;
///< Currently active vehicle from a ui perspective
Vehicle
*
_disconnectedVehicle
;
///< Disconnected vechicle for FactGroup access
QList
<
Vehicle
*>
_vehiclesBeingDeleted
;
///< List of Vehicles being deleted in queued phases
QList
<
Vehicle
*>
_vehiclesBeingDeleted
;
///< List of Vehicles being deleted in queued phases
Vehicle
*
_vehicleBeingSetActive
;
///< Vehicle being set active in queued phases
Vehicle
*
_vehicleBeingSetActive
;
///< Vehicle being set active in queued phases
...
...
src/Vehicle/Vehicle.cc
View file @
3ff23278
...
@@ -60,36 +60,7 @@ const char* Vehicle::_batteryFactGroupName = "battery";
...
@@ -60,36 +60,7 @@ const char* Vehicle::_batteryFactGroupName = "battery";
const
char
*
Vehicle
::
_windFactGroupName
=
"wind"
;
const
char
*
Vehicle
::
_windFactGroupName
=
"wind"
;
const
char
*
Vehicle
::
_vibrationFactGroupName
=
"vibration"
;
const
char
*
Vehicle
::
_vibrationFactGroupName
=
"vibration"
;
const
char
*
VehicleGPSFactGroup
::
_hdopFactName
=
"hdop"
;
const
int
Vehicle
::
_lowBatteryAnnounceRepeatMSecs
=
30
*
1000
;
const
char
*
VehicleGPSFactGroup
::
_vdopFactName
=
"vdop"
;
const
char
*
VehicleGPSFactGroup
::
_courseOverGroundFactName
=
"courseOverGround"
;
const
char
*
VehicleGPSFactGroup
::
_countFactName
=
"count"
;
const
char
*
VehicleGPSFactGroup
::
_lockFactName
=
"lock"
;
const
char
*
VehicleBatteryFactGroup
::
_voltageFactName
=
"voltage"
;
const
char
*
VehicleBatteryFactGroup
::
_percentRemainingFactName
=
"percentRemaining"
;
const
char
*
VehicleBatteryFactGroup
::
_mahConsumedFactName
=
"mahConsumed"
;
const
char
*
VehicleBatteryFactGroup
::
_currentFactName
=
"current"
;
const
char
*
VehicleBatteryFactGroup
::
_temperatureFactName
=
"temperature"
;
const
char
*
VehicleBatteryFactGroup
::
_cellCountFactName
=
"cellCount"
;
const
double
VehicleBatteryFactGroup
::
_voltageUnavailable
=
-
1.0
;
const
int
VehicleBatteryFactGroup
::
_percentRemainingUnavailable
=
-
1
;
const
int
VehicleBatteryFactGroup
::
_mahConsumedUnavailable
=
-
1
;
const
int
VehicleBatteryFactGroup
::
_currentUnavailable
=
-
1
;
const
double
VehicleBatteryFactGroup
::
_temperatureUnavailable
=
-
1.0
;
const
int
VehicleBatteryFactGroup
::
_cellCountUnavailable
=
-
1.0
;
const
char
*
VehicleWindFactGroup
::
_directionFactName
=
"direction"
;
const
char
*
VehicleWindFactGroup
::
_speedFactName
=
"speed"
;
const
char
*
VehicleWindFactGroup
::
_verticalSpeedFactName
=
"verticalSpeed"
;
const
char
*
VehicleVibrationFactGroup
::
_xAxisFactName
=
"xAxis"
;
const
char
*
VehicleVibrationFactGroup
::
_yAxisFactName
=
"yAxis"
;
const
char
*
VehicleVibrationFactGroup
::
_zAxisFactName
=
"zAxis"
;
const
char
*
VehicleVibrationFactGroup
::
_clipCount1FactName
=
"clipCount1"
;
const
char
*
VehicleVibrationFactGroup
::
_clipCount2FactName
=
"clipCount2"
;
const
char
*
VehicleVibrationFactGroup
::
_clipCount3FactName
=
"clipCount3"
;
Vehicle
::
Vehicle
(
LinkInterface
*
link
,
Vehicle
::
Vehicle
(
LinkInterface
*
link
,
int
vehicleId
,
int
vehicleId
,
...
@@ -101,6 +72,7 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -101,6 +72,7 @@ Vehicle::Vehicle(LinkInterface* link,
:
FactGroup
(
_vehicleUIUpdateRateMSecs
,
":/json/Vehicle/VehicleFact.json"
)
:
FactGroup
(
_vehicleUIUpdateRateMSecs
,
":/json/Vehicle/VehicleFact.json"
)
,
_id
(
vehicleId
)
,
_id
(
vehicleId
)
,
_active
(
false
)
,
_active
(
false
)
,
_disconnectedVehicle
(
false
)
,
_firmwareType
(
firmwareType
)
,
_firmwareType
(
firmwareType
)
,
_vehicleType
(
vehicleType
)
,
_vehicleType
(
vehicleType
)
,
_firmwarePlugin
(
NULL
)
,
_firmwarePlugin
(
NULL
)
...
@@ -165,8 +137,10 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -165,8 +137,10 @@ Vehicle::Vehicle(LinkInterface* link,
_mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
_mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
connect
(
_mavlink
,
&
MAVLinkProtocol
::
messageReceived
,
this
,
&
Vehicle
::
_mavlinkMessageReceived
);
connect
(
_mavlink
,
&
MAVLinkProtocol
::
messageReceived
,
this
,
&
Vehicle
::
_mavlinkMessageReceived
);
connect
(
this
,
&
Vehicle
::
_sendMessageOnThread
,
this
,
&
Vehicle
::
_sendMessage
,
Qt
::
QueuedConnection
);
connect
(
this
,
&
Vehicle
::
_sendMessageOnThread
,
this
,
&
Vehicle
::
_sendMessage
,
Qt
::
QueuedConnection
);
connect
(
this
,
&
Vehicle
::
_sendMessageOnLinkOnThread
,
this
,
&
Vehicle
::
_sendMessageOnLink
,
Qt
::
QueuedConnection
);
connect
(
this
,
&
Vehicle
::
_sendMessageOnLinkOnThread
,
this
,
&
Vehicle
::
_sendMessageOnLink
,
Qt
::
QueuedConnection
);
connect
(
this
,
&
Vehicle
::
flightModeChanged
,
this
,
&
Vehicle
::
_announceflightModeChanged
);
_uas
=
new
UAS
(
_mavlink
,
this
,
_firmwarePluginManager
);
_uas
=
new
UAS
(
_mavlink
,
this
,
_firmwarePluginManager
);
...
@@ -227,6 +201,9 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -227,6 +201,9 @@ Vehicle::Vehicle(LinkInterface* link,
_mapTrajectoryTimer
.
setInterval
(
_mapTrajectoryMsecsBetweenPoints
);
_mapTrajectoryTimer
.
setInterval
(
_mapTrajectoryMsecsBetweenPoints
);
connect
(
&
_mapTrajectoryTimer
,
&
QTimer
::
timeout
,
this
,
&
Vehicle
::
_addNewMapTrajectoryPoint
);
connect
(
&
_mapTrajectoryTimer
,
&
QTimer
::
timeout
,
this
,
&
Vehicle
::
_addNewMapTrajectoryPoint
);
// Invalidate the timer to signal first announce
_lowBatteryAnnounceTimer
.
invalidate
();
// Build FactGroup object model
// Build FactGroup object model
_addFact
(
&
_rollFact
,
_rollFactName
);
_addFact
(
&
_rollFact
,
_rollFactName
);
...
@@ -249,6 +226,93 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -249,6 +226,93 @@ Vehicle::Vehicle(LinkInterface* link,
_vibrationFactGroup
.
setVehicle
(
this
);
_vibrationFactGroup
.
setVehicle
(
this
);
}
}
// Disconnected Vehicle
Vehicle
::
Vehicle
(
QObject
*
parent
)
:
FactGroup
(
_vehicleUIUpdateRateMSecs
,
":/json/Vehicle/VehicleFact.json"
,
parent
)
,
_id
(
0
)
,
_active
(
false
)
,
_disconnectedVehicle
(
false
)
,
_firmwareType
(
MAV_AUTOPILOT_PX4
)
,
_vehicleType
(
MAV_TYPE_QUADROTOR
)
,
_firmwarePlugin
(
NULL
)
,
_autopilotPlugin
(
NULL
)
,
_joystickMode
(
JoystickModeRC
)
,
_joystickEnabled
(
false
)
,
_uas
(
NULL
)
,
_coordinate
(
37.803784
,
-
122.462276
)
,
_coordinateValid
(
false
)
,
_homePositionAvailable
(
false
)
,
_mav
(
NULL
)
,
_currentMessageCount
(
0
)
,
_messageCount
(
0
)
,
_currentErrorCount
(
0
)
,
_currentWarningCount
(
0
)
,
_currentNormalCount
(
0
)
,
_currentMessageType
(
MessageNone
)
,
_navigationAltitudeError
(
0.0
f
)
,
_navigationSpeedError
(
0.0
f
)
,
_navigationCrosstrackError
(
0.0
f
)
,
_navigationTargetBearing
(
0.0
f
)
,
_refreshTimer
(
new
QTimer
(
this
))
,
_updateCount
(
0
)
,
_rcRSSI
(
0
)
,
_rcRSSIstore
(
100.0
)
,
_autoDisconnect
(
false
)
,
_connectionLost
(
false
)
,
_connectionLostEnabled
(
true
)
,
_missionManager
(
NULL
)
,
_missionManagerInitialRequestComplete
(
false
)
,
_parameterLoader
(
NULL
)
,
_armed
(
false
)
,
_base_mode
(
0
)
,
_custom_mode
(
0
)
,
_nextSendMessageMultipleIndex
(
0
)
,
_firmwarePluginManager
(
NULL
)
,
_autopilotPluginManager
(
NULL
)
,
_joystickManager
(
NULL
)
,
_flowImageIndex
(
0
)
,
_allLinksInactiveSent
(
false
)
,
_messagesReceived
(
0
)
,
_messagesSent
(
0
)
,
_messagesLost
(
0
)
,
_messageSeq
(
0
)
,
_compID
(
0
)
,
_heardFrom
(
false
)
,
_rollFact
(
0
,
_rollFactName
,
FactMetaData
::
valueTypeDouble
)
,
_pitchFact
(
0
,
_pitchFactName
,
FactMetaData
::
valueTypeDouble
)
,
_headingFact
(
0
,
_headingFactName
,
FactMetaData
::
valueTypeDouble
)
,
_groundSpeedFact
(
0
,
_groundSpeedFactName
,
FactMetaData
::
valueTypeDouble
)
,
_airSpeedFact
(
0
,
_airSpeedFactName
,
FactMetaData
::
valueTypeDouble
)
,
_climbRateFact
(
0
,
_climbRateFactName
,
FactMetaData
::
valueTypeDouble
)
,
_altitudeRelativeFact
(
0
,
_altitudeRelativeFactName
,
FactMetaData
::
valueTypeDouble
)
,
_altitudeAMSLFact
(
0
,
_altitudeAMSLFactName
,
FactMetaData
::
valueTypeDouble
)
,
_gpsFactGroup
(
this
)
,
_batteryFactGroup
(
this
)
,
_windFactGroup
(
this
)
,
_vibrationFactGroup
(
this
)
{
// Build FactGroup object model
_addFact
(
&
_rollFact
,
_rollFactName
);
_addFact
(
&
_pitchFact
,
_pitchFactName
);
_addFact
(
&
_headingFact
,
_headingFactName
);
_addFact
(
&
_groundSpeedFact
,
_groundSpeedFactName
);
_addFact
(
&
_airSpeedFact
,
_airSpeedFactName
);
_addFact
(
&
_climbRateFact
,
_climbRateFactName
);
_addFact
(
&
_altitudeRelativeFact
,
_altitudeRelativeFactName
);
_addFact
(
&
_altitudeAMSLFact
,
_altitudeAMSLFactName
);
_addFactGroup
(
&
_gpsFactGroup
,
_gpsFactGroupName
);
_addFactGroup
(
&
_batteryFactGroup
,
_batteryFactGroupName
);
_addFactGroup
(
&
_windFactGroup
,
_windFactGroupName
);
_addFactGroup
(
&
_vibrationFactGroup
,
_vibrationFactGroupName
);
_gpsFactGroup
.
setVehicle
(
NULL
);
_batteryFactGroup
.
setVehicle
(
NULL
);
_windFactGroup
.
setVehicle
(
NULL
);
_vibrationFactGroup
.
setVehicle
(
NULL
);
}
Vehicle
::~
Vehicle
()
Vehicle
::~
Vehicle
()
{
{
qCDebug
(
VehicleLog
)
<<
"~Vehicle"
<<
this
;
qCDebug
(
VehicleLog
)
<<
"~Vehicle"
<<
this
;
...
@@ -399,6 +463,13 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message)
...
@@ -399,6 +463,13 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message)
_batteryFactGroup
.
voltage
()
->
setRawValue
((
double
)
sysStatus
.
voltage_battery
/
1000.0
);
_batteryFactGroup
.
voltage
()
->
setRawValue
((
double
)
sysStatus
.
voltage_battery
/
1000.0
);
}
}
_batteryFactGroup
.
percentRemaining
()
->
setRawValue
(
sysStatus
.
battery_remaining
);
_batteryFactGroup
.
percentRemaining
()
->
setRawValue
(
sysStatus
.
battery_remaining
);
if
(
sysStatus
.
battery_remaining
>
0
&&
sysStatus
.
battery_remaining
<
_batteryFactGroup
.
percentRemainingAnnounce
()
->
rawValue
().
toInt
())
{
if
(
!
_lowBatteryAnnounceTimer
.
isValid
()
||
_lowBatteryAnnounceTimer
.
elapsed
()
>
_lowBatteryAnnounceRepeatMSecs
)
{
_lowBatteryAnnounceTimer
.
restart
();
_say
(
QString
(
"Low battery on %1: %2 percent remaining"
).
arg
(
_vehicleIdSpeech
()).
arg
(
sysStatus
.
battery_remaining
));
}
}
}
}
void
Vehicle
::
_handleBatteryStatus
(
mavlink_message_t
&
message
)
void
Vehicle
::
_handleBatteryStatus
(
mavlink_message_t
&
message
)
...
@@ -1217,7 +1288,7 @@ void Vehicle::_connectionLostTimeout(void)
...
@@ -1217,7 +1288,7 @@ void Vehicle::_connectionLostTimeout(void)
_connectionLost
=
true
;
_connectionLost
=
true
;
_heardFrom
=
false
;
_heardFrom
=
false
;
emit
connectionLostChanged
(
true
);
emit
connectionLostChanged
(
true
);
_say
(
QString
(
"co
nnection lost to vehicle %1"
).
arg
(
id
()),
GAudioOutput
::
AUDIO_SEVERITY_NOTICE
);
_say
(
QString
(
"co
mmunication lost to %1"
).
arg
(
_vehicleIdSpeech
())
);
if
(
_autoDisconnect
)
{
if
(
_autoDisconnect
)
{
disconnectInactiveVehicle
();
disconnectInactiveVehicle
();
}
}
...
@@ -1230,14 +1301,13 @@ void Vehicle::_connectionActive(void)
...
@@ -1230,14 +1301,13 @@ void Vehicle::_connectionActive(void)
if
(
_connectionLost
)
{
if
(
_connectionLost
)
{
_connectionLost
=
false
;
_connectionLost
=
false
;
emit
connectionLostChanged
(
false
);
emit
connectionLostChanged
(
false
);
_say
(
QString
(
"co
nnection regained to vehicle %1"
).
arg
(
id
()),
GAudioOutput
::
AUDIO_SEVERITY_NOTICE
);
_say
(
QString
(
"co
mmunication regained to %1"
).
arg
(
_vehicleIdSpeech
())
);
}
}
}
}
void
Vehicle
::
_say
(
const
QString
&
text
,
int
severity
)
void
Vehicle
::
_say
(
const
QString
&
text
)
{
{
if
(
!
qgcApp
()
->
runningUnitTests
())
qgcApp
()
->
toolbox
()
->
audioOutput
()
->
say
(
text
.
toLower
());
qgcApp
()
->
toolbox
()
->
audioOutput
()
->
say
(
text
.
toLower
(),
severity
);
}
}
bool
Vehicle
::
fixedWing
(
void
)
const
bool
Vehicle
::
fixedWing
(
void
)
const
...
@@ -1268,6 +1338,26 @@ void Vehicle::_setCoordinateValid(bool coordinateValid)
...
@@ -1268,6 +1338,26 @@ void Vehicle::_setCoordinateValid(bool coordinateValid)
}
}
}
}
/// Returns the string to speak to identify the vehicle
QString
Vehicle
::
_vehicleIdSpeech
(
void
)
{
if
(
qgcApp
()
->
toolbox
()
->
multiVehicleManager
()
->
vehicles
()
->
count
()
>
1
)
{
return
QString
(
"vehicle %1"
).
arg
(
id
());
}
else
{
return
QStringLiteral
(
"vehicle"
);
}
}
void
Vehicle
::
_announceflightModeChanged
(
const
QString
&
flightMode
)
{
_say
(
QString
(
"%1 is now in %2 flight mode"
).
arg
(
_vehicleIdSpeech
()).
arg
(
flightMode
));
}
const
char
*
VehicleGPSFactGroup
::
_hdopFactName
=
"hdop"
;
const
char
*
VehicleGPSFactGroup
::
_vdopFactName
=
"vdop"
;
const
char
*
VehicleGPSFactGroup
::
_courseOverGroundFactName
=
"courseOverGround"
;
const
char
*
VehicleGPSFactGroup
::
_countFactName
=
"count"
;
const
char
*
VehicleGPSFactGroup
::
_lockFactName
=
"lock"
;
VehicleGPSFactGroup
::
VehicleGPSFactGroup
(
QObject
*
parent
)
VehicleGPSFactGroup
::
VehicleGPSFactGroup
(
QObject
*
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/GPSFact.json"
,
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/GPSFact.json"
,
parent
)
...
@@ -1293,6 +1383,11 @@ void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle)
...
@@ -1293,6 +1383,11 @@ void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle)
{
{
_vehicle
=
vehicle
;
_vehicle
=
vehicle
;
if
(
!
vehicle
)
{
// Disconnected Vehicle
return
;
}
connect
(
_vehicle
->
uas
(),
&
UASInterface
::
localizationChanged
,
this
,
&
VehicleGPSFactGroup
::
_setSatLoc
);
connect
(
_vehicle
->
uas
(),
&
UASInterface
::
localizationChanged
,
this
,
&
VehicleGPSFactGroup
::
_setSatLoc
);
UAS
*
pUas
=
dynamic_cast
<
UAS
*>
(
_vehicle
->
uas
());
UAS
*
pUas
=
dynamic_cast
<
UAS
*>
(
_vehicle
->
uas
());
...
@@ -1336,22 +1431,43 @@ void VehicleGPSFactGroup::_setSatLoc(UASInterface*, int fix)
...
@@ -1336,22 +1431,43 @@ void VehicleGPSFactGroup::_setSatLoc(UASInterface*, int fix)
}
}
}
}
const
char
*
VehicleBatteryFactGroup
::
_voltageFactName
=
"voltage"
;
const
char
*
VehicleBatteryFactGroup
::
_percentRemainingFactName
=
"percentRemaining"
;
const
char
*
VehicleBatteryFactGroup
::
_percentRemainingAnnounceFactName
=
"percentRemainingAnnounce"
;
const
char
*
VehicleBatteryFactGroup
::
_mahConsumedFactName
=
"mahConsumed"
;
const
char
*
VehicleBatteryFactGroup
::
_currentFactName
=
"current"
;
const
char
*
VehicleBatteryFactGroup
::
_temperatureFactName
=
"temperature"
;
const
char
*
VehicleBatteryFactGroup
::
_cellCountFactName
=
"cellCount"
;
const
char
*
VehicleBatteryFactGroup
::
_settingsGroup
=
"Vehicle.battery"
;
const
int
VehicleBatteryFactGroup
::
_percentRemainingAnnounceDefault
=
30
;
const
double
VehicleBatteryFactGroup
::
_voltageUnavailable
=
-
1.0
;
const
int
VehicleBatteryFactGroup
::
_percentRemainingUnavailable
=
-
1
;
const
int
VehicleBatteryFactGroup
::
_mahConsumedUnavailable
=
-
1
;
const
int
VehicleBatteryFactGroup
::
_currentUnavailable
=
-
1
;
const
double
VehicleBatteryFactGroup
::
_temperatureUnavailable
=
-
1.0
;
const
int
VehicleBatteryFactGroup
::
_cellCountUnavailable
=
-
1.0
;
SettingsFact
VehicleBatteryFactGroup
::
_percentRemainingAnnounceFact
(
_settingsGroup
,
_percentRemainingAnnounceFactName
,
FactMetaData
::
valueTypeInt32
,
_percentRemainingAnnounceDefault
);
VehicleBatteryFactGroup
::
VehicleBatteryFactGroup
(
QObject
*
parent
)
VehicleBatteryFactGroup
::
VehicleBatteryFactGroup
(
QObject
*
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/BatteryFact.json"
,
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/BatteryFact.json"
,
parent
)
,
_vehicle
(
NULL
)
,
_vehicle
(
NULL
)
,
_voltageFact
(
0
,
_voltageFactName
,
FactMetaData
::
valueTypeDouble
)
,
_voltageFact
(
0
,
_voltageFactName
,
FactMetaData
::
valueTypeDouble
)
,
_percentRemainingFact
(
0
,
_percentRemainingFactName
,
FactMetaData
::
valueTypeInt32
)
,
_percentRemainingFact
(
0
,
_percentRemainingFactName
,
FactMetaData
::
valueTypeInt32
)
,
_mahConsumedFact
(
0
,
_mahConsumedFactName
,
FactMetaData
::
valueTypeInt32
)
,
_mahConsumedFact
(
0
,
_mahConsumedFactName
,
FactMetaData
::
valueTypeInt32
)
,
_currentFact
(
0
,
_currentFactName
,
FactMetaData
::
valueTypeInt32
)
,
_currentFact
(
0
,
_currentFactName
,
FactMetaData
::
valueTypeInt32
)
,
_temperatureFact
(
0
,
_temperatureFactName
,
FactMetaData
::
valueTypeDouble
)
,
_temperatureFact
(
0
,
_temperatureFactName
,
FactMetaData
::
valueTypeDouble
)
,
_cellCountFact
(
0
,
_cellCountFactName
,
FactMetaData
::
valueTypeInt32
)
,
_cellCountFact
(
0
,
_cellCountFactName
,
FactMetaData
::
valueTypeInt32
)
{
{
_addFact
(
&
_voltageFact
,
_voltageFactName
);
_addFact
(
&
_voltageFact
,
_voltageFactName
);
_addFact
(
&
_percentRemainingFact
,
_percentRemainingFactName
);
_addFact
(
&
_percentRemainingFact
,
_percentRemainingFactName
);
_addFact
(
&
_mahConsumedFact
,
_mahConsumedFactName
);
_addFact
(
&
_percentRemainingAnnounceFact
,
_percentRemainingAnnounceFactName
);
_addFact
(
&
_currentFact
,
_currentFactName
);
_addFact
(
&
_mahConsumedFact
,
_mahConsumedFactName
);
_addFact
(
&
_temperatureFact
,
_temperatureFactName
);
_addFact
(
&
_currentFact
,
_currentFactName
);
_addFact
(
&
_cellCountFact
,
_cellCountFactName
);
_addFact
(
&
_temperatureFact
,
_temperatureFactName
);
_addFact
(
&
_cellCountFact
,
_cellCountFactName
);
// Start out as not available
// Start out as not available
_voltageFact
.
setRawValue
(
_voltageUnavailable
);
_voltageFact
.
setRawValue
(
_voltageUnavailable
);
...
@@ -1367,6 +1483,10 @@ void VehicleBatteryFactGroup::setVehicle(Vehicle* vehicle)
...
@@ -1367,6 +1483,10 @@ void VehicleBatteryFactGroup::setVehicle(Vehicle* vehicle)
_vehicle
=
vehicle
;
_vehicle
=
vehicle
;
}
}
const
char
*
VehicleWindFactGroup
::
_directionFactName
=
"direction"
;
const
char
*
VehicleWindFactGroup
::
_speedFactName
=
"speed"
;
const
char
*
VehicleWindFactGroup
::
_verticalSpeedFactName
=
"verticalSpeed"
;
VehicleWindFactGroup
::
VehicleWindFactGroup
(
QObject
*
parent
)
VehicleWindFactGroup
::
VehicleWindFactGroup
(
QObject
*
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/WindFact.json"
,
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/WindFact.json"
,
parent
)
,
_vehicle
(
NULL
)
,
_vehicle
(
NULL
)
...
@@ -1389,6 +1509,13 @@ void VehicleWindFactGroup::setVehicle(Vehicle* vehicle)
...
@@ -1389,6 +1509,13 @@ void VehicleWindFactGroup::setVehicle(Vehicle* vehicle)
_vehicle
=
vehicle
;
_vehicle
=
vehicle
;
}
}
const
char
*
VehicleVibrationFactGroup
::
_xAxisFactName
=
"xAxis"
;
const
char
*
VehicleVibrationFactGroup
::
_yAxisFactName
=
"yAxis"
;
const
char
*
VehicleVibrationFactGroup
::
_zAxisFactName
=
"zAxis"
;
const
char
*
VehicleVibrationFactGroup
::
_clipCount1FactName
=
"clipCount1"
;
const
char
*
VehicleVibrationFactGroup
::
_clipCount2FactName
=
"clipCount2"
;
const
char
*
VehicleVibrationFactGroup
::
_clipCount3FactName
=
"clipCount3"
;
VehicleVibrationFactGroup
::
VehicleVibrationFactGroup
(
QObject
*
parent
)
VehicleVibrationFactGroup
::
VehicleVibrationFactGroup
(
QObject
*
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/VibrationFact.json"
,
parent
)
:
FactGroup
(
1000
,
":/json/Vehicle/VibrationFact.json"
,
parent
)
,
_vehicle
(
NULL
)
,
_vehicle
(
NULL
)
...
...
src/Vehicle/Vehicle.h
View file @
3ff23278
...
@@ -29,6 +29,7 @@
...
@@ -29,6 +29,7 @@
#include <QObject>
#include <QObject>
#include <QGeoCoordinate>
#include <QGeoCoordinate>
#include <QElapsedTimer>
#include "FactGroup.h"
#include "FactGroup.h"
#include "LinkInterface.h"
#include "LinkInterface.h"
...
@@ -36,6 +37,7 @@
...
@@ -36,6 +37,7 @@
#include "QmlObjectListModel.h"
#include "QmlObjectListModel.h"
#include "MAVLinkProtocol.h"
#include "MAVLinkProtocol.h"
#include "UASMessageHandler.h"
#include "UASMessageHandler.h"
#include "SettingsFact.h"
class
UAS
;
class
UAS
;
class
UASInterface
;
class
UASInterface
;
...
@@ -66,12 +68,12 @@ public:
...
@@ -66,12 +68,12 @@ public:
Q_PROPERTY
(
Fact
*
clipCount2
READ
clipCount2
CONSTANT
)
Q_PROPERTY
(
Fact
*
clipCount2
READ
clipCount2
CONSTANT
)
Q_PROPERTY
(
Fact
*
clipCount3
READ
clipCount3
CONSTANT
)
Q_PROPERTY
(
Fact
*
clipCount3
READ
clipCount3
CONSTANT
)
Fact
*
xAxis
(
void
)
{
return
&
_xAxisFact
;
}
Fact
*
xAxis
(
void
)
{
return
&
_xAxisFact
;
}
Fact
*
yAxis
(
void
)
{
return
&
_yAxisFact
;
}
Fact
*
yAxis
(
void
)
{
return
&
_yAxisFact
;
}
Fact
*
zAxis
(
void
)
{
return
&
_zAxisFact
;
}
Fact
*
zAxis
(
void
)
{
return
&
_zAxisFact
;
}
Fact
*
clipCount1
(
void
)
{
return
&
_clipCount1Fact
;
}
Fact
*
clipCount1
(
void
)
{
return
&
_clipCount1Fact
;
}
Fact
*
clipCount2
(
void
)
{
return
&
_clipCount2Fact
;
}
Fact
*
clipCount2
(
void
)
{
return
&
_clipCount2Fact
;
}
Fact
*
clipCount3
(
void
)
{
return
&
_clipCount3Fact
;
}
Fact
*
clipCount3
(
void
)
{
return
&
_clipCount3Fact
;
}
void
setVehicle
(
Vehicle
*
vehicle
);
void
setVehicle
(
Vehicle
*
vehicle
);
...
@@ -103,9 +105,9 @@ public:
...
@@ -103,9 +105,9 @@ public:
Q_PROPERTY
(
Fact
*
speed
READ
speed
CONSTANT
)
Q_PROPERTY
(
Fact
*
speed
READ
speed
CONSTANT
)
Q_PROPERTY
(
Fact
*
verticalSpeed
READ
verticalSpeed
CONSTANT
)
Q_PROPERTY
(
Fact
*
verticalSpeed
READ
verticalSpeed
CONSTANT
)
Fact
*
direction
(
void
)
{
return
&
_directionFact
;
}
Fact
*
direction
(
void
)
{
return
&
_directionFact
;
}
Fact
*
speed
(
void
)
{
return
&
_speedFact
;
}
Fact
*
speed
(
void
)
{
return
&
_speedFact
;
}
Fact
*
verticalSpeed
(
void
)
{
return
&
_verticalSpeedFact
;
}
Fact
*
verticalSpeed
(
void
)
{
return
&
_verticalSpeedFact
;
}
void
setVehicle
(
Vehicle
*
vehicle
);
void
setVehicle
(
Vehicle
*
vehicle
);
...
@@ -133,11 +135,11 @@ public:
...
@@ -133,11 +135,11 @@ public:
Q_PROPERTY
(
Fact
*
count
READ
count
CONSTANT
)
Q_PROPERTY
(
Fact
*
count
READ
count
CONSTANT
)
Q_PROPERTY
(
Fact
*
lock
READ
lock
CONSTANT
)
Q_PROPERTY
(
Fact
*
lock
READ
lock
CONSTANT
)
Fact
*
hdop
(
void
)
{
return
&
_hdopFact
;
}
Fact
*
hdop
(
void
)
{
return
&
_hdopFact
;
}
Fact
*
vdop
(
void
)
{
return
&
_vdopFact
;
}
Fact
*
vdop
(
void
)
{
return
&
_vdopFact
;
}
Fact
*
courseOverGround
(
void
)
{
return
&
_courseOverGroundFact
;
}
Fact
*
courseOverGround
(
void
)
{
return
&
_courseOverGroundFact
;
}
Fact
*
count
(
void
)
{
return
&
_countFact
;
}
Fact
*
count
(
void
)
{
return
&
_countFact
;
}
Fact
*
lock
(
void
)
{
return
&
_lockFact
;
}
Fact
*
lock
(
void
)
{
return
&
_lockFact
;
}
void
setVehicle
(
Vehicle
*
vehicle
);
void
setVehicle
(
Vehicle
*
vehicle
);
...
@@ -175,24 +177,33 @@ public:
...
@@ -175,24 +177,33 @@ public:
Q_PROPERTY
(
Fact
*
mahConsumed
READ
mahConsumed
CONSTANT
)
Q_PROPERTY
(
Fact
*
mahConsumed
READ
mahConsumed
CONSTANT
)
Q_PROPERTY
(
Fact
*
current
READ
current
CONSTANT
)
Q_PROPERTY
(
Fact
*
current
READ
current
CONSTANT
)
Q_PROPERTY
(
Fact
*
temperature
READ
temperature
CONSTANT
)
Q_PROPERTY
(
Fact
*
temperature
READ
temperature
CONSTANT
)
Q_PROPERTY
(
Fact
*
cellCount
READ
cellCount
CONSTANT
)
Q_PROPERTY
(
Fact
*
cellCount
READ
cellCount
CONSTANT
)
/// If percentRemaining falls below this value, warning will be output through speech
Q_PROPERTY
(
Fact
*
percentRemainingAnnounce
READ
percentRemainingAnnounce
CONSTANT
)
Fact
*
voltage
(
void
)
{
return
&
_voltageFact
;
}
Fact
*
percentRemaining
(
void
)
{
return
&
_percentRemainingFact
;
}
Fact
*
percentRemainingAnnounce
(
void
)
{
return
&
_percentRemainingAnnounceFact
;
}
Fact
*
mahConsumed
(
void
)
{
return
&
_mahConsumedFact
;
}
Fact
*
current
(
void
)
{
return
&
_currentFact
;
}
Fact
*
temperature
(
void
)
{
return
&
_temperatureFact
;
}
Fact
*
cellCount
(
void
)
{
return
&
_cellCountFact
;
}
Fact
*
voltage
(
void
)
{
return
&
_voltageFact
;
}
Fact
*
percentRemaining
(
void
)
{
return
&
_percentRemainingFact
;
}
Fact
*
mahConsumed
(
void
)
{
return
&
_mahConsumedFact
;
}
Fact
*
current
(
void
)
{
return
&
_currentFact
;
}
Fact
*
temperature
(
void
)
{
return
&
_temperatureFact
;
}
Fact
*
cellCount
(
void
)
{
return
&
_cellCountFact
;
}
void
setVehicle
(
Vehicle
*
vehicle
);
void
setVehicle
(
Vehicle
*
vehicle
);
static
const
char
*
_voltageFactName
;
static
const
char
*
_voltageFactName
;
static
const
char
*
_percentRemainingFactName
;
static
const
char
*
_percentRemainingFactName
;
static
const
char
*
_percentRemainingAnnounceFactName
;
static
const
char
*
_mahConsumedFactName
;
static
const
char
*
_mahConsumedFactName
;
static
const
char
*
_currentFactName
;
static
const
char
*
_currentFactName
;
static
const
char
*
_temperatureFactName
;
static
const
char
*
_temperatureFactName
;
static
const
char
*
_cellCountFactName
;
static
const
char
*
_cellCountFactName
;
static
const
char
*
_settingsGroup
;
static
const
int
_percentRemainingAnnounceDefault
;
static
const
double
_voltageUnavailable
;
static
const
double
_voltageUnavailable
;
static
const
int
_percentRemainingUnavailable
;
static
const
int
_percentRemainingUnavailable
;
static
const
int
_mahConsumedUnavailable
;
static
const
int
_mahConsumedUnavailable
;
...
@@ -201,13 +212,15 @@ public:
...
@@ -201,13 +212,15 @@ public:
static
const
int
_cellCountUnavailable
;
static
const
int
_cellCountUnavailable
;
private:
private:
Vehicle
*
_vehicle
;
Vehicle
*
_vehicle
;
Fact
_voltageFact
;
Fact
_voltageFact
;
Fact
_percentRemainingFact
;
Fact
_percentRemainingFact
;
Fact
_mahConsumedFact
;
Fact
_mahConsumedFact
;
Fact
_currentFact
;
Fact
_currentFact
;
Fact
_temperatureFact
;
Fact
_temperatureFact
;
Fact
_cellCountFact
;
Fact
_cellCountFact
;
static
SettingsFact
_percentRemainingAnnounceFact
;
};
};
class
Vehicle
:
public
FactGroup
class
Vehicle
:
public
FactGroup
...
@@ -222,6 +235,11 @@ public:
...
@@ -222,6 +235,11 @@ public:
FirmwarePluginManager
*
firmwarePluginManager
,
FirmwarePluginManager
*
firmwarePluginManager
,
AutoPilotPluginManager
*
autopilotPluginManager
,
AutoPilotPluginManager
*
autopilotPluginManager
,
JoystickManager
*
joystickManager
);
JoystickManager
*
joystickManager
);
// The following is used to create a disconnected Vehicle. A disconnected vehicle is primarily used to access FactGroup information
// without needing a real connection.
Vehicle
(
QObject
*
parent
=
NULL
);
~
Vehicle
();
~
Vehicle
();
Q_PROPERTY
(
int
id
READ
id
CONSTANT
)
Q_PROPERTY
(
int
id
READ
id
CONSTANT
)
...
@@ -505,6 +523,7 @@ private slots:
...
@@ -505,6 +523,7 @@ private slots:
void
_addNewMapTrajectoryPoint
(
void
);
void
_addNewMapTrajectoryPoint
(
void
);
void
_parametersReady
(
bool
parametersReady
);
void
_parametersReady
(
bool
parametersReady
);
void
_remoteControlRSSIChanged
(
uint8_t
rssi
);
void
_remoteControlRSSIChanged
(
uint8_t
rssi
);
void
_announceflightModeChanged
(
const
QString
&
flightMode
);
void
_handleTextMessage
(
int
newCount
);
void
_handleTextMessage
(
int
newCount
);
void
_handletextMessageReceived
(
UASMessage
*
message
);
void
_handletextMessageReceived
(
UASMessage
*
message
);
...
@@ -540,11 +559,13 @@ private:
...
@@ -540,11 +559,13 @@ private:
void
_mapTrajectoryStart
(
void
);
void
_mapTrajectoryStart
(
void
);
void
_mapTrajectoryStop
(
void
);
void
_mapTrajectoryStop
(
void
);
void
_connectionActive
(
void
);
void
_connectionActive
(
void
);
void
_say
(
const
QString
&
text
,
int
severity
);
void
_say
(
const
QString
&
text
);
QString
_vehicleIdSpeech
(
void
);
private:
private:
int
_id
;
///< Mavlink system id
int
_id
;
///< Mavlink system id
bool
_active
;
bool
_active
;
bool
_disconnectedVehicle
;
///< This Vehicle is a "disconnected" vehicle for ui use when no active vehicle is available
MAV_AUTOPILOT
_firmwareType
;
MAV_AUTOPILOT
_firmwareType
;
MAV_TYPE
_vehicleType
;
MAV_TYPE
_vehicleType
;
...
@@ -635,6 +656,9 @@ private:
...
@@ -635,6 +656,9 @@ private:
uint8_t
_compID
;
uint8_t
_compID
;
bool
_heardFrom
;
bool
_heardFrom
;
static
const
int
_lowBatteryAnnounceRepeatMSecs
;
// Amount of time in between each low battery announcement
QElapsedTimer
_lowBatteryAnnounceTimer
;
// FactGroup facts
// FactGroup facts
Fact
_rollFact
;
Fact
_rollFact
;
...
...
src/audio/QGCAudioWorker.cpp
View file @
3ff23278
...
@@ -76,9 +76,6 @@ QGCAudioWorker::QGCAudioWorker(QObject *parent) :
...
@@ -76,9 +76,6 @@ QGCAudioWorker::QGCAudioWorker(QObject *parent) :
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
pVoice
(
NULL
),
pVoice
(
NULL
),
#endif
#endif
#ifdef QGC_NOTIFY_TUNES_ENABLED
sound
(
NULL
),
#endif
emergency
(
false
),
emergency
(
false
),
muted
(
false
)
muted
(
false
)
{
{
...
@@ -89,10 +86,6 @@ QGCAudioWorker::QGCAudioWorker(QObject *parent) :
...
@@ -89,10 +86,6 @@ QGCAudioWorker::QGCAudioWorker(QObject *parent) :
void
QGCAudioWorker
::
init
()
void
QGCAudioWorker
::
init
()
{
{
#ifdef QGC_NOTIFY_TUNES_ENABLED
sound
=
new
QSound
(
":/res/Alert"
);
#endif
#if defined Q_OS_LINUX && !defined __android__ && defined QGC_SPEECH_ENABLED
#if defined Q_OS_LINUX && !defined __android__ && defined QGC_SPEECH_ENABLED
espeak_Initialize
(
AUDIO_OUTPUT_SYNCH_PLAYBACK
,
500
,
NULL
,
0
);
// initialize for playback with 500ms buffer and no options (see speak_lib.h)
espeak_Initialize
(
AUDIO_OUTPUT_SYNCH_PLAYBACK
,
500
,
NULL
,
0
);
// initialize for playback with 500ms buffer and no options (see speak_lib.h)
espeak_VOICE
*
espeak_voice
=
espeak_GetCurrentVoice
();
espeak_VOICE
*
espeak_voice
=
espeak_GetCurrentVoice
();
...
@@ -135,11 +128,10 @@ QGCAudioWorker::~QGCAudioWorker()
...
@@ -135,11 +128,10 @@ QGCAudioWorker::~QGCAudioWorker()
#endif
#endif
}
}
void
QGCAudioWorker
::
say
(
QString
inText
,
int
severity
)
void
QGCAudioWorker
::
say
(
QString
inText
)
{
{
#ifdef __android__
#ifdef __android__
Q_UNUSED
(
inText
);
Q_UNUSED
(
inText
);
Q_UNUSED
(
severity
);
#else
#else
static
bool
threadInit
=
false
;
static
bool
threadInit
=
false
;
if
(
!
threadInit
)
{
if
(
!
threadInit
)
{
...
@@ -150,17 +142,6 @@ void QGCAudioWorker::say(QString inText, int severity)
...
@@ -150,17 +142,6 @@ void QGCAudioWorker::say(QString inText, int severity)
if
(
!
muted
)
if
(
!
muted
)
{
{
QString
text
=
fixTextMessageForAudio
(
inText
);
QString
text
=
fixTextMessageForAudio
(
inText
);
// Prepend high priority text with alert beep
if
(
severity
<
GAudioOutput
::
AUDIO_SEVERITY_CRITICAL
)
{
beep
();
}
#ifdef QGC_NOTIFY_TUNES_ENABLED
// Wait for the last sound to finish
while
(
!
sound
->
isFinished
())
{
QGC
::
SLEEP
::
msleep
(
100
);
}
#endif
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
HRESULT
hr
=
pVoice
->
Speak
(
text
.
toStdWString
().
c_str
(),
SPF_DEFAULT
,
NULL
);
HRESULT
hr
=
pVoice
->
Speak
(
text
.
toStdWString
().
c_str
(),
SPF_DEFAULT
,
NULL
);
...
@@ -195,17 +176,6 @@ void QGCAudioWorker::mute(bool mute)
...
@@ -195,17 +176,6 @@ void QGCAudioWorker::mute(bool mute)
}
}
}
}
void
QGCAudioWorker
::
beep
()
{
if
(
!
muted
)
{
#ifdef QGC_NOTIFY_TUNES_ENABLED
sound
->
play
(
":/res/Alert"
);
#endif
}
}
bool
QGCAudioWorker
::
isMuted
()
bool
QGCAudioWorker
::
isMuted
()
{
{
return
this
->
muted
;
return
this
->
muted
;
...
...
src/audio/QGCAudioWorker.h
View file @
3ff23278
...
@@ -3,9 +3,6 @@
...
@@ -3,9 +3,6 @@
#include <QObject>
#include <QObject>
#include <QTimer>
#include <QTimer>
#ifdef QGC_NOTIFY_TUNES_ENABLED
#include <QSound>
#endif
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
// Documentation: http://msdn.microsoft.com/en-us/library/ee125082%28v=VS.85%29.aspx
// Documentation: http://msdn.microsoft.com/en-us/library/ee125082%28v=VS.85%29.aspx
...
@@ -29,19 +26,13 @@ public:
...
@@ -29,19 +26,13 @@ public:
signals:
signals:
public
slots
:
public
slots
:
/** @brief Say this text if current output priority matches */
/** @brief Say this text */
void
say
(
QString
text
,
int
severity
=
1
);
void
say
(
QString
text
);
/** @brief Sound a single beep */
void
beep
();
protected:
protected:
int
voiceIndex
;
///< The index of the flite voice to use (awb, slt, rms)
int
voiceIndex
;
///< The index of the flite voice to use (awb, slt, rms)
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
ISpVoice
*
pVoice
;
ISpVoice
*
pVoice
;
#endif
#ifdef QGC_NOTIFY_TUNES_ENABLED
QSound
*
sound
;
#endif
#endif
bool
emergency
;
///< Emergency status flag
bool
emergency
;
///< Emergency status flag
QTimer
*
emergencyTimer
;
QTimer
*
emergencyTimer
;
...
...
src/uas/UAS.cc
View file @
3ff23278
...
@@ -41,8 +41,6 @@
...
@@ -41,8 +41,6 @@
QGC_LOGGING_CATEGORY
(
UASLog
,
"UASLog"
)
QGC_LOGGING_CATEGORY
(
UASLog
,
"UASLog"
)
#define UAS_DEFAULT_BATTERY_WARNLEVEL 20
/**
/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* by calling readSettings. This means the new UAS will have the same settings
* by calling readSettings. This means the new UAS will have the same settings
...
@@ -63,17 +61,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
...
@@ -63,17 +61,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
custom_mode
(
0
),
custom_mode
(
0
),
status
(
-
1
),
status
(
-
1
),
startVoltage
(
-
1.0
f
),
tickVoltage
(
10.5
f
),
lastTickVoltageValue
(
13.0
f
),
tickLowpassVoltage
(
12.0
f
),
warnLevelPercent
(
UAS_DEFAULT_BATTERY_WARNLEVEL
),
currentVoltage
(
12.6
f
),
lpVoltage
(
-
1.0
f
),
currentCurrent
(
0.4
f
),
chargeLevel
(
-
1
),
lowBattAlarm
(
false
),
startTime
(
QGC
::
groundTimeMilliseconds
()),
startTime
(
QGC
::
groundTimeMilliseconds
()),
onboardTimeOffset
(
0
),
onboardTimeOffset
(
0
),
...
@@ -294,12 +281,8 @@ void UAS::receiveMessage(mavlink_message_t message)
...
@@ -294,12 +281,8 @@ void UAS::receiveMessage(mavlink_message_t message)
QString
audiostring
=
QString
(
"System %1"
).
arg
(
uasId
);
QString
audiostring
=
QString
(
"System %1"
).
arg
(
uasId
);
QString
stateAudio
=
""
;
QString
stateAudio
=
""
;
QString
modeAudio
=
""
;
QString
navModeAudio
=
""
;
QString
navModeAudio
=
""
;
bool
statechanged
=
false
;
bool
statechanged
=
false
;
bool
modechanged
=
false
;
QString
audiomodeText
=
_firmwarePluginManager
->
firmwarePluginForAutopilot
((
MAV_AUTOPILOT
)
state
.
autopilot
,
(
MAV_TYPE
)
state
.
type
)
->
flightMode
(
state
.
base_mode
,
state
.
custom_mode
);
if
((
state
.
system_status
!=
this
->
status
)
&&
state
.
system_status
!=
MAV_STATE_UNINIT
)
if
((
state
.
system_status
!=
this
->
status
)
&&
state
.
system_status
!=
MAV_STATE_UNINIT
)
{
{
...
@@ -318,27 +301,14 @@ void UAS::receiveMessage(mavlink_message_t message)
...
@@ -318,27 +301,14 @@ void UAS::receiveMessage(mavlink_message_t message)
stateAudio
=
uasState
;
stateAudio
=
uasState
;
}
}
if
(
this
->
base_mode
!=
state
.
base_mode
||
this
->
custom_mode
!=
state
.
custom_mode
)
{
modechanged
=
true
;
this
->
base_mode
=
state
.
base_mode
;
this
->
custom_mode
=
state
.
custom_mode
;
modeAudio
=
" is now in "
+
audiomodeText
+
" flight mode"
;
}
// We got the mode
// We got the mode
receivedMode
=
true
;
receivedMode
=
true
;
// AUDIO
// AUDIO
if
(
modechanged
&&
statechanged
)
if
(
statechanged
)
{
// Output both messages
audiostring
+=
modeAudio
+
" and "
+
stateAudio
;
}
else
if
(
modechanged
||
statechanged
)
{
{
// Output the one message
// Output the one message
audiostring
+=
modeAudio
+
stateAudio
;
audiostring
+=
stateAudio
;
}
}
if
(
statechanged
&&
((
int
)
state
.
system_status
==
(
int
)
MAV_STATE_CRITICAL
||
state
.
system_status
==
(
int
)
MAV_STATE_EMERGENCY
))
if
(
statechanged
&&
((
int
)
state
.
system_status
==
(
int
)
MAV_STATE_CRITICAL
||
state
.
system_status
==
(
int
)
MAV_STATE_EMERGENCY
))
...
@@ -346,7 +316,7 @@ void UAS::receiveMessage(mavlink_message_t message)
...
@@ -346,7 +316,7 @@ void UAS::receiveMessage(mavlink_message_t message)
_say
(
QString
(
"Emergency for system %1"
).
arg
(
this
->
getUASID
()),
GAudioOutput
::
AUDIO_SEVERITY_EMERGENCY
);
_say
(
QString
(
"Emergency for system %1"
).
arg
(
this
->
getUASID
()),
GAudioOutput
::
AUDIO_SEVERITY_EMERGENCY
);
QTimer
::
singleShot
(
3000
,
qgcApp
()
->
toolbox
()
->
audioOutput
(),
SLOT
(
startEmergency
()));
QTimer
::
singleShot
(
3000
,
qgcApp
()
->
toolbox
()
->
audioOutput
(),
SLOT
(
startEmergency
()));
}
}
else
if
(
modechanged
||
statechanged
)
else
if
(
statechanged
)
{
{
_say
(
audiostring
.
toLower
());
_say
(
audiostring
.
toLower
());
}
}
...
@@ -378,62 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message)
...
@@ -378,62 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit
loadChanged
(
this
,
state
.
load
/
10.0
f
);
emit
loadChanged
(
this
,
state
.
load
/
10.0
f
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"load"
),
"%"
,
state
.
load
/
10.0
f
,
time
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"load"
),
"%"
,
state
.
load
/
10.0
f
,
time
);
currentVoltage
=
state
.
voltage_battery
/
1000.0
f
;
if
(
state
.
voltage_battery
>
0.0
f
&&
state
.
voltage_battery
!=
UINT16_MAX
)
{
// Battery charge/time remaining/voltage calculations
currentVoltage
=
state
.
voltage_battery
/
1000.0
f
;
filterVoltage
(
currentVoltage
);
tickLowpassVoltage
=
tickLowpassVoltage
*
0.8
f
+
0.2
f
*
currentVoltage
;
// We don't want to tick above the threshold
if
(
tickLowpassVoltage
>
tickVoltage
)
{
lastTickVoltageValue
=
tickLowpassVoltage
;
}
if
((
startVoltage
>
0.0
f
)
&&
(
tickLowpassVoltage
<
tickVoltage
)
&&
(
fabs
(
lastTickVoltageValue
-
tickLowpassVoltage
)
>
0.1
f
)
/* warn if lower than treshold */
&&
(
lpVoltage
<
tickVoltage
)
/* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */
&&
(
currentVoltage
>
3.3
f
)
/* warn only if current voltage is really still lower by a reasonable amount */
&&
((
currentVoltage
-
0.2
f
)
<
tickVoltage
)
/* warn only every 20 seconds */
&&
(
QGC
::
groundTimeUsecs
()
-
lastVoltageWarning
)
>
20000000
)
{
_say
(
QString
(
"Low battery system %1: %2 volts"
).
arg
(
getUASID
()).
arg
(
lpVoltage
,
0
,
'f'
,
1
,
QChar
(
' '
)));
lastVoltageWarning
=
QGC
::
groundTimeUsecs
();
lastTickVoltageValue
=
tickLowpassVoltage
;
}
if
(
startVoltage
==
-
1.0
f
&&
currentVoltage
>
0.1
f
)
startVoltage
=
currentVoltage
;
chargeLevel
=
state
.
battery_remaining
;
emit
batteryChanged
(
this
,
lpVoltage
,
currentCurrent
,
getChargeLevel
(),
0
);
}
emit
valueChanged
(
uasId
,
name
.
arg
(
"battery_remaining"
),
"%"
,
getChargeLevel
(),
time
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"battery_voltage"
),
"V"
,
currentVoltage
,
time
);
// And if the battery current draw is measured, log that also.
if
(
state
.
current_battery
!=
-
1
)
{
currentCurrent
=
((
double
)
state
.
current_battery
)
/
100.0
f
;
emit
valueChanged
(
uasId
,
name
.
arg
(
"battery_current"
),
"A"
,
currentCurrent
,
time
);
}
// LOW BATTERY ALARM
if
(
chargeLevel
>=
0
&&
(
getChargeLevel
()
<
warnLevelPercent
))
{
// An audio alarm. Does not generate any signals.
startLowBattAlarm
();
}
else
{
stopLowBattAlarm
();
}
// control_sensors_enabled:
// control_sensors_enabled:
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
emit
attitudeControlEnabled
(
state
.
onboard_control_sensors_enabled
&
(
1
<<
11
));
emit
attitudeControlEnabled
(
state
.
onboard_control_sensors_enabled
&
(
1
<<
11
));
...
@@ -1178,19 +1092,6 @@ quint64 UAS::getUnixTime(quint64 time)
...
@@ -1178,19 +1092,6 @@ quint64 UAS::getUnixTime(quint64 time)
return
ret
;
return
ret
;
}
}
/**
* @param value battery voltage
*/
float
UAS
::
filterVoltage
(
float
value
)
{
if
(
lpVoltage
<
0.0
f
)
{
lpVoltage
=
value
;
}
lpVoltage
=
lpVoltage
*
0.6
f
+
value
*
0.4
f
;
return
lpVoltage
;
}
/**
/**
* Get the status of the code and a description of the status.
* Get the status of the code and a description of the status.
* Status can be unitialized, booting up, calibrating sensors, active
* Status can be unitialized, booting up, calibrating sensors, active
...
@@ -2077,31 +1978,6 @@ QMap<int, QString> UAS::getComponents()
...
@@ -2077,31 +1978,6 @@ QMap<int, QString> UAS::getComponents()
return
components
;
return
components
;
}
}
/**
* @return charge level in percent - 0 - 100
*/
float
UAS
::
getChargeLevel
()
{
return
chargeLevel
;
}
void
UAS
::
startLowBattAlarm
()
{
if
(
!
lowBattAlarm
)
{
_say
(
tr
(
"System %1 has low battery"
).
arg
(
getUASID
()));
lowBattAlarm
=
true
;
}
}
void
UAS
::
stopLowBattAlarm
()
{
if
(
lowBattAlarm
)
{
lowBattAlarm
=
false
;
}
}
void
UAS
::
sendMapRCToParam
(
QString
param_id
,
float
scale
,
float
value0
,
quint8
param_rc_channel_index
,
float
valueMin
,
float
valueMax
)
void
UAS
::
sendMapRCToParam
(
QString
param_id
,
float
scale
,
float
value0
,
quint8
param_rc_channel_index
,
float
valueMin
,
float
valueMax
)
{
{
if
(
!
_vehicle
)
{
if
(
!
_vehicle
)
{
...
@@ -2164,8 +2040,8 @@ void UAS::unsetRCToParameterMap()
...
@@ -2164,8 +2040,8 @@ void UAS::unsetRCToParameterMap()
void
UAS
::
_say
(
const
QString
&
text
,
int
severity
)
void
UAS
::
_say
(
const
QString
&
text
,
int
severity
)
{
{
if
(
!
qgcApp
()
->
runningUnitTests
())
Q_UNUSED
(
severity
);
qgcApp
()
->
toolbox
()
->
audioOutput
()
->
say
(
text
,
severity
);
qgcApp
()
->
toolbox
()
->
audioOutput
()
->
say
(
text
);
}
}
void
UAS
::
shutdownVehicle
(
void
)
void
UAS
::
shutdownVehicle
(
void
)
...
...
src/uas/UAS.h
View file @
3ff23278
...
@@ -77,8 +77,6 @@ public:
...
@@ -77,8 +77,6 @@ public:
/** @brief The time interval the robot is switched on */
/** @brief The time interval the robot is switched on */
quint64
getUptime
()
const
;
quint64
getUptime
()
const
;
/** @brief Add one measurement and get low-passed voltage */
float
filterVoltage
(
float
value
);
Q_PROPERTY
(
double
latitude
READ
getLatitude
WRITE
setLatitude
NOTIFY
latitudeChanged
)
Q_PROPERTY
(
double
latitude
READ
getLatitude
WRITE
setLatitude
NOTIFY
latitudeChanged
)
Q_PROPERTY
(
double
longitude
READ
getLongitude
WRITE
setLongitude
NOTIFY
longitudeChanged
)
Q_PROPERTY
(
double
longitude
READ
getLongitude
WRITE
setLongitude
NOTIFY
longitudeChanged
)
...
@@ -376,21 +374,6 @@ protected: //COMMENTS FOR TEST UNIT
...
@@ -376,21 +374,6 @@ protected: //COMMENTS FOR TEST UNIT
uint32_t
custom_mode
;
///< The current mode of the MAV
uint32_t
custom_mode
;
///< The current mode of the MAV
int
status
;
///< The current status of the MAV
int
status
;
///< The current status of the MAV
// dongfang: This looks like a candidate for being moved off to a separate class.
/// BATTERY / ENERGY
float
startVoltage
;
///< Voltage at system start
float
tickVoltage
;
///< Voltage where 0.1 V ticks are told
float
lastTickVoltageValue
;
///< The last voltage where a tick was announced
float
tickLowpassVoltage
;
///< Lowpass-filtered voltage for the tick announcement
float
warnLevelPercent
;
///< Warning level, in percent
double
currentVoltage
;
///< Voltage currently measured
float
lpVoltage
;
///< Low-pass filtered voltage
double
currentCurrent
;
///< Battery current currently measured
bool
batteryRemainingEstimateEnabled
;
///< If the estimate is enabled, QGC will try to estimate the remaining battery life
float
chargeLevel
;
///< Charge level of battery, in percent
bool
lowBattAlarm
;
///< Switch if battery is low
/// TIMEKEEPING
/// TIMEKEEPING
quint64
startTime
;
///< The time the UAS was switched on
quint64
startTime
;
///< The time the UAS was switched on
quint64
onboardTimeOffset
;
quint64
onboardTimeOffset
;
...
@@ -486,8 +469,6 @@ protected: //COMMENTS FOR TEST UNIT
...
@@ -486,8 +469,6 @@ protected: //COMMENTS FOR TEST UNIT
#endif
#endif
public:
public:
/** @brief Get the current charge level */
float
getChargeLevel
();
/** @brief Get the human-readable status message for this code */
/** @brief Get the human-readable status message for this code */
void
getStatusForCode
(
int
statusCode
,
QString
&
uasState
,
QString
&
stateDescription
);
void
getStatusForCode
(
int
statusCode
,
QString
&
uasState
,
QString
&
stateDescription
);
...
@@ -559,9 +540,6 @@ public slots:
...
@@ -559,9 +540,6 @@ public slots:
void
stopHil
();
void
stopHil
();
#endif
#endif
void
startLowBattAlarm
();
void
stopLowBattAlarm
();
/** @brief Set the values for the manual control of the vehicle */
/** @brief Set the values for the manual control of the vehicle */
void
setExternalControlSetpoint
(
float
roll
,
float
pitch
,
float
yaw
,
float
thrust
,
quint16
buttons
,
int
joystickMode
);
void
setExternalControlSetpoint
(
float
roll
,
float
pitch
,
float
yaw
,
float
thrust
,
quint16
buttons
,
int
joystickMode
);
...
...
src/ui/preferences/GeneralSettings.qml
View file @
3ff23278
...
@@ -40,6 +40,8 @@ Rectangle {
...
@@ -40,6 +40,8 @@ Rectangle {
anchors.fill
:
parent
anchors.fill
:
parent
anchors.margins
:
ScreenTools
.
defaultFontPixelWidth
anchors.margins
:
ScreenTools
.
defaultFontPixelWidth
property
Fact
_percentRemainingAnnounce
:
QGroundControl
.
multiVehicleManager
.
disconnectedVehicle
.
battery
.
percentRemainingAnnounce
QGCPalette
{
QGCPalette
{
id
:
qgcPal
id
:
qgcPal
colorGroupEnabled
:
enabled
colorGroupEnabled
:
enabled
...
@@ -127,6 +129,32 @@ Rectangle {
...
@@ -127,6 +129,32 @@ Rectangle {
}
}
}
}
}
}
//-----------------------------------------------------------------
//-- Battery talker
Row
{
spacing
:
ScreenTools
.
defaultFontPixelWidth
QGCCheckBox
{
id
:
announcePercentCheckbox
anchors.baseline
:
announcePercent
.
baseline
text
:
"
Announce battery percent lower than:
"
checked
:
_percentRemainingAnnounce
.
value
!=
0
onClicked
:
{
if
(
checked
)
{
_percentRemainingAnnounce
.
value
=
_percentRemainingAnnounce
.
defaultValueString
}
else
{
_percentRemainingAnnounce
.
value
=
0
}
}
}
FactTextField
{
id
:
announcePercent
fact
:
_percentRemainingAnnounce
enabled
:
announcePercentCheckbox
.
checked
}
}
Item
{
Item
{
height
:
ScreenTools
.
defaultFontPixelHeight
/
2
height
:
ScreenTools
.
defaultFontPixelHeight
/
2
...
...
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