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
875e2514
Commit
875e2514
authored
Jul 17, 2017
by
Beat Küng
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
AirMapManager: send velocity & HDOP Telemetry
parent
01d5caa7
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
50 additions
and
4 deletions
+50
-4
AirMapManager.cc
src/MissionManager/AirMapManager.cc
+45
-4
AirMapManager.h
src/MissionManager/AirMapManager.h
+5
-0
No files found.
src/MissionManager/AirMapManager.cc
View file @
875e2514
...
...
@@ -589,13 +589,37 @@ void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& mes
case
MAVLINK_MSG_ID_GLOBAL_POSITION_INT
:
_handleGlobalPositionInt
(
message
);
break
;
case
MAVLINK_MSG_ID_GPS_RAW_INT
:
_handleGPSRawInt
(
message
);
break
;
}
}
bool
AirMapTelemetry
::
_isTelemetryStreaming
()
const
{
return
_state
==
State
::
Streaming
&&
!
_udpHost
.
isNull
();
}
void
AirMapTelemetry
::
_handleGPSRawInt
(
const
mavlink_message_t
&
message
)
{
if
(
!
_isTelemetryStreaming
())
{
return
;
}
mavlink_gps_raw_int_t
gps_raw
;
mavlink_msg_gps_raw_int_decode
(
&
message
,
&
gps_raw
);
if
(
gps_raw
.
eph
==
UINT16_MAX
)
{
_lastHdop
=
1.
f
;
}
else
{
_lastHdop
=
gps_raw
.
eph
/
100.
f
;
}
}
void
AirMapTelemetry
::
_handleGlobalPositionInt
(
const
mavlink_message_t
&
message
)
{
if
(
_state
!=
State
::
Streaming
||
_udpHost
.
isNull
())
{
if
(
!
_isTelemetryStreaming
())
{
return
;
}
...
...
@@ -630,22 +654,39 @@ void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message)
packetHeaderLength
+=
sizeof
(
iv
);
// write payload
// Position message
airmap
::
telemetry
::
Position
position
;
position
.
set_timestamp
(
QDateTime
::
currentMSecsSinceEpoch
());
position
.
set_latitude
((
double
)
globalPosition
.
lat
/
1e7
);
position
.
set_longitude
((
double
)
globalPosition
.
lon
/
1e7
);
position
.
set_altitude_msl
((
float
)
globalPosition
.
alt
/
1000.
f
);
position
.
set_altitude_agl
((
float
)
globalPosition
.
relative_alt
/
1000.
f
);
position
.
set_horizontal_accuracy
(
1.
);
position
.
set_horizontal_accuracy
(
_lastHdop
);
uint16_t
msgID
=
1
;
// Position: 1, Attitude: 2, Speed: 3, Barometer: 4
uint16_t
msgLength
=
position
.
ByteSize
();
qToBigEndian
(
msgID
,
payload
);
qToBigEndian
(
msgLength
,
payload
+
sizeof
(
msgID
));
qToBigEndian
(
msgID
,
payload
+
payloadLength
);
qToBigEndian
(
msgLength
,
payload
+
payloadLength
+
sizeof
(
msgID
));
payloadLength
+=
sizeof
(
msgID
)
+
sizeof
(
msgLength
);
position
.
SerializeToArray
(
payload
+
payloadLength
,
msgLength
);
payloadLength
+=
msgLength
;
// Speed message
airmap
::
telemetry
::
Speed
speed
;
speed
.
set_timestamp
(
QDateTime
::
currentMSecsSinceEpoch
());
speed
.
set_velocity_x
(
globalPosition
.
vx
/
100.
f
);
speed
.
set_velocity_y
(
globalPosition
.
vy
/
100.
f
);
speed
.
set_velocity_z
(
globalPosition
.
vz
/
100.
f
);
msgID
=
3
;
// Position: 1, Attitude: 2, Speed: 3, Barometer: 4
msgLength
=
speed
.
ByteSize
();
qToBigEndian
(
msgID
,
payload
+
payloadLength
);
qToBigEndian
(
msgLength
,
payload
+
payloadLength
+
sizeof
(
msgID
));
payloadLength
+=
sizeof
(
msgID
)
+
sizeof
(
msgLength
);
speed
.
SerializeToArray
(
payload
+
payloadLength
,
msgLength
);
payloadLength
+=
msgLength
;
// pad to 16 bytes if necessary
int
padding
=
16
-
(
payloadLength
%
16
);
if
(
padding
!=
16
)
{
...
...
src/MissionManager/AirMapManager.h
View file @
875e2514
...
...
@@ -291,6 +291,9 @@ private slots:
private:
void
_handleGlobalPositionInt
(
const
mavlink_message_t
&
message
);
void
_handleGPSRawInt
(
const
mavlink_message_t
&
message
);
bool
_isTelemetryStreaming
()
const
;
enum
class
State
{
Idle
,
...
...
@@ -308,6 +311,8 @@ private:
QUdpSocket
*
_socket
=
nullptr
;
QHostAddress
_udpHost
;
static
constexpr
int
_udpPort
=
16060
;
float
_lastHdop
=
1
.
f
;
};
/// AirMap server communication support.
...
...
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