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
0b156cdd
Commit
0b156cdd
authored
Apr 25, 2017
by
DonLakeFlyer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Handle autopilot home position updates
parent
736947a2
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
18 additions
and
6 deletions
+18
-6
MissionManager.cc
src/MissionManager/MissionManager.cc
+18
-6
No files found.
src/MissionManager/MissionManager.cc
View file @
0b156cdd
...
@@ -409,11 +409,6 @@ void MissionManager::_requestNextMissionItem(void)
...
@@ -409,11 +409,6 @@ void MissionManager::_requestNextMissionItem(void)
void
MissionManager
::
_handleMissionItem
(
const
mavlink_message_t
&
message
,
bool
missionItemInt
)
void
MissionManager
::
_handleMissionItem
(
const
mavlink_message_t
&
message
,
bool
missionItemInt
)
{
{
if
(
!
_checkForExpectedAck
(
AckMissionItem
))
{
return
;
}
MAV_CMD
command
;
MAV_CMD
command
;
MAV_FRAME
frame
;
MAV_FRAME
frame
;
double
param1
;
double
param1
;
...
@@ -468,7 +463,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
...
@@ -468,7 +463,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
frame
=
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
frame
=
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
}
}
qCDebug
(
MissionManagerLog
)
<<
"_handleMissionItem seq:command:current"
<<
seq
<<
command
<<
isCurrentItem
;
bool
ardupilotHomePositionUpdate
=
false
;
if
(
!
_checkForExpectedAck
(
AckMissionItem
))
{
if
(
_vehicle
->
apmFirmware
()
&&
seq
==
0
)
{
ardupilotHomePositionUpdate
=
true
;
}
else
{
qCDebug
(
MissionManagerLog
)
<<
"_handleMissionItem dropping spurious item seq:command:current"
<<
seq
<<
command
<<
isCurrentItem
;
return
;
}
}
qCDebug
(
MissionManagerLog
)
<<
"_handleMissionItem seq:command:current:ardupilotHomePositionUpdate"
<<
seq
<<
command
<<
isCurrentItem
<<
ardupilotHomePositionUpdate
;
if
(
ardupilotHomePositionUpdate
)
{
QGeoCoordinate
newHomePosition
(
param5
,
param6
,
param7
);
_vehicle
->
_setHomePosition
(
newHomePosition
);
return
;
}
if
(
_itemIndicesToRead
.
contains
(
seq
))
{
if
(
_itemIndicesToRead
.
contains
(
seq
))
{
_itemIndicesToRead
.
removeOne
(
seq
);
_itemIndicesToRead
.
removeOne
(
seq
);
...
...
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