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
0640d456
Unverified
Commit
0640d456
authored
6 years ago
by
Don Gagne
Committed by
GitHub
6 years ago
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #7066 from DonLakeFlyer/OrbitTelemetry
Show Orbit telemetry on map
parents
a9fe38d2
4bd1390c
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
68 additions
and
3 deletions
+68
-3
FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewMap.qml
+7
-0
QGCMapCircleVisuals.qml
src/MissionManager/QGCMapCircleVisuals.qml
+1
-2
Vehicle.cc
src/Vehicle/Vehicle.cc
+43
-1
Vehicle.h
src/Vehicle/Vehicle.h
+17
-0
No files found.
src/FlightDisplay/FlightDisplayViewMap.qml
View file @
0640d456
...
@@ -354,6 +354,13 @@ FlightMap {
...
@@ -354,6 +354,13 @@ FlightMap {
}
}
}
}
// Used to show orbit status telemetry from the vehicle
QGCMapCircleVisuals
{
mapControl
:
parent
mapCircle
:
_activeVehicle
.
orbitMapCircle
visible
:
_activeVehicle
?
_activeVehicle
.
orbitActive
:
false
}
// Handle guided mode clicks
// Handle guided mode clicks
MouseArea
{
MouseArea
{
anchors.fill
:
parent
anchors.fill
:
parent
...
...
This diff is collapsed.
Click to expand it.
src/MissionManager/QGCMapCircleVisuals.qml
View file @
0640d456
...
@@ -105,8 +105,7 @@ Item {
...
@@ -105,8 +105,7 @@ Item {
id
:
rotationIndicatorComponent
id
:
rotationIndicatorComponent
MapQuickItem
{
MapQuickItem
{
z
:
QGroundControl
.
zOrderMapItems
+
2
visible
:
mapCircle
.
showRotation
visible
:
mapCircle
.
showRotation
property
bool
topIndicator
:
true
property
bool
topIndicator
:
true
...
...
This diff is collapsed.
Click to expand it.
src/Vehicle/Vehicle.cc
View file @
0640d456
...
@@ -182,6 +182,7 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -182,6 +182,7 @@ Vehicle::Vehicle(LinkInterface* link,
,
_uid
(
0
)
,
_uid
(
0
)
,
_lastAnnouncedLowBatteryPercent
(
100
)
,
_lastAnnouncedLowBatteryPercent
(
100
)
,
_priorityLinkCommanded
(
false
)
,
_priorityLinkCommanded
(
false
)
,
_orbitActive
(
false
)
,
_rollFact
(
0
,
_rollFactName
,
FactMetaData
::
valueTypeDouble
)
,
_rollFact
(
0
,
_rollFactName
,
FactMetaData
::
valueTypeDouble
)
,
_pitchFact
(
0
,
_pitchFactName
,
FactMetaData
::
valueTypeDouble
)
,
_pitchFact
(
0
,
_pitchFactName
,
FactMetaData
::
valueTypeDouble
)
,
_headingFact
(
0
,
_headingFactName
,
FactMetaData
::
valueTypeDouble
)
,
_headingFact
(
0
,
_headingFactName
,
FactMetaData
::
valueTypeDouble
)
...
@@ -282,6 +283,8 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -282,6 +283,8 @@ Vehicle::Vehicle(LinkInterface* link,
_mapTrajectoryTimer
.
setInterval
(
_mapTrajectoryMsecsBetweenPoints
);
_mapTrajectoryTimer
.
setInterval
(
_mapTrajectoryMsecsBetweenPoints
);
connect
(
&
_mapTrajectoryTimer
,
&
QTimer
::
timeout
,
this
,
&
Vehicle
::
_addNewMapTrajectoryPoint
);
connect
(
&
_mapTrajectoryTimer
,
&
QTimer
::
timeout
,
this
,
&
Vehicle
::
_addNewMapTrajectoryPoint
);
connect
(
&
_orbitTelemetryTimer
,
&
QTimer
::
timeout
,
this
,
&
Vehicle
::
_orbitTelemetryTimeout
);
// Create camera manager instance
// Create camera manager instance
_cameras
=
_firmwarePlugin
->
createCameraManager
(
this
);
_cameras
=
_firmwarePlugin
->
createCameraManager
(
this
);
emit
dynamicCamerasChanged
();
emit
dynamicCamerasChanged
();
...
@@ -377,6 +380,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
...
@@ -377,6 +380,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
,
_gitHash
(
versionNotSetValue
)
,
_gitHash
(
versionNotSetValue
)
,
_uid
(
0
)
,
_uid
(
0
)
,
_lastAnnouncedLowBatteryPercent
(
100
)
,
_lastAnnouncedLowBatteryPercent
(
100
)
,
_orbitActive
(
false
)
,
_rollFact
(
0
,
_rollFactName
,
FactMetaData
::
valueTypeDouble
)
,
_rollFact
(
0
,
_rollFactName
,
FactMetaData
::
valueTypeDouble
)
,
_pitchFact
(
0
,
_pitchFactName
,
FactMetaData
::
valueTypeDouble
)
,
_pitchFact
(
0
,
_pitchFactName
,
FactMetaData
::
valueTypeDouble
)
,
_headingFact
(
0
,
_headingFactName
,
FactMetaData
::
valueTypeDouble
)
,
_headingFact
(
0
,
_headingFactName
,
FactMetaData
::
valueTypeDouble
)
...
@@ -772,6 +776,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
...
@@ -772,6 +776,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case
MAVLINK_MSG_ID_STATUSTEXT
:
case
MAVLINK_MSG_ID_STATUSTEXT
:
_handleStatusText
(
message
);
_handleStatusText
(
message
);
break
;
break
;
case
MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS
:
_handleOrbitExecutionStatus
(
message
);
break
;
case
MAVLINK_MSG_ID_PING
:
case
MAVLINK_MSG_ID_PING
:
_handlePing
(
link
,
message
);
_handlePing
(
link
,
message
);
...
@@ -803,7 +810,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
...
@@ -803,7 +810,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas
->
receiveMessage
(
message
);
_uas
->
receiveMessage
(
message
);
}
}
#if !defined(NO_ARDUPILOT_DIALECT)
#if !defined(NO_ARDUPILOT_DIALECT)
void
Vehicle
::
_handleCameraFeedback
(
const
mavlink_message_t
&
message
)
void
Vehicle
::
_handleCameraFeedback
(
const
mavlink_message_t
&
message
)
{
{
...
@@ -817,6 +823,42 @@ void Vehicle::_handleCameraFeedback(const mavlink_message_t& message)
...
@@ -817,6 +823,42 @@ void Vehicle::_handleCameraFeedback(const mavlink_message_t& message)
}
}
#endif
#endif
void
Vehicle
::
_handleOrbitExecutionStatus
(
const
mavlink_message_t
&
message
)
{
mavlink_orbit_execution_status_t
orbitStatus
;
mavlink_msg_orbit_execution_status_decode
(
&
message
,
&
orbitStatus
);
double
newRadius
=
qAbs
(
static_cast
<
double
>
(
orbitStatus
.
radius
));
if
(
!
qFuzzyCompare
(
_orbitMapCircle
.
radius
()
->
rawValue
().
toDouble
(),
newRadius
))
{
_orbitMapCircle
.
radius
()
->
setRawValue
(
newRadius
);
}
bool
newOrbitClockwise
=
orbitStatus
.
radius
>
0
?
true
:
false
;
if
(
_orbitMapCircle
.
clockwiseRotation
()
!=
newOrbitClockwise
)
{
_orbitMapCircle
.
setClockwiseRotation
(
newOrbitClockwise
);
}
QGeoCoordinate
newCenter
(
static_cast
<
double
>
(
orbitStatus
.
x
)
/
qPow
(
10.0
,
7.0
),
static_cast
<
double
>
(
orbitStatus
.
y
)
/
qPow
(
10.0
,
7.0
));
if
(
_orbitMapCircle
.
center
()
!=
newCenter
)
{
_orbitMapCircle
.
setCenter
(
newCenter
);
}
if
(
!
_orbitActive
)
{
_orbitActive
=
true
;
_orbitMapCircle
.
setShowRotation
(
true
);
emit
orbitActiveChanged
(
true
);
}
_orbitTelemetryTimer
.
start
(
_orbitTelemetryTimeoutMsecs
);
}
void
Vehicle
::
_orbitTelemetryTimeout
(
void
)
{
_orbitActive
=
false
;
emit
orbitActiveChanged
(
false
);
}
void
Vehicle
::
_handleCameraImageCaptured
(
const
mavlink_message_t
&
message
)
void
Vehicle
::
_handleCameraImageCaptured
(
const
mavlink_message_t
&
message
)
{
{
mavlink_camera_image_captured_t
feedback
;
mavlink_camera_image_captured_t
feedback
;
...
...
This diff is collapsed.
Click to expand it.
src/Vehicle/Vehicle.h
View file @
0640d456
...
@@ -20,6 +20,7 @@
...
@@ -20,6 +20,7 @@
#include "MAVLinkProtocol.h"
#include "MAVLinkProtocol.h"
#include "UASMessageHandler.h"
#include "UASMessageHandler.h"
#include "SettingsFact.h"
#include "SettingsFact.h"
#include "QGCMapCircle.h"
class
UAS
;
class
UAS
;
class
UASInterface
;
class
UASInterface
;
...
@@ -630,6 +631,10 @@ public:
...
@@ -630,6 +631,10 @@ public:
Q_PROPERTY
(
quint64
mavlinkLossCount
READ
mavlinkLossCount
NOTIFY
mavlinkStatusChanged
)
Q_PROPERTY
(
quint64
mavlinkLossCount
READ
mavlinkLossCount
NOTIFY
mavlinkStatusChanged
)
Q_PROPERTY
(
float
mavlinkLossPercent
READ
mavlinkLossPercent
NOTIFY
mavlinkStatusChanged
)
Q_PROPERTY
(
float
mavlinkLossPercent
READ
mavlinkLossPercent
NOTIFY
mavlinkStatusChanged
)
// The following properties relate to Orbit status
Q_PROPERTY
(
bool
orbitActive
READ
orbitActive
NOTIFY
orbitActiveChanged
)
Q_PROPERTY
(
QGCMapCircle
*
orbitMapCircle
READ
orbitMapCircle
CONSTANT
)
// Vehicle state used for guided control
// Vehicle state used for guided control
Q_PROPERTY
(
bool
flying
READ
flying
NOTIFY
flyingChanged
)
///< Vehicle is flying
Q_PROPERTY
(
bool
flying
READ
flying
NOTIFY
flyingChanged
)
///< Vehicle is flying
Q_PROPERTY
(
bool
landing
READ
landing
NOTIFY
landingChanged
)
///< Vehicle is in landing pattern (DO_LAND_START)
Q_PROPERTY
(
bool
landing
READ
landing
NOTIFY
landingChanged
)
///< Vehicle is in landing pattern (DO_LAND_START)
...
@@ -931,6 +936,9 @@ public:
...
@@ -931,6 +936,9 @@ public:
int
telemetryRNoise
()
{
return
_telemetryRNoise
;
}
int
telemetryRNoise
()
{
return
_telemetryRNoise
;
}
bool
autoDisarm
();
bool
autoDisarm
();
bool
highLatencyLink
()
const
{
return
_highLatencyLink
;
}
bool
highLatencyLink
()
const
{
return
_highLatencyLink
;
}
bool
orbitActive
()
const
{
return
_orbitActive
;
}
QGCMapCircle
*
orbitMapCircle
()
{
return
&
_orbitMapCircle
;
}
/// Get the maximum MAVLink protocol version supported
/// Get the maximum MAVLink protocol version supported
/// @return the maximum version
/// @return the maximum version
unsigned
maxProtoVersion
()
const
{
return
_maxProtoVersion
;
}
unsigned
maxProtoVersion
()
const
{
return
_maxProtoVersion
;
}
...
@@ -1130,6 +1138,7 @@ signals:
...
@@ -1130,6 +1138,7 @@ signals:
void
sensorsEnabledBitsChanged
(
int
sensorsEnabledBits
);
void
sensorsEnabledBitsChanged
(
int
sensorsEnabledBits
);
void
sensorsHealthBitsChanged
(
int
sensorsHealthBits
);
void
sensorsHealthBitsChanged
(
int
sensorsHealthBits
);
void
sensorsUnhealthyBitsChanged
(
int
sensorsUnhealthyBits
);
void
sensorsUnhealthyBitsChanged
(
int
sensorsUnhealthyBits
);
void
orbitActiveChanged
(
bool
orbitActive
);
void
firmwareVersionChanged
(
void
);
void
firmwareVersionChanged
(
void
);
void
firmwareCustomVersionChanged
(
void
);
void
firmwareCustomVersionChanged
(
void
);
...
@@ -1203,6 +1212,7 @@ private slots:
...
@@ -1203,6 +1212,7 @@ private slots:
void
_trafficUpdate
(
bool
alert
,
QString
traffic_id
,
QString
vehicle_id
,
QGeoCoordinate
location
,
float
heading
);
void
_trafficUpdate
(
bool
alert
,
QString
traffic_id
,
QString
vehicle_id
,
QGeoCoordinate
location
,
float
heading
);
void
_adsbTimerTimeout
();
void
_adsbTimerTimeout
();
void
_orbitTelemetryTimeout
(
void
);
private:
private:
bool
_containsLink
(
LinkInterface
*
link
);
bool
_containsLink
(
LinkInterface
*
link
);
...
@@ -1241,6 +1251,7 @@ private:
...
@@ -1241,6 +1251,7 @@ private:
void
_handleDistanceSensor
(
mavlink_message_t
&
message
);
void
_handleDistanceSensor
(
mavlink_message_t
&
message
);
void
_handleEstimatorStatus
(
mavlink_message_t
&
message
);
void
_handleEstimatorStatus
(
mavlink_message_t
&
message
);
void
_handleStatusText
(
mavlink_message_t
&
message
);
void
_handleStatusText
(
mavlink_message_t
&
message
);
void
_handleOrbitExecutionStatus
(
const
mavlink_message_t
&
message
);
// ArduPilot dialect messages
// ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT)
#if !defined(NO_ARDUPILOT_DIALECT)
void
_handleCameraFeedback
(
const
mavlink_message_t
&
message
);
void
_handleCameraFeedback
(
const
mavlink_message_t
&
message
);
...
@@ -1443,6 +1454,12 @@ private:
...
@@ -1443,6 +1454,12 @@ private:
QMap
<
QString
,
QTime
>
_noisySpokenPrearmMap
;
///< Used to prevent PreArm messages from being spoken too often
QMap
<
QString
,
QTime
>
_noisySpokenPrearmMap
;
///< Used to prevent PreArm messages from being spoken too often
// Orbit status values
bool
_orbitActive
;
QGCMapCircle
_orbitMapCircle
;
QTimer
_orbitTelemetryTimer
;
static
const
int
_orbitTelemetryTimeoutMsecs
=
3000
;
// No telemetry for this amount and orbit will go inactive
// FactGroup facts
// FactGroup facts
Fact
_rollFact
;
Fact
_rollFact
;
...
...
This diff is collapsed.
Click to expand it.
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