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
d910670d
Commit
d910670d
authored
Jul 29, 2020
by
DonLakeFlyer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
parent
ac51a6d2
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
206 additions
and
154 deletions
+206
-154
MissionController.cc
src/MissionManager/MissionController.cc
+122
-124
MissionController.h
src/MissionManager/MissionController.h
+23
-21
MissionControllerTest.cc
src/MissionManager/MissionControllerTest.cc
+59
-8
MissionControllerTest.h
src/MissionManager/MissionControllerTest.h
+2
-1
No files found.
src/MissionManager/MissionController.cc
View file @
d910670d
...
@@ -96,12 +96,9 @@ void MissionController::_resetMissionFlightStatus(void)
...
@@ -96,12 +96,9 @@ void MissionController::_resetMissionFlightStatus(void)
_missionFlightStatus
.
cruiseSpeed
=
_controllerVehicle
->
defaultCruiseSpeed
();
_missionFlightStatus
.
cruiseSpeed
=
_controllerVehicle
->
defaultCruiseSpeed
();
_missionFlightStatus
.
hoverSpeed
=
_controllerVehicle
->
defaultHoverSpeed
();
_missionFlightStatus
.
hoverSpeed
=
_controllerVehicle
->
defaultHoverSpeed
();
_missionFlightStatus
.
vehicleSpeed
=
_controllerVehicle
->
multiRotor
()
||
_managerVehicle
->
vtol
()
?
_missionFlightStatus
.
hoverSpeed
:
_missionFlightStatus
.
cruiseSpeed
;
_missionFlightStatus
.
vehicleSpeed
=
_controllerVehicle
->
multiRotor
()
||
_managerVehicle
->
vtol
()
?
_missionFlightStatus
.
hoverSpeed
:
_missionFlightStatus
.
cruiseSpeed
;
_missionFlightStatus
.
vehicleYaw
=
0.0
;
_missionFlightStatus
.
vehicleYaw
=
qQNaN
();
_missionFlightStatus
.
gimbalYaw
=
std
::
numeric_limits
<
double
>::
quiet_NaN
();
_missionFlightStatus
.
gimbalYaw
=
qQNaN
();
_missionFlightStatus
.
gimbalPitch
=
std
::
numeric_limits
<
double
>::
quiet_NaN
();
_missionFlightStatus
.
gimbalPitch
=
qQNaN
();
// Battery information
_missionFlightStatus
.
mAhBattery
=
0
;
_missionFlightStatus
.
mAhBattery
=
0
;
_missionFlightStatus
.
hoverAmps
=
0
;
_missionFlightStatus
.
hoverAmps
=
0
;
_missionFlightStatus
.
cruiseAmps
=
0
;
_missionFlightStatus
.
cruiseAmps
=
0
;
...
@@ -110,6 +107,7 @@ void MissionController::_resetMissionFlightStatus(void)
...
@@ -110,6 +107,7 @@ void MissionController::_resetMissionFlightStatus(void)
_missionFlightStatus
.
cruiseAmpsTotal
=
0
;
_missionFlightStatus
.
cruiseAmpsTotal
=
0
;
_missionFlightStatus
.
batteryChangePoint
=
-
1
;
_missionFlightStatus
.
batteryChangePoint
=
-
1
;
_missionFlightStatus
.
batteriesRequired
=
-
1
;
_missionFlightStatus
.
batteriesRequired
=
-
1
;
_missionFlightStatus
.
vtolMode
=
_missionContainsVTOLTakeoff
?
QGCMAVLink
::
VehicleClassMultiRotor
:
QGCMAVLink
::
VehicleClassFixedWing
;
_controllerVehicle
->
firmwarePlugin
()
->
batteryConsumptionData
(
_controllerVehicle
,
_missionFlightStatus
.
mAhBattery
,
_missionFlightStatus
.
hoverAmps
,
_missionFlightStatus
.
cruiseAmps
);
_controllerVehicle
->
firmwarePlugin
()
->
batteryConsumptionData
(
_controllerVehicle
,
_missionFlightStatus
.
mAhBattery
,
_missionFlightStatus
.
hoverAmps
,
_missionFlightStatus
.
cruiseAmps
);
if
(
_missionFlightStatus
.
mAhBattery
!=
0
)
{
if
(
_missionFlightStatus
.
mAhBattery
!=
0
)
{
...
@@ -904,11 +902,11 @@ bool MissionController::_loadItemsFromJson(const QJsonObject& json, QmlObjectLis
...
@@ -904,11 +902,11 @@ bool MissionController::_loadItemsFromJson(const QJsonObject& json, QmlObjectLis
int
fileVersion
;
int
fileVersion
;
JsonHelper
::
validateExternalQGCJsonFile
(
json
,
JsonHelper
::
validateExternalQGCJsonFile
(
json
,
_jsonFileTypeValue
,
// expected file type
_jsonFileTypeValue
,
// expected file type
1
,
// minimum supported version
1
,
// minimum supported version
2
,
// maximum supported version
2
,
// maximum supported version
fileVersion
,
fileVersion
,
errorString
);
errorString
);
if
(
fileVersion
==
1
)
{
if
(
fileVersion
==
1
)
{
return
_loadJsonMissionFileV1
(
json
,
visualItems
,
errorString
);
return
_loadJsonMissionFileV1
(
json
,
visualItems
,
errorString
);
...
@@ -1505,10 +1503,8 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1505,10 +1503,8 @@ void MissionController::_recalcMissionFlightStatus()
_resetMissionFlightStatus
();
_resetMissionFlightStatus
();
bool
vtolInHover
=
_missionContainsVTOLTakeoff
;
bool
linkStartToHome
=
false
;
bool
linkStartToHome
=
false
;
bool
foundRTL
=
false
;
bool
foundRTL
=
false
;
bool
vehicleYawSpecificallySet
=
false
;
double
totalHorizontalDistance
=
0
;
double
totalHorizontalDistance
=
0
;
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
...
@@ -1525,22 +1521,7 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1525,22 +1521,7 @@ void MissionController::_recalcMissionFlightStatus()
item
->
setDistance
(
0
);
item
->
setDistance
(
0
);
item
->
setDistanceFromStart
(
0
);
item
->
setDistanceFromStart
(
0
);
// Look for speed changed
// Gimbal states reflect the state AFTER executing the item
double
newSpeed
=
item
->
specifiedFlightSpeed
();
if
(
!
qIsNaN
(
newSpeed
))
{
if
(
_controllerVehicle
->
multiRotor
())
{
_missionFlightStatus
.
hoverSpeed
=
newSpeed
;
}
else
if
(
_controllerVehicle
->
vtol
())
{
if
(
vtolInHover
)
{
_missionFlightStatus
.
hoverSpeed
=
newSpeed
;
}
else
{
_missionFlightStatus
.
cruiseSpeed
=
newSpeed
;
}
}
else
{
_missionFlightStatus
.
cruiseSpeed
=
newSpeed
;
}
_missionFlightStatus
.
vehicleSpeed
=
newSpeed
;
}
// ROI commands cancel out previous gimbal yaw/pitch
// ROI commands cancel out previous gimbal yaw/pitch
if
(
simpleItem
)
{
if
(
simpleItem
)
{
...
@@ -1548,8 +1529,8 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1548,8 +1529,8 @@ void MissionController::_recalcMissionFlightStatus()
case
MAV_CMD_NAV_ROI
:
case
MAV_CMD_NAV_ROI
:
case
MAV_CMD_DO_SET_ROI_LOCATION
:
case
MAV_CMD_DO_SET_ROI_LOCATION
:
case
MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET
:
case
MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET
:
_missionFlightStatus
.
gimbalYaw
=
std
::
numeric_limits
<
double
>::
quiet_
NaN
();
_missionFlightStatus
.
gimbalYaw
=
qQ
NaN
();
_missionFlightStatus
.
gimbalPitch
=
std
::
numeric_limits
<
double
>::
quiet_
NaN
();
_missionFlightStatus
.
gimbalPitch
=
qQ
NaN
();
break
;
break
;
default:
default:
break
;
break
;
...
@@ -1566,28 +1547,115 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1566,28 +1547,115 @@ void MissionController::_recalcMissionFlightStatus()
_missionFlightStatus
.
gimbalPitch
=
gimbalPitch
;
_missionFlightStatus
.
gimbalPitch
=
gimbalPitch
;
}
}
if
(
i
==
0
)
{
// We don't need to do any more processing if:
// We only process speed and gimbal from Mission Settings item
// Mission Settings Item
continue
;
// We are after an RTL command
if
(
i
!=
0
&&
!
foundRTL
)
{
// We must set the mission flight status prior to querying for any values from the item. This is because things like
// current speed, gimbal, vtol state impact the values.
item
->
setMissionFlightStatus
(
_missionFlightStatus
);
// Link back to home if first item is takeoff and we have home position
if
(
firstCoordinateItem
&&
simpleItem
&&
(
simpleItem
->
mavCommand
()
==
MAV_CMD_NAV_TAKEOFF
||
simpleItem
->
mavCommand
()
==
MAV_CMD_NAV_VTOL_TAKEOFF
))
{
if
(
homePositionValid
)
{
linkStartToHome
=
true
;
if
(
_controllerVehicle
->
multiRotor
()
||
_controllerVehicle
->
vtol
())
{
// We have to special case takeoff, assuming vehicle takes off straight up to specified altitude
double
azimuth
,
distance
,
altDifference
;
_calcPrevWaypointValues
(
_settingsItem
,
simpleItem
,
&
azimuth
,
&
distance
,
&
altDifference
);
double
takeoffTime
=
qAbs
(
altDifference
)
/
_appSettings
->
offlineEditingAscentSpeed
()
->
rawValue
().
toDouble
();
_addHoverTime
(
takeoffTime
,
0
,
-
1
);
}
}
}
_addTimeDistance
(
_missionFlightStatus
.
vtolMode
==
QGCMAVLink
::
VehicleClassMultiRotor
,
0
,
0
,
item
->
additionalTimeDelay
(),
0
,
-
1
);
if
(
item
->
specifiesCoordinate
())
{
// Keep track of the min/max AMSL altitude for entire mission so we can calculate altitude percentages in terrain status display
if
(
simpleItem
)
{
double
amslAltitude
=
item
->
amslEntryAlt
();
_minAMSLAltitude
=
std
::
min
(
_minAMSLAltitude
,
amslAltitude
);
_maxAMSLAltitude
=
std
::
max
(
_maxAMSLAltitude
,
amslAltitude
);
}
else
{
// Complex item
double
complexMinAMSLAltitude
=
complexItem
->
minAMSLAltitude
();
double
complexMaxAMSLAltitude
=
complexItem
->
maxAMSLAltitude
();
_minAMSLAltitude
=
std
::
min
(
_minAMSLAltitude
,
complexMinAMSLAltitude
);
_maxAMSLAltitude
=
std
::
max
(
_maxAMSLAltitude
,
complexMaxAMSLAltitude
);
}
if
(
!
item
->
isStandaloneCoordinate
())
{
firstCoordinateItem
=
false
;
// Update vehicle yaw assuming direction to next waypoint and/or mission item change
if
(
simpleItem
)
{
double
newVehicleYaw
=
simpleItem
->
specifiedVehicleYaw
();
if
(
qIsNaN
(
newVehicleYaw
))
{
// No specific vehicle yaw set. Current vehicle yaw is determined from flight path segment direction.
if
(
simpleItem
!=
lastFlyThroughVI
)
{
_missionFlightStatus
.
vehicleYaw
=
lastFlyThroughVI
->
exitCoordinate
().
azimuthTo
(
simpleItem
->
coordinate
());
}
}
else
{
_missionFlightStatus
.
vehicleYaw
=
newVehicleYaw
;
}
simpleItem
->
setMissionVehicleYaw
(
_missionFlightStatus
.
vehicleYaw
);
}
if
(
lastFlyThroughVI
!=
_settingsItem
||
linkStartToHome
)
{
// This is a subsequent waypoint or we are forcing the first waypoint back to home
double
azimuth
,
distance
,
altDifference
;
_calcPrevWaypointValues
(
item
,
lastFlyThroughVI
,
&
azimuth
,
&
distance
,
&
altDifference
);
totalHorizontalDistance
+=
distance
;
item
->
setAltDifference
(
altDifference
);
item
->
setAzimuth
(
azimuth
);
item
->
setDistance
(
distance
);
item
->
setDistanceFromStart
(
totalHorizontalDistance
);
_missionFlightStatus
.
maxTelemetryDistance
=
qMax
(
_missionFlightStatus
.
maxTelemetryDistance
,
_calcDistanceToHome
(
item
,
_settingsItem
));
// Calculate time/distance
double
hoverTime
=
distance
/
_missionFlightStatus
.
hoverSpeed
;
double
cruiseTime
=
distance
/
_missionFlightStatus
.
cruiseSpeed
;
_addTimeDistance
(
_missionFlightStatus
.
vtolMode
==
QGCMAVLink
::
VehicleClassMultiRotor
,
hoverTime
,
cruiseTime
,
0
,
distance
,
item
->
sequenceNumber
());
}
if
(
complexItem
)
{
// Add in distance/time inside complex items as well
double
distance
=
complexItem
->
complexDistance
();
_missionFlightStatus
.
maxTelemetryDistance
=
qMax
(
_missionFlightStatus
.
maxTelemetryDistance
,
complexItem
->
greatestDistanceTo
(
complexItem
->
exitCoordinate
()));
double
hoverTime
=
distance
/
_missionFlightStatus
.
hoverSpeed
;
double
cruiseTime
=
distance
/
_missionFlightStatus
.
cruiseSpeed
;
_addTimeDistance
(
_missionFlightStatus
.
vtolMode
==
QGCMAVLink
::
VehicleClassMultiRotor
,
hoverTime
,
cruiseTime
,
0
,
distance
,
item
->
sequenceNumber
());
totalHorizontalDistance
+=
distance
;
}
lastFlyThroughVI
=
item
;
}
}
}
}
if
(
foundRTL
)
{
// Speed, VTOL states changes are processed last since they take affect on the next item
// No more vehicle distances after RTL
continue
;
double
newSpeed
=
item
->
specifiedFlightSpeed
();
}
if
(
!
qIsNaN
(
newSpeed
))
{
if
(
_controllerVehicle
->
multiRotor
())
{
// Link back to home if first item is takeoff and we have home position
_missionFlightStatus
.
hoverSpeed
=
newSpeed
;
if
(
firstCoordinateItem
&&
simpleItem
&&
(
simpleItem
->
mavCommand
()
==
MAV_CMD_NAV_TAKEOFF
||
simpleItem
->
mavCommand
()
==
MAV_CMD_NAV_VTOL_TAKEOFF
))
{
}
else
if
(
_controllerVehicle
->
vtol
())
{
if
(
homePositionValid
)
{
if
(
_missionFlightStatus
.
vtolMode
==
QGCMAVLink
::
VehicleClassMultiRotor
)
{
linkStartToHome
=
true
;
_missionFlightStatus
.
hoverSpeed
=
newSpeed
;
if
(
_controllerVehicle
->
multiRotor
()
||
_controllerVehicle
->
vtol
())
{
}
else
{
// We have to special case takeoff, assuming vehicle takes off straight up to specified altitude
_missionFlightStatus
.
cruiseSpeed
=
newSpeed
;
double
azimuth
,
distance
,
altDifference
;
_calcPrevWaypointValues
(
_settingsItem
,
simpleItem
,
&
azimuth
,
&
distance
,
&
altDifference
);
double
takeoffTime
=
qAbs
(
altDifference
)
/
_appSettings
->
offlineEditingAscentSpeed
()
->
rawValue
().
toDouble
();
_addHoverTime
(
takeoffTime
,
0
,
-
1
);
}
}
}
else
{
_missionFlightStatus
.
cruiseSpeed
=
newSpeed
;
}
}
_missionFlightStatus
.
vehicleSpeed
=
newSpeed
;
}
}
// Update VTOL state
// Update VTOL state
...
@@ -1595,21 +1663,19 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1595,21 +1663,19 @@ void MissionController::_recalcMissionFlightStatus()
switch
(
simpleItem
->
command
())
{
switch
(
simpleItem
->
command
())
{
case
MAV_CMD_NAV_TAKEOFF
:
// This will do a fixed wing style takeoff
case
MAV_CMD_NAV_TAKEOFF
:
// This will do a fixed wing style takeoff
case
MAV_CMD_NAV_VTOL_TAKEOFF
:
// Vehicle goes straight up and then transitions to FW
case
MAV_CMD_NAV_VTOL_TAKEOFF
:
// Vehicle goes straight up and then transitions to FW
vtolInHover
=
false
;
break
;
case
MAV_CMD_NAV_LAND
:
case
MAV_CMD_NAV_LAND
:
vtolInHover
=
false
;
_missionFlightStatus
.
vtolMode
=
QGCMAVLink
::
VehicleClassFixedWing
;
break
;
break
;
case
MAV_CMD_NAV_VTOL_LAND
:
case
MAV_CMD_NAV_VTOL_LAND
:
vtolInHover
=
true
;
_missionFlightStatus
.
vtolMode
=
QGCMAVLink
::
VehicleClassMultiRotor
;
break
;
break
;
case
MAV_CMD_DO_VTOL_TRANSITION
:
case
MAV_CMD_DO_VTOL_TRANSITION
:
{
{
int
transitionState
=
simpleItem
->
missionItem
().
param1
();
int
transitionState
=
simpleItem
->
missionItem
().
param1
();
if
(
transitionState
==
MAV_VTOL_STATE_TRANSITION_TO_MC
)
{
if
(
transitionState
==
MAV_VTOL_STATE_TRANSITION_TO_MC
)
{
vtolInHover
=
true
;
_missionFlightStatus
.
vtolMode
=
QGCMAVLink
::
VehicleClassMultiRotor
;
}
else
if
(
transitionState
==
MAV_VTOL_STATE_TRANSITION_TO_FW
)
{
}
else
if
(
transitionState
==
MAV_VTOL_STATE_TRANSITION_TO_FW
)
{
vtolInHover
=
false
;
_missionFlightStatus
.
vtolMode
=
QGCMAVLink
::
VehicleClassFixedWing
;
}
}
}
}
break
;
break
;
...
@@ -1617,74 +1683,6 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1617,74 +1683,6 @@ void MissionController::_recalcMissionFlightStatus()
break
;
break
;
}
}
}
}
_addTimeDistance
(
vtolInHover
,
0
,
0
,
item
->
additionalTimeDelay
(),
0
,
-
1
);
if
(
item
->
specifiesCoordinate
())
{
// Keep track of the min/max AMSL altitude for entire mission so we can calculate altitude percentages in terrain status display
if
(
simpleItem
)
{
double
amslAltitude
=
item
->
amslEntryAlt
();
_minAMSLAltitude
=
std
::
min
(
_minAMSLAltitude
,
amslAltitude
);
_maxAMSLAltitude
=
std
::
max
(
_maxAMSLAltitude
,
amslAltitude
);
}
else
{
// Complex item
double
complexMinAMSLAltitude
=
complexItem
->
minAMSLAltitude
();
double
complexMaxAMSLAltitude
=
complexItem
->
maxAMSLAltitude
();
_minAMSLAltitude
=
std
::
min
(
_minAMSLAltitude
,
complexMinAMSLAltitude
);
_maxAMSLAltitude
=
std
::
max
(
_maxAMSLAltitude
,
complexMaxAMSLAltitude
);
}
if
(
!
item
->
isStandaloneCoordinate
())
{
firstCoordinateItem
=
false
;
// Update vehicle yaw assuming direction to next waypoint and/or mission item change
if
(
item
!=
lastFlyThroughVI
)
{
if
(
simpleItem
&&
!
qIsNaN
(
simpleItem
->
specifiedVehicleYaw
()))
{
vehicleYawSpecificallySet
=
true
;
_missionFlightStatus
.
vehicleYaw
=
simpleItem
->
specifiedVehicleYaw
();
}
else
if
(
!
vehicleYawSpecificallySet
)
{
_missionFlightStatus
.
vehicleYaw
=
lastFlyThroughVI
->
exitCoordinate
().
azimuthTo
(
item
->
coordinate
());
}
lastFlyThroughVI
->
setMissionVehicleYaw
(
_missionFlightStatus
.
vehicleYaw
);
}
if
(
lastFlyThroughVI
!=
_settingsItem
||
linkStartToHome
)
{
// This is a subsequent waypoint or we are forcing the first waypoint back to home
double
azimuth
,
distance
,
altDifference
;
_calcPrevWaypointValues
(
item
,
lastFlyThroughVI
,
&
azimuth
,
&
distance
,
&
altDifference
);
totalHorizontalDistance
+=
distance
;
item
->
setAltDifference
(
altDifference
);
item
->
setAzimuth
(
azimuth
);
item
->
setDistance
(
distance
);
item
->
setDistanceFromStart
(
totalHorizontalDistance
);
_missionFlightStatus
.
maxTelemetryDistance
=
qMax
(
_missionFlightStatus
.
maxTelemetryDistance
,
_calcDistanceToHome
(
item
,
_settingsItem
));
// Calculate time/distance
double
hoverTime
=
distance
/
_missionFlightStatus
.
hoverSpeed
;
double
cruiseTime
=
distance
/
_missionFlightStatus
.
cruiseSpeed
;
_addTimeDistance
(
vtolInHover
,
hoverTime
,
cruiseTime
,
0
,
distance
,
item
->
sequenceNumber
());
}
if
(
complexItem
)
{
// Add in distance/time inside complex items as well
double
distance
=
complexItem
->
complexDistance
();
_missionFlightStatus
.
maxTelemetryDistance
=
qMax
(
_missionFlightStatus
.
maxTelemetryDistance
,
complexItem
->
greatestDistanceTo
(
complexItem
->
exitCoordinate
()));
double
hoverTime
=
distance
/
_missionFlightStatus
.
hoverSpeed
;
double
cruiseTime
=
distance
/
_missionFlightStatus
.
cruiseSpeed
;
_addTimeDistance
(
vtolInHover
,
hoverTime
,
cruiseTime
,
0
,
distance
,
item
->
sequenceNumber
());
totalHorizontalDistance
+=
distance
;
}
item
->
setMissionFlightStatus
(
_missionFlightStatus
);
lastFlyThroughVI
=
item
;
}
}
}
}
lastFlyThroughVI
->
setMissionVehicleYaw
(
_missionFlightStatus
.
vehicleYaw
);
lastFlyThroughVI
->
setMissionVehicleYaw
(
_missionFlightStatus
.
vehicleYaw
);
...
@@ -1697,7 +1695,7 @@ void MissionController::_recalcMissionFlightStatus()
...
@@ -1697,7 +1695,7 @@ void MissionController::_recalcMissionFlightStatus()
double
hoverTime
=
distance
/
_missionFlightStatus
.
hoverSpeed
;
double
hoverTime
=
distance
/
_missionFlightStatus
.
hoverSpeed
;
double
cruiseTime
=
distance
/
_missionFlightStatus
.
cruiseSpeed
;
double
cruiseTime
=
distance
/
_missionFlightStatus
.
cruiseSpeed
;
double
landTime
=
qAbs
(
altDifference
)
/
_appSettings
->
offlineEditingDescentSpeed
()
->
rawValue
().
toDouble
();
double
landTime
=
qAbs
(
altDifference
)
/
_appSettings
->
offlineEditingDescentSpeed
()
->
rawValue
().
toDouble
();
_addTimeDistance
(
vtolInHove
r
,
hoverTime
,
cruiseTime
,
distance
,
landTime
,
-
1
);
_addTimeDistance
(
_missionFlightStatus
.
vtolMode
==
QGCMAVLink
::
VehicleClassMultiRoto
r
,
hoverTime
,
cruiseTime
,
distance
,
landTime
,
-
1
);
}
}
if
(
_missionFlightStatus
.
mAhBattery
!=
0
&&
_missionFlightStatus
.
batteryChangePoint
==
-
1
)
{
if
(
_missionFlightStatus
.
mAhBattery
!=
0
&&
_missionFlightStatus
.
batteryChangePoint
==
-
1
)
{
...
...
src/MissionManager/MissionController.h
View file @
d910670d
...
@@ -45,27 +45,29 @@ public:
...
@@ -45,27 +45,29 @@ public:
~
MissionController
();
~
MissionController
();
typedef
struct
{
typedef
struct
{
double
maxTelemetryDistance
;
double
maxTelemetryDistance
;
double
totalDistance
;
double
totalDistance
;
double
totalTime
;
double
totalTime
;
double
hoverDistance
;
double
hoverDistance
;
double
hoverTime
;
double
hoverTime
;
double
cruiseDistance
;
double
cruiseDistance
;
double
cruiseTime
;
double
cruiseTime
;
double
cruiseSpeed
;
int
mAhBattery
;
///< 0 for not available
double
hoverSpeed
;
double
hoverAmps
;
///< Amp consumption during hover
double
vehicleSpeed
;
///< Either cruise or hover speed based on vehicle type and vtol state
double
cruiseAmps
;
///< Amp consumption during cruise
double
vehicleYaw
;
double
ampMinutesAvailable
;
///< Amp minutes available from single battery
double
gimbalYaw
;
///< NaN signals yaw was never changed
double
hoverAmpsTotal
;
///< Total hover amps used
double
gimbalPitch
;
///< NaN signals pitch was never changed
double
cruiseAmpsTotal
;
///< Total cruise amps used
int
mAhBattery
;
///< 0 for not available
int
batteryChangePoint
;
///< -1 for not supported, 0 for not needed
double
hoverAmps
;
///< Amp consumption during hover
int
batteriesRequired
;
///< -1 for not supported
double
cruiseAmps
;
///< Amp consumption during cruise
double
vehicleYaw
;
double
ampMinutesAvailable
;
///< Amp minutes available from single battery
double
gimbalYaw
;
///< NaN signals yaw was never changed
double
hoverAmpsTotal
;
///< Total hover amps used
double
gimbalPitch
;
///< NaN signals pitch was never changed
double
cruiseAmpsTotal
;
///< Total cruise amps used
// The following values are the state prior to executing this item
int
batteryChangePoint
;
///< -1 for not supported, 0 for not needed
QGCMAVLink
::
VehicleClass_t
vtolMode
;
///< Either VehicleClassFixedWing, VehicleClassMultiRotor, VehicleClassGeneric (mode unknown)
int
batteriesRequired
;
///< -1 for not supported
double
cruiseSpeed
;
double
hoverSpeed
;
double
vehicleSpeed
;
///< Either cruise or hover speed based on vehicle type and vtol state
}
MissionFlightStatus_t
;
}
MissionFlightStatus_t
;
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
...
...
src/MissionManager/MissionControllerTest.cc
View file @
d910670d
...
@@ -167,17 +167,68 @@ void MissionControllerTest::_testGimbalRecalc(void)
...
@@ -167,17 +167,68 @@ void MissionControllerTest::_testGimbalRecalc(void)
QVERIFY
(
qIsNaN
(
visualItem
->
missionGimbalYaw
()));
QVERIFY
(
qIsNaN
(
visualItem
->
missionGimbalYaw
()));
}
}
#if 0
// Specify gimbal yaw on settings item should generate yaw on all subsequent items
// FIXME: No longer works due to signal compression
const
int
yawIndex
=
2
;
// Specify gimbal yaw on settings item should generate yaw on all items
SimpleMissionItem
*
item
=
_missionController
->
visualItems
()
->
value
<
SimpleMissionItem
*>
(
yawIndex
);
MissionSettingsItem* settingsItem = _missionController->visualItems()->value<MissionSettingsItem*>(0
);
item
->
cameraSection
()
->
setSpecifyGimbal
(
true
);
settingsItem->cameraSection()->setSpecifyGimbal(true
);
item
->
cameraSection
()
->
gimbalYaw
()
->
setRawValue
(
0.0
);
settingsItem->cameraSection()->gimbalYaw()->setRawValue(0.0);
QTest
::
qWait
(
100
);
// Recalcs in MissionController are queued to remove dups. Allow return to main message loop.
for
(
int
i
=
1
;
i
<
_missionController
->
visualItems
()
->
count
();
i
++
)
{
for
(
int
i
=
1
;
i
<
_missionController
->
visualItems
()
->
count
();
i
++
)
{
//qDebug() << i;
VisualMissionItem
*
visualItem
=
_missionController
->
visualItems
()
->
value
<
VisualMissionItem
*>
(
i
);
VisualMissionItem
*
visualItem
=
_missionController
->
visualItems
()
->
value
<
VisualMissionItem
*>
(
i
);
QCOMPARE(visualItem->missionGimbalYaw(), 0.0);
if
(
i
>=
yawIndex
)
{
QCOMPARE
(
visualItem
->
missionGimbalYaw
(),
0.0
);
}
else
{
QVERIFY
(
qIsNaN
(
visualItem
->
missionGimbalYaw
()));
}
}
}
void
MissionControllerTest
::
_testVehicleYawRecalc
(
void
)
{
_initForFirmwareType
(
MAV_AUTOPILOT_PX4
);
double
wpDistance
=
1000
;
double
wpAngleInc
=
45
;
double
wpAngle
=
0
;
int
cMissionItems
=
4
;
QGeoCoordinate
currentCoord
(
0
,
0
);
_missionController
->
insertSimpleMissionItem
(
currentCoord
,
1
);
for
(
int
i
=
2
;
i
<=
cMissionItems
;
i
++
)
{
wpAngle
+=
wpAngleInc
;
currentCoord
=
currentCoord
.
atDistanceAndAzimuth
(
wpDistance
,
wpAngle
);
_missionController
->
insertSimpleMissionItem
(
currentCoord
,
i
);
}
QTest
::
qWait
(
100
);
// Recalcs in MissionController are queued to remove dups. Allow return to main message loop.
// No specific vehicle yaw set yet. Vehicle yaw should track flight path.
double
expectedVehicleYaw
=
wpAngleInc
;
for
(
int
i
=
2
;
i
<
cMissionItems
;
i
++
)
{
//qDebug() << i;
VisualMissionItem
*
visualItem
=
_missionController
->
visualItems
()
->
value
<
VisualMissionItem
*>
(
i
);
QCOMPARE
(
visualItem
->
missionVehicleYaw
(),
expectedVehicleYaw
);
if
(
i
<=
cMissionItems
-
1
)
{
expectedVehicleYaw
+=
wpAngleInc
;
}
}
SimpleMissionItem
*
simpleItem
=
_missionController
->
visualItems
()
->
value
<
SimpleMissionItem
*>
(
3
);
simpleItem
->
missionItem
().
setParam4
(
66
);
QTest
::
qWait
(
100
);
// Recalcs in MissionController are queued to remove dups. Allow return to main message loop.
// All item should track vehicle path except for the one changed
expectedVehicleYaw
=
wpAngleInc
;
for
(
int
i
=
2
;
i
<
cMissionItems
;
i
++
)
{
//qDebug() << i;
VisualMissionItem
*
visualItem
=
_missionController
->
visualItems
()
->
value
<
VisualMissionItem
*>
(
i
);
QCOMPARE
(
visualItem
->
missionVehicleYaw
(),
i
==
3
?
66.0
:
expectedVehicleYaw
);
if
(
i
<=
cMissionItems
-
1
)
{
expectedVehicleYaw
+=
wpAngleInc
;
}
}
}
#endif
}
}
void
MissionControllerTest
::
_testLoadJsonSectionAvailable
(
void
)
void
MissionControllerTest
::
_testLoadJsonSectionAvailable
(
void
)
...
...
src/MissionManager/MissionControllerTest.h
View file @
d910670d
...
@@ -32,11 +32,12 @@ public:
...
@@ -32,11 +32,12 @@ public:
private
slots
:
private
slots
:
void
cleanup
(
void
);
void
cleanup
(
void
);
void
_testGimbalRecalc
(
void
);
void
_testLoadJsonSectionAvailable
(
void
);
void
_testLoadJsonSectionAvailable
(
void
);
void
_testEmptyVehicleAPM
(
void
);
void
_testEmptyVehicleAPM
(
void
);
void
_testEmptyVehiclePX4
(
void
);
void
_testEmptyVehiclePX4
(
void
);
void
_testGlobalAltMode
(
void
);
void
_testGlobalAltMode
(
void
);
void
_testGimbalRecalc
(
void
);
void
_testVehicleYawRecalc
(
void
);
private:
private:
#if 0
#if 0
...
...
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