@ -18,6 +18,7 @@
@@ -18,6 +18,7 @@
# include "ComplexMissionItem.h"
# include "JsonHelper.h"
# include "ParameterLoader.h"
# include "QGroundControlQmlGlobal.h"
# ifndef __mobile__
# include "QGCFileDialog.h"
@ -44,6 +45,10 @@ MissionController::MissionController(QObject *parent)
@@ -44,6 +45,10 @@ MissionController::MissionController(QObject *parent)
, _firstItemsFromVehicle ( false )
, _missionItemsRequested ( false )
, _queuedSend ( false )
, _missionDistance ( 0.0 )
, _missionMaxTelemetry ( 0.0 )
, _cruiseDistance ( 0.0 )
, _hoverDistance ( 0.0 )
{
}
@ -602,6 +607,23 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
@@ -602,6 +607,23 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
}
}
void MissionController : : _calcHomeDist ( VisualMissionItem * currentItem , VisualMissionItem * homeItem , double * distance )
{
QGeoCoordinate currentCoord = currentItem - > coordinate ( ) ;
QGeoCoordinate homeCoord = homeItem - > exitCoordinate ( ) ;
bool distanceOk = false ;
distanceOk = true ;
qCDebug ( MissionControllerLog ) < < " distanceOk " < < distanceOk ;
if ( distanceOk ) {
* distance = homeCoord . distanceTo ( currentCoord ) ;
} else {
* distance = 0.0 ;
}
}
void MissionController : : _recalcWaypointLines ( void )
{
bool firstCoordinateItem = true ;
@ -692,7 +714,6 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -692,7 +714,6 @@ void MissionController::_recalcAltitudeRangeBearing()
bool firstCoordinateItem = true ;
VisualMissionItem * lastCoordinateItem = qobject_cast < VisualMissionItem * > ( _visualItems - > get ( 0 ) ) ;
SimpleMissionItem * homeItem = qobject_cast < SimpleMissionItem * > ( lastCoordinateItem ) ;
if ( ! homeItem ) {
@ -717,19 +738,50 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -717,19 +738,50 @@ void MissionController::_recalcAltitudeRangeBearing()
const double homePositionAltitude = homeItem - > coordinate ( ) . altitude ( ) ;
minAltSeen = maxAltSeen = homeItem - > coordinate ( ) . altitude ( ) ;
double missionDistance = 0.0 ;
double missionMaxTelemetry = 0.0 ;
bool vtolCalc = ( QGroundControlQmlGlobal : : offlineEditingVehicleType ( ) - > enumStringValue ( ) = = " VTOL " | | ( _activeVehicle & & _activeVehicle - > vtol ( ) ) ) ? true : false ;
double cruiseDistance = 0.0 ;
double hoverDistance = 0.0 ;
bool hoverDistanceCalc = false ;
bool hoverTransition = false ;
bool cruiseTransition = false ;
bool hoverDistanceReset = false ;
bool linkBackToHome = false ;
for ( int i = 1 ; i < _visualItems - > count ( ) ; i + + ) {
VisualMissionItem * item = qobject_cast < VisualMissionItem * > ( _visualItems - > get ( i ) ) ;
// Assume the worst
item - > setAzimuth ( 0.0 ) ;
item - > setDistance ( 0.0 ) ;
// If we still haven't found the first coordinate item and we hit a a takeoff command link back to home
// If we still haven't found the first coordinate item and we hit a takeoff command link back to home
if ( firstCoordinateItem & &
item - > isSimpleItem ( ) & &
qobject_cast < SimpleMissionItem * > ( item ) - > command ( ) = = MavlinkQmlSingleton : : MAV_CMD_NAV_TAKEOFF ) {
linkBackToHome = true ;
hoverDistanceCalc = true ;
}
if ( item - > isSimpleItem ( ) & & vtolCalc ) {
SimpleMissionItem * simpleItem = qobject_cast < SimpleMissionItem * > ( item ) ;
if ( simpleItem - > command ( ) = = MavlinkQmlSingleton : : MAV_CMD_DO_VTOL_TRANSITION ) { //transition waypoint value
if ( simpleItem - > missionItem ( ) . param1 ( ) = = 3 ) { //hover mode value
hoverDistanceCalc = true ;
hoverTransition = true ;
}
else if ( simpleItem - > missionItem ( ) . param1 ( ) = = 4 ) {
hoverDistanceCalc = false ;
cruiseTransition = true ;
}
}
if ( ! hoverTransition & & cruiseTransition & & ! hoverDistanceReset & & ! linkBackToHome ) {
hoverDistance = missionDistance ;
hoverDistanceReset = true ;
}
}
if ( item - > specifiesCoordinate ( ) ) {
@ -754,7 +806,7 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -754,7 +806,7 @@ void MissionController::_recalcAltitudeRangeBearing()
if ( ! item - > isStandaloneCoordinate ( ) ) {
firstCoordinateItem = false ;
if ( lastCoordinateItem ! = homeItem | | ( showHomePosition & & linkBackToHome ) ) {
double azimuth , distance , altDifference ;
double azimuth , distance , altDifference , telemetryDistance ;
// Subsequent coordinate items link to last coordinate item. If the last coordinate item
// is an invalid home position we skip the line
@ -762,12 +814,46 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -762,12 +814,46 @@ void MissionController::_recalcAltitudeRangeBearing()
item - > setAltDifference ( altDifference ) ;
item - > setAzimuth ( azimuth ) ;
item - > setDistance ( distance ) ;
missionDistance + = distance ;
if ( item - > isSimpleItem ( ) ) {
_calcHomeDist ( item , homeItem , & telemetryDistance ) ;
if ( vtolCalc ) {
SimpleMissionItem * simpleItem = qobject_cast < SimpleMissionItem * > ( item ) ;
if ( simpleItem - > command ( ) = = MavlinkQmlSingleton : : MAV_CMD_NAV_TAKEOFF | | hoverDistanceCalc ) {
hoverDistance + = distance ;
}
cruiseDistance = missionDistance - hoverDistance ;
if ( simpleItem - > command ( ) = = MavlinkQmlSingleton : : MAV_CMD_NAV_LAND & & ! linkBackToHome & & ! cruiseTransition & & ! hoverTransition ) {
hoverDistance = cruiseDistance ;
cruiseDistance = missionDistance - hoverDistance ;
}
}
} else {
missionDistance + = qobject_cast < ComplexMissionItem * > ( item ) - > surveyDistance ( ) ;
telemetryDistance = qobject_cast < ComplexMissionItem * > ( item ) - > greatestDistanceTo ( homeItem - > exitCoordinate ( ) ) ;
if ( vtolCalc ) {
cruiseDistance + = qobject_cast < ComplexMissionItem * > ( item ) - > surveyDistance ( ) ; //assume all survey missions undertaken in cruise
}
}
if ( telemetryDistance > missionMaxTelemetry ) {
missionMaxTelemetry = telemetryDistance ;
}
}
lastCoordinateItem = item ;
}
}
}
setMissionDistance ( missionDistance ) ;
setMissionMaxTelemetry ( missionMaxTelemetry ) ;
setCruiseDistance ( cruiseDistance ) ;
setHoverDistance ( hoverDistance ) ;
// Walk the list again calculating altitude percentages
double altRange = maxAltSeen - minAltSeen ;
for ( int i = 0 ; i < _visualItems - > count ( ) ; i + + ) {
@ -921,6 +1007,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
@@ -921,6 +1007,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
// We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items
ComplexMissionItem * complexItem = qobject_cast < ComplexMissionItem * > ( visualItem ) ;
connect ( complexItem , & ComplexMissionItem : : lastSequenceNumberChanged , this , & MissionController : : _recalcSequence ) ;
connect ( complexItem , & ComplexMissionItem : : surveyDistanceChanged , this , & MissionController : : _recalcAltitudeRangeBearing ) ;
}
}
@ -1017,6 +1104,38 @@ void MissionController::setAutoSync(bool autoSync)
@@ -1017,6 +1104,38 @@ void MissionController::setAutoSync(bool autoSync)
# endif
}
void MissionController : : setMissionDistance ( double missionDistance )
{
if ( ! qFuzzyCompare ( _missionDistance , missionDistance ) ) {
_missionDistance = missionDistance ;
emit missionDistanceChanged ( _missionDistance ) ;
}
}
void MissionController : : setMissionMaxTelemetry ( double missionMaxTelemetry )
{
if ( ! qFuzzyCompare ( _missionMaxTelemetry , missionMaxTelemetry ) ) {
_missionMaxTelemetry = missionMaxTelemetry ;
emit missionMaxTelemetryChanged ( _missionMaxTelemetry ) ;
}
}
void MissionController : : setCruiseDistance ( double cruiseDistance )
{
if ( ! qFuzzyCompare ( _cruiseDistance , cruiseDistance ) ) {
_cruiseDistance = cruiseDistance ;
emit cruiseDistanceChanged ( _cruiseDistance ) ;
}
}
void MissionController : : setHoverDistance ( double hoverDistance )
{
if ( ! qFuzzyCompare ( _hoverDistance , hoverDistance ) ) {
_hoverDistance = hoverDistance ;
emit hoverDistanceChanged ( _hoverDistance ) ;
}
}
void MissionController : : _dirtyChanged ( bool dirty )
{
if ( dirty & & _autoSync ) {
@ -1051,7 +1170,7 @@ void MissionController::_inProgressChanged(bool inProgress)
@@ -1051,7 +1170,7 @@ void MissionController::_inProgressChanged(bool inProgress)
bool MissionController : : _findLastAltitude ( double * lastAltitude , MAV_FRAME * frame )
{
bool found = false ;
bool found = false ;
double foundAltitude ;
MAV_FRAME foundFrame ;