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
d4303c21
Commit
d4303c21
authored
Nov 29, 2010
by
Mariano Lizarraga
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Trying to get the waypoint protocol to work inside SlugsMav so not to call UAS::receiveMessage()
parent
0b761f97
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
53 additions
and
9 deletions
+53
-9
SlugsMAV.cc
src/uas/SlugsMAV.cc
+53
-9
No files found.
src/uas/SlugsMAV.cc
View file @
d4303c21
...
...
@@ -59,16 +59,14 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
void
SlugsMAV
::
receiveMessage
(
LinkInterface
*
link
,
mavlink_message_t
message
)
{
// Let UAS handle the default message set
//
UAS::receiveMessage(link, message);
//
UAS::receiveMessage(link, message);
// Handle your special messages mavlink_message_t* msg = &message;
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_HEARTBEAT
:
case
MAVLINK_MSG_ID_HEARTBEAT
:
emit
heartbeat
(
this
);
// Set new type if it has changed
if
(
this
->
type
!=
mavlink_msg_heartbeat_get_type
(
&
message
))
...
...
@@ -80,7 +78,57 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
case
MAVLINK_MSG_ID_RAW_IMU
:
mavlink_msg_raw_imu_decode
(
&
message
,
&
mlRawImuData
);
break
;
break
;
case
MAVLINK_MSG_ID_WAYPOINT_COUNT
:
mavlink_waypoint_count_t
wpct
;
mavlink_msg_waypoint_count_decode
(
&
message
,
&
wpct
);
if
(
wpct
.
target_system
==
mavlink
->
getSystemId
()
&&
wpct
.
target_component
==
mavlink
->
getComponentId
())
{
waypointManager
.
handleWaypointCount
(
message
.
sysid
,
message
.
compid
,
wpct
.
count
);
}
break
;
case
MAVLINK_MSG_ID_WAYPOINT
:
mavlink_waypoint_t
wp
;
mavlink_msg_waypoint_decode
(
&
message
,
&
wp
);
//qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
if
(
wp
.
target_system
==
mavlink
->
getSystemId
()
&&
wp
.
target_component
==
mavlink
->
getComponentId
())
{
waypointManager
.
handleWaypoint
(
message
.
sysid
,
message
.
compid
,
&
wp
);
}
break
;
case
MAVLINK_MSG_ID_WAYPOINT_ACK
:
mavlink_waypoint_ack_t
wpa
;
mavlink_msg_waypoint_ack_decode
(
&
message
,
&
wpa
);
if
(
wpa
.
target_system
==
mavlink
->
getSystemId
()
&&
wpa
.
target_component
==
mavlink
->
getComponentId
())
{
waypointManager
.
handleWaypointAck
(
message
.
sysid
,
message
.
compid
,
&
wpa
);
}
break
;
case
MAVLINK_MSG_ID_WAYPOINT_REQUEST
:
mavlink_waypoint_request_t
wpr
;
mavlink_msg_waypoint_request_decode
(
&
message
,
&
wpr
);
qDebug
()
<<
"Waypoint Request"
<<
wpr
.
seq
;
if
(
wpr
.
target_system
==
mavlink
->
getSystemId
()
&&
wpr
.
target_component
==
mavlink
->
getComponentId
())
{
waypointManager
.
handleWaypointRequest
(
message
.
sysid
,
message
.
compid
,
&
wpr
);
}
break
;
case
MAVLINK_MSG_ID_WAYPOINT_REACHED
:
mavlink_waypoint_reached_t
wpre
;
mavlink_msg_waypoint_reached_decode
(
&
message
,
&
wpre
);
waypointManager
.
handleWaypointReached
(
message
.
sysid
,
message
.
compid
,
&
wpre
);
break
;
case
MAVLINK_MSG_ID_WAYPOINT_CURRENT
:
mavlink_waypoint_current_t
wpc
;
mavlink_msg_waypoint_current_decode
(
&
message
,
&
wpc
);
waypointManager
.
handleWaypointCurrent
(
message
.
sysid
,
message
.
compid
,
&
wpc
);
break
;
#ifdef MAVLINK_ENABLED_SLUGS
...
...
@@ -89,8 +137,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
slugsBootMsg
(
uasId
,
mlBoot
);
break
;
case
MAVLINK_MSG_ID_ATTITUDE
:
mavlink_msg_attitude_decode
(
&
message
,
&
mlAttitude
);
break
;
...
...
@@ -103,8 +149,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_msg_action_ack_decode
(
&
message
,
&
mlActionAck
);
break
;
case
MAVLINK_MSG_ID_CPU_LOAD
:
//170
mavlink_msg_cpu_load_decode
(
&
message
,
&
mlCpuLoadData
);
break
;
...
...
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