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
ee83b76b
Commit
ee83b76b
authored
Aug 18, 2017
by
Don Gagne
Committed by
GitHub
Aug 18, 2017
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #5592 from DonLakeFlyer/ResumeMission
Resume mission fixes
parents
bad9ed57
a476cf58
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
55 additions
and
73 deletions
+55
-73
GuidedActionsController.qml
src/FlightDisplay/GuidedActionsController.qml
+4
-2
MissionManager.cc
src/MissionManager/MissionManager.cc
+51
-71
No files found.
src/FlightDisplay/GuidedActionsController.qml
View file @
ee83b76b
...
@@ -125,6 +125,8 @@ Item {
...
@@ -125,6 +125,8 @@ Item {
property
bool
__pauseVehicleSupported
:
_activeVehicle
?
_activeVehicle
.
pauseVehicleSupported
:
false
property
bool
__pauseVehicleSupported
:
_activeVehicle
?
_activeVehicle
.
pauseVehicleSupported
:
false
property
bool
__flightMode
:
_flightMode
property
bool
__flightMode
:
_flightMode
/*
Leaving this code in for use while debugging
function _outputState() {
function _outputState() {
console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode))
console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode))
}
}
...
@@ -139,16 +141,16 @@ Item {
...
@@ -139,16 +141,16 @@ Item {
on__PauseVehicleSupportedChanged: _outputState()
on__PauseVehicleSupportedChanged: _outputState()
// End of hack
// End of hack
*/
on_VehicleFlyingChanged
:
{
on_VehicleFlyingChanged
:
{
_outputState
()
//
_outputState()
if
(
!
_vehicleFlying
)
{
if
(
!
_vehicleFlying
)
{
// We use _vehicleWasFLying to help trigger Resume Mission only if the vehicle actually flew and came back down.
// We use _vehicleWasFLying to help trigger Resume Mission only if the vehicle actually flew and came back down.
// Otherwise it may trigger during the Start Mission sequence due to signal ordering or armed and resume mission index.
// Otherwise it may trigger during the Start Mission sequence due to signal ordering or armed and resume mission index.
_vehicleWasFlying
=
true
_vehicleWasFlying
=
true
}
}
}
}
property
var
_actionData
property
var
_actionData
on_CurrentMissionIndexChanged
:
console
.
log
(
"
_currentMissionIndex
"
,
_currentMissionIndex
)
on_CurrentMissionIndexChanged
:
console
.
log
(
"
_currentMissionIndex
"
,
_currentMissionIndex
)
...
...
src/MissionManager/MissionManager.cc
View file @
ee83b76b
...
@@ -988,7 +988,23 @@ void MissionManager::generateResumeMission(int resumeIndex)
...
@@ -988,7 +988,23 @@ void MissionManager::generateResumeMission(int resumeIndex)
}
}
}
}
resumeIndex
=
qMin
(
resumeIndex
,
_missionItems
.
count
()
-
1
);
// Be anal about crap input
resumeIndex
=
qMax
(
0
,
qMin
(
resumeIndex
,
_missionItems
.
count
()
-
1
));
// Adjust resume index to be a location based command
const
MissionCommandUIInfo
*
uiInfo
=
qgcApp
()
->
toolbox
()
->
missionCommandTree
()
->
getUIInfo
(
_vehicle
,
_missionItems
[
resumeIndex
]
->
command
());
if
(
!
uiInfo
||
uiInfo
->
isStandaloneCoordinate
()
||
!
uiInfo
->
specifiesCoordinate
())
{
// We have to back up to the last command which the vehicle flies through
while
(
--
resumeIndex
>
0
)
{
uiInfo
=
qgcApp
()
->
toolbox
()
->
missionCommandTree
()
->
getUIInfo
(
_vehicle
,
_missionItems
[
resumeIndex
]
->
command
());
if
(
uiInfo
&&
(
uiInfo
->
specifiesCoordinate
()
&&
!
uiInfo
->
isStandaloneCoordinate
()))
{
// Found it
break
;
}
}
}
resumeIndex
=
qMax
(
0
,
resumeIndex
);
int
seqNum
=
0
;
int
seqNum
=
0
;
QList
<
MissionItem
*>
resumeMission
;
QList
<
MissionItem
*>
resumeMission
;
...
@@ -1008,20 +1024,19 @@ void MissionManager::generateResumeMission(int resumeIndex)
...
@@ -1008,20 +1024,19 @@ void MissionManager::generateResumeMission(int resumeIndex)
<<
MAV_CMD_IMAGE_STOP_CAPTURE
<<
MAV_CMD_IMAGE_STOP_CAPTURE
<<
MAV_CMD_VIDEO_START_CAPTURE
<<
MAV_CMD_VIDEO_START_CAPTURE
<<
MAV_CMD_VIDEO_STOP_CAPTURE
<<
MAV_CMD_VIDEO_STOP_CAPTURE
<<
MAV_CMD_DO_CHANGE_SPEED
;
<<
MAV_CMD_DO_CHANGE_SPEED
if
(
_vehicle
->
fixedWing
()
&&
_vehicle
->
px4Firmware
())
{
<<
MAV_CMD_SET_CAMERA_MODE
includedResumeCommands
<<
MAV_CMD_NAV_TAKEOFF
;
<<
MAV_CMD_NAV_TAKEOFF
;
}
bool
addHomePosition
=
_vehicle
->
firmwarePlugin
()
->
sendHomePositionToVehicle
();
bool
addHomePosition
=
_vehicle
->
firmwarePlugin
()
->
sendHomePositionToVehicle
();
int
setCurrentIndex
=
addHomePosition
?
1
:
0
;
int
setCurrentIndex
=
addHomePosition
?
1
:
0
;
int
resume
CommandCount
=
0
;
int
prefix
CommandCount
=
0
;
for
(
int
i
=
0
;
i
<
_missionItems
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_missionItems
.
count
();
i
++
)
{
MissionItem
*
oldItem
=
_missionItems
[
i
];
MissionItem
*
oldItem
=
_missionItems
[
i
];
if
((
i
==
0
&&
addHomePosition
)
||
i
>=
resumeIndex
||
includedResumeCommands
.
contains
(
oldItem
->
command
()))
{
if
((
i
==
0
&&
addHomePosition
)
||
i
>=
resumeIndex
||
includedResumeCommands
.
contains
(
oldItem
->
command
()))
{
if
(
i
<
resumeIndex
)
{
if
(
i
<
resumeIndex
)
{
resume
CommandCount
++
;
prefix
CommandCount
++
;
}
}
MissionItem
*
newItem
=
new
MissionItem
(
*
oldItem
,
this
);
MissionItem
*
newItem
=
new
MissionItem
(
*
oldItem
,
this
);
newItem
->
setIsCurrentItem
(
i
==
setCurrentIndex
);
newItem
->
setIsCurrentItem
(
i
==
setCurrentIndex
);
...
@@ -1029,91 +1044,56 @@ void MissionManager::generateResumeMission(int resumeIndex)
...
@@ -1029,91 +1044,56 @@ void MissionManager::generateResumeMission(int resumeIndex)
resumeMission
.
append
(
newItem
);
resumeMission
.
append
(
newItem
);
}
}
}
}
prefixCommandCount
=
qMax
(
0
,
qMin
(
prefixCommandCount
,
resumeMission
.
count
()));
// Anal prevention against crashes
// De-dup and remove no-ops from the commands which were added to the front of the mission
// De-dup and remove no-ops from the commands which were added to the front of the mission
bool
foundROI
=
false
;
bool
foundROI
=
false
;
bool
foundCamTrigDist
=
false
;
bool
foundCameraSetMode
=
false
;
QList
<
int
>
imageStartCameraIds
;
bool
foundCameraStartStop
=
false
;
QList
<
int
>
imageStopCameraIds
;
prefixCommandCount
--
;
// Change from count to array index
QList
<
int
>
videoStartCameraIds
;
while
(
prefixCommandCount
>=
0
)
{
QList
<
int
>
videoStopCameraIds
;
MissionItem
*
resumeItem
=
resumeMission
[
prefixCommandCount
];
while
(
resumeIndex
>=
0
)
{
MissionItem
*
resumeItem
=
resumeMission
[
resumeIndex
];
switch
(
resumeItem
->
command
())
{
switch
(
resumeItem
->
command
())
{
case
MAV_CMD_SET_CAMERA_MODE
:
// Only keep the last one
if
(
foundCameraSetMode
)
{
resumeMission
.
removeAt
(
prefixCommandCount
);
}
foundCameraSetMode
=
true
;
break
;
case
MAV_CMD_DO_SET_ROI
:
case
MAV_CMD_DO_SET_ROI
:
// Only keep the last one
// Only keep the last one
if
(
foundROI
)
{
if
(
foundROI
)
{
resumeMission
.
removeAt
(
resumeIndex
);
resumeMission
.
removeAt
(
prefixCommandCount
);
}
}
foundROI
=
true
;
foundROI
=
true
;
break
;
break
;
case
MAV_CMD_DO_SET_CAM_TRIGG_DIST
:
case
MAV_CMD_DO_SET_CAM_TRIGG_DIST
:
// Only keep the last one
if
(
foundCamTrigDist
)
{
resumeMission
.
removeAt
(
resumeIndex
);
}
foundCamTrigDist
=
true
;
break
;
case
MAV_CMD_IMAGE_START_CAPTURE
:
{
// FIXME: Handle single image capture
int
cameraId
=
resumeItem
->
param1
();
if
(
resumeItem
->
param3
()
==
1
)
{
// This is an individual image capture command, remove it
resumeMission
.
removeAt
(
resumeIndex
);
break
;
}
// If we already found an image stop, then all image start/stop commands are useless
// De-dup repeated image start commands
// Otherwise keep only the last image start
if
(
imageStopCameraIds
.
contains
(
cameraId
)
||
imageStartCameraIds
.
contains
(
cameraId
))
{
resumeMission
.
removeAt
(
resumeIndex
);
}
if
(
!
imageStopCameraIds
.
contains
(
cameraId
))
{
imageStopCameraIds
.
append
(
cameraId
);
}
}
break
;
case
MAV_CMD_IMAGE_STOP_CAPTURE
:
case
MAV_CMD_IMAGE_STOP_CAPTURE
:
{
int
cameraId
=
resumeItem
->
param1
();
// Image stop only matters to kill all previous image starts
if
(
!
imageStopCameraIds
.
contains
(
cameraId
))
{
imageStopCameraIds
.
append
(
cameraId
);
}
resumeMission
.
removeAt
(
resumeIndex
);
}
break
;
case
MAV_CMD_VIDEO_START_CAPTURE
:
case
MAV_CMD_VIDEO_START_CAPTURE
:
{
case
MAV_CMD_VIDEO_STOP_CAPTURE
:
int
cameraId
=
resumeItem
->
param1
();
// Only keep the first of these commands that are found
// If we've already found a video stop, then all video start/stop commands are useless
if
(
foundCameraStartStop
)
{
// De-dup repeated video start commands
resumeMission
.
removeAt
(
prefixCommandCount
);
// Otherwise keep only the last video start
if
(
videoStopCameraIds
.
contains
(
cameraId
)
||
videoStopCameraIds
.
contains
(
cameraId
))
{
resumeMission
.
removeAt
(
resumeIndex
);
}
if
(
!
videoStopCameraIds
.
contains
(
cameraId
))
{
videoStopCameraIds
.
append
(
cameraId
);
}
}
}
foundCameraStartStop
=
true
;
break
;
break
;
case
MAV_CMD_VIDEO_STOP_CAPTURE
:
case
MAV_CMD_IMAGE_START_CAPTURE
:
{
// Only keep the first of these commands that are found
int
cameraId
=
resumeItem
->
param1
();
if
(
foundCameraStartStop
)
{
// Video stop only matters to kill all previous video starts
resumeMission
.
removeAt
(
prefixCommandCount
);
if
(
!
videoStopCameraIds
.
contains
(
cameraId
))
{
videoStopCameraIds
.
append
(
cameraId
);
}
}
resumeMission
.
removeAt
(
resumeIndex
);
if
(
resumeItem
->
param3
()
!=
0
)
{
}
// Remove commands which do no trigger by time
resumeMission
.
removeAt
(
prefixCommandCount
);
}
foundCameraStartStop
=
true
;
break
;
break
;
default:
default:
break
;
break
;
}
}
resumeIndex
--
;
prefixCommandCount
--
;
}
}
// Send to vehicle
// Send to vehicle
...
...
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