diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index ac122e954f84d3f445af230ca1bcaa4033025d52..8bb8ad3210d6a1ae3278053f2e253e5ed3e8c06b 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -189,9 +189,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque _addMissionSettings(_visualItems, _editMode && _visualItems->count() > 0 /* addToCenter */); } - if (_editMode) { - MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); - } + MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); _initAllVisualItems(); _updateContainsItems(); @@ -290,14 +288,14 @@ void MissionController::convertToKMLDocument(QDomDocument& document) continue; } const MissionCommandUIInfo* uiInfo = \ - qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command()); + qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command()); if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) { coord = QString::number(item->param6(),'f',7) \ - + "," \ - + QString::number(item->param5(),'f',7) \ - + "," \ - + QString::number(item->param7() + altitude,'f',2); + + "," \ + + QString::number(item->param5(),'f',7) \ + + "," \ + + QString::number(item->param7() + altitude,'f',2); coords.append(coord); } } @@ -1218,6 +1216,8 @@ void MissionController::_recalcMissionFlightStatus() } } + qDebug() << "Gimbal calc"; + for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); SimpleMissionItem* simpleItem = qobject_cast(item); @@ -1247,6 +1247,7 @@ void MissionController::_recalcMissionFlightStatus() // Look for gimbal change double gimbalYaw = item->specifiedGimbalYaw(); if (!qIsNaN(gimbalYaw)) { + qDebug() << _editMode << gimbalYaw; _missionFlightStatus.gimbalYaw = gimbalYaw; } double gimbalPitch = item->specifiedGimbalPitch(); @@ -1352,6 +1353,7 @@ void MissionController::_recalcMissionFlightStatus() _addTimeDistance(vtolInHover, hoverTime, cruiseTime, extraTime, distance, item->sequenceNumber()); } + qDebug() << "setMissionFlightStatus" << item->sequenceNumber() << _missionFlightStatus.gimbalYaw << item->commandName(); item->setMissionFlightStatus(_missionFlightStatus); } @@ -1783,8 +1785,10 @@ void MissionController::setDirty(bool dirty) void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle) { - // First we look for a Fixed Wing Landing Pattern which is at the end - FixedWingLandingComplexItem::scanForItem(visualItems, vehicle); + if (_editMode) { + // First we look for a Fixed Wing Landing Pattern which is at the end + FixedWingLandingComplexItem::scanForItem(visualItems, vehicle); + } int scanIndex = 0; while (scanIndex < visualItems->count()) { @@ -1792,11 +1796,13 @@ void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualIte qCDebug(MissionControllerLog) << "MissionController::_scanForAdditionalSettings count:scanIndex" << visualItems->count() << scanIndex; - MissionSettingsItem* settingsItem = qobject_cast(visualItem); - if (settingsItem) { - scanIndex++; - settingsItem->scanForMissionSettings(visualItems, scanIndex); - continue; + if (_editMode) { + MissionSettingsItem* settingsItem = qobject_cast(visualItem); + if (settingsItem) { + scanIndex++; + settingsItem->scanForMissionSettings(visualItems, scanIndex); + continue; + } } SimpleMissionItem* simpleItem = qobject_cast(visualItem); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 89eecafda2300165c0889d72adb6c34ceee72448..1f2818823862dcb9bd834af176958c60a2d82f2d 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -227,7 +227,7 @@ private: bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); bool _loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString); int _nextSequenceNumber(void); - static void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle); + void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle); static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent); void _setPlannedHomePositionFromFirstCoordinate(void); void _resetMissionFlightStatus(void);