diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 3af285e00b90a4e924184072459b985780fee982..0ae992bd7a454840d2687ec97d6a1593b5609085 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -134,19 +134,6 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle) return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0; } -bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const -{ - if (vehicle->isOfflineEditingVehicle()) { - return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle); - } else { - if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { - Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); - return yawMode && yawMode->rawValue().toInt() != 0; - } - } - return true; -} - #if 0 // Follow me not ready for Stable void ArduCopterFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 05c596994f8964498e2efd3c2f051cd7b679817e..0294872c65f87d6073cb3ec156b5d923c802e6f2 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -71,7 +71,6 @@ public: QString landFlightMode (void) const override { return QStringLiteral("Land"); } QString takeControlFlightMode (void) const override { return QStringLiteral("Loiter"); } QString followFlightMode (void) const override { return QStringLiteral("Follow"); } - bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const override; QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } bool supportsSmartRTL (void) const override { return true; } #if 0 diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 3a4e07807badb0dd3cd516010cec6c7a3335dc38..1b992dfe6cf0006ba1f690a9f42df2ab27ad05d4 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -698,11 +698,6 @@ QMap* FirmwarePlugin::factGroups(void) { return nullptr; } -bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const -{ - return vehicle->multiRotor() ? false : true; -} - bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle) { if (vehicle->armed()) { diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index a6d0f8c5378de98da20ac57f17fbf2000ea6a8e5..8f2f626f1c9d290f7e869e1bb8a233ccdd74583a 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -297,9 +297,6 @@ public: /// Returns a pointer to a dictionary of firmware-specific FactGroups virtual QMap* factGroups(void); - /// @true: When flying a mission the vehicle is always facing towards the next waypoint - virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const; - /// Returns the data needed to do battery consumption calculations /// @param[out] mAhBattery Battery milliamp-hours rating (0 for no battery data available) /// @param[out] hoverAmps Current draw in amps during hover diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 1e2f3c71e39a3489f95113f893af7c0366b32985..c415f8afda0f786ea159c9debd367100416cd818 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -1057,7 +1057,7 @@ void CameraSectionTest::_testScanForMultipleItems(void) item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); - qDebug() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item2->command())->rawName();; + qDebug() << commandTree->getUIInfo(_controllerVehicle, QGCMAVLink::VehicleClassGeneric, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_controllerVehicle, QGCMAVLink::VehicleClassGeneric, (MAV_CMD)item2->command())->rawName();; scanIndex = 0; QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -1078,7 +1078,7 @@ void CameraSectionTest::_testScanForMultipleItems(void) item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); - qDebug() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item2->command())->rawName();; + qDebug() << commandTree->getUIInfo(_controllerVehicle, QGCMAVLink::VehicleClassGeneric, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_controllerVehicle, QGCMAVLink::VehicleClassGeneric, (MAV_CMD)item2->command())->rawName();; scanIndex = 0; QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); diff --git a/src/MissionManager/KMLPlanDomDocument.cc b/src/MissionManager/KMLPlanDomDocument.cc index cc75ed2072a0c7e81e5940d08f59a3c042f40af1..28f1536fe1c99f5ad1808ad62d36d6ba02ac1ab3 100644 --- a/src/MissionManager/KMLPlanDomDocument.cc +++ b/src/MissionManager/KMLPlanDomDocument.cc @@ -51,7 +51,7 @@ void KMLPlanDomDocument::_addFlightPath(Vehicle* vehicle, QList rg QList rgFlightCoords; QGeoCoordinate homeCoord = rgMissionItems[0]->coordinate(); for (const MissionItem* item : rgMissionItems) { - const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(vehicle, item->command()); + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(vehicle, QGCMAVLink::VehicleClassGeneric, item->command()); if (uiInfo) { double altAdjustment = item->frame() == MAV_FRAME_GLOBAL ? 0 : homeCoord.altitude(); // Used to convert to amsl if (uiInfo->isTakeoffCommand() && !vehicle->fixedWing()) { diff --git a/src/MissionManager/MavCmdInfoFixedWing.json b/src/MissionManager/MavCmdInfoFixedWing.json index a1535265f96563d7cdd47671a9199d59630a0966..c83b7d149754fd5f0e939f6c4186891ec2937958 100644 --- a/src/MissionManager/MavCmdInfoFixedWing.json +++ b/src/MissionManager/MavCmdInfoFixedWing.json @@ -7,7 +7,7 @@ { "id": 16, "comment": "MAV_CMD_NAV_WAYPOINT", - "paramRemove": "4" + "paramRemove": "1,4" }, { "id": 17, diff --git a/src/MissionManager/MavCmdInfoMultiRotor.json b/src/MissionManager/MavCmdInfoMultiRotor.json index b2f3607868f20555145dfdf151c93ebec18b75bb..abd97e5a79c325bfa5b452b04848f2c93ce4424f 100644 --- a/src/MissionManager/MavCmdInfoMultiRotor.json +++ b/src/MissionManager/MavCmdInfoMultiRotor.json @@ -9,6 +9,11 @@ "comment": "MAV_CMD_NAV_LOITER_UNLIM", "paramRemove": "3" }, + { + "id": 18, + "comment": "MAV_CMD_NAV_LOITER_TURNS", + "paramRemove": "1,2,3,4" + }, { "id": 19, "comment": "MAV_CMD_NAV_LOITER_TIME", diff --git a/src/MissionManager/MissionCommandTree.cc b/src/MissionManager/MissionCommandTree.cc index 2b874ed0e555b2bcf34adeb649eceb798437a8fa..ce1c06f4802acd79a12cec2cd736c3be7da32bdb 100644 --- a/src/MissionManager/MissionCommandTree.cc +++ b/src/MissionManager/MissionCommandTree.cc @@ -81,12 +81,12 @@ void MissionCommandTree::_collapseHierarchy(const MissionCommandList* } } -void MissionCommandTree::_buildAllCommands(Vehicle* vehicle) +void MissionCommandTree::_buildAllCommands(Vehicle* vehicle, QGCMAVLink::VehicleClass_t vtolMode) { QGCMAVLink::FirmwareClass_t firmwareClass; QGCMAVLink::VehicleClass_t vehicleClass; - _firmwareAndVehicleClassInfo(vehicle, firmwareClass, vehicleClass); + _firmwareAndVehicleClassInfo(vehicle, vtolMode, firmwareClass, vehicleClass); if (_allCommands.contains(firmwareClass) && _allCommands[firmwareClass].contains(vehicleClass)) { // Already built @@ -131,8 +131,8 @@ QStringList MissionCommandTree::_availableCategoriesForVehicle(Vehicle* vehicle) QGCMAVLink::FirmwareClass_t firmwareClass; QGCMAVLink::VehicleClass_t vehicleClass; - _firmwareAndVehicleClassInfo(vehicle, firmwareClass, vehicleClass); - _buildAllCommands(vehicle); + _firmwareAndVehicleClassInfo(vehicle, QGCMAVLink::VehicleClassGeneric, firmwareClass, vehicleClass); + _buildAllCommands(vehicle, QGCMAVLink::VehicleClassGeneric); return _supportedCategories[firmwareClass][vehicleClass]; } @@ -166,13 +166,13 @@ const QList& MissionCommandTree::allCommandIds(void) const return _staticCommandTree[QGCMAVLink::FirmwareClassGeneric][QGCMAVLink::VehicleClassGeneric]->commandIds(); } -const MissionCommandUIInfo* MissionCommandTree::getUIInfo(Vehicle* vehicle, MAV_CMD command) +const MissionCommandUIInfo* MissionCommandTree::getUIInfo(Vehicle* vehicle, QGCMAVLink::VehicleClass_t vtolMode, MAV_CMD command) { QGCMAVLink::FirmwareClass_t firmwareClass; QGCMAVLink::VehicleClass_t vehicleClass; - _firmwareAndVehicleClassInfo(vehicle, firmwareClass, vehicleClass); - _buildAllCommands(vehicle); + _firmwareAndVehicleClassInfo(vehicle, vtolMode, firmwareClass, vehicleClass); + _buildAllCommands(vehicle, vtolMode); const QMap& infoMap = _allCommands[firmwareClass][vehicleClass]; if (infoMap.contains(command)) { @@ -187,8 +187,8 @@ QVariantList MissionCommandTree::getCommandsForCategory(Vehicle* vehicle, const QGCMAVLink::FirmwareClass_t firmwareClass; QGCMAVLink::VehicleClass_t vehicleClass; - _firmwareAndVehicleClassInfo(vehicle, firmwareClass, vehicleClass); - _buildAllCommands(vehicle); + _firmwareAndVehicleClassInfo(vehicle, QGCMAVLink::VehicleClassGeneric, firmwareClass, vehicleClass); + _buildAllCommands(vehicle, QGCMAVLink::VehicleClassGeneric); // vehicle can be null in which case _firmwareAndVehicleClassInfo will tell of the firmware/vehicle type for the offline editing vehicle. // We then use that to get a firmware plugin so we can get the list of supported commands. @@ -210,8 +210,11 @@ QVariantList MissionCommandTree::getCommandsForCategory(Vehicle* vehicle, const return list; } -void MissionCommandTree::_firmwareAndVehicleClassInfo(Vehicle* vehicle, QGCMAVLink::FirmwareClass_t& firmwareClass, QGCMAVLink::VehicleClass_t& vehicleClass) const +void MissionCommandTree::_firmwareAndVehicleClassInfo(Vehicle* vehicle, QGCMAVLink::VehicleClass_t vtolMode, QGCMAVLink::FirmwareClass_t& firmwareClass, QGCMAVLink::VehicleClass_t& vehicleClass) const { firmwareClass = QGCMAVLink::firmwareClass(vehicle->firmwareType()); vehicleClass = QGCMAVLink::vehicleClass(vehicle->vehicleType()); + if (vehicleClass == QGCMAVLink::VehicleClassVTOL && vtolMode != QGCMAVLink::VehicleClassGeneric) { + vehicleClass = vtolMode; + } } diff --git a/src/MissionManager/MissionCommandTree.h b/src/MissionManager/MissionCommandTree.h index 2df8241f6a85ccb010dfbebbdb9462a1f4e70d8d..80e4945d97fb0bcf5ae032ef7f55acd5ad44847e 100644 --- a/src/MissionManager/MissionCommandTree.h +++ b/src/MissionManager/MissionCommandTree.h @@ -59,7 +59,7 @@ public: Q_INVOKABLE QStringList categoriesForVehicle(Vehicle* vehicle) { return _availableCategoriesForVehicle(vehicle); } - const MissionCommandUIInfo* getUIInfo(Vehicle* vehicle, MAV_CMD command); + const MissionCommandUIInfo* getUIInfo(Vehicle* vehicle, QGCMAVLink::VehicleClass_t vtolMode, MAV_CMD command); /// @param showFlyThroughCommands - true: all commands shows, false: filter out commands which the vehicle flies through (specifiedCoordinate=true, standaloneCoordinate=false) Q_INVOKABLE QVariantList getCommandsForCategory(Vehicle* vehicle, const QString& category, bool showFlyThroughCommands); @@ -69,9 +69,9 @@ public: private: void _collapseHierarchy (const MissionCommandList* cmdList, QMap& collapsedTree); - void _buildAllCommands (Vehicle* vehicle); + void _buildAllCommands (Vehicle* vehicle, QGCMAVLink::VehicleClass_t vtolMode); QStringList _availableCategoriesForVehicle (Vehicle* vehicle); - void _firmwareAndVehicleClassInfo (Vehicle* vehicle, QGCMAVLink::FirmwareClass_t& firmwareClass, QGCMAVLink::VehicleClass_t& vehicleClass) const; + void _firmwareAndVehicleClassInfo (Vehicle* vehicle, QGCMAVLink::VehicleClass_t vtolMode, QGCMAVLink::FirmwareClass_t& firmwareClass, QGCMAVLink::VehicleClass_t& vehicleClass) const; private: QString _allCommandsCategory; ///< Category which contains all available commands diff --git a/src/MissionManager/MissionCommandTreeTest.cc b/src/MissionManager/MissionCommandTreeTest.cc index f0b086a8f37895b6d0128dbf4845bcce0b14d687..80bb37878b1b87a5144b708027cc50188b359363 100644 --- a/src/MissionManager/MissionCommandTreeTest.cc +++ b/src/MissionManager/MissionCommandTreeTest.cc @@ -188,12 +188,12 @@ void MissionCommandTreeTest::testOverride(void) { // Generic/Generic should not have any overrides Vehicle* vehicle = new Vehicle(MAV_AUTOPILOT_GENERIC, MAV_TYPE_GENERIC, qgcApp()->toolbox()->firmwarePluginManager()); - _checkBaseValues(_commandTree->getUIInfo(vehicle, (MAV_CMD)4), 4); + _checkBaseValues(_commandTree->getUIInfo(vehicle, QGCMAVLink::VehicleClassGeneric, (MAV_CMD)4), 4); delete vehicle; // Generic/FixedWing should have overrides vehicle = new Vehicle(MAV_AUTOPILOT_GENERIC, MAV_TYPE_FIXED_WING, qgcApp()->toolbox()->firmwarePluginManager()); - _checkOverrideValues(_commandTree->getUIInfo(vehicle, (MAV_CMD)4), 4); + _checkOverrideValues(_commandTree->getUIInfo(vehicle, QGCMAVLink::VehicleClassGeneric, (MAV_CMD)4), 4); delete vehicle; } @@ -214,7 +214,7 @@ void MissionCommandTreeTest::testAllTrees(void) } qDebug() << firmwareType << vehicleType; Vehicle* vehicle = new Vehicle(firmwareType, vehicleType, qgcApp()->toolbox()->firmwarePluginManager()); - QVERIFY(qgcApp()->toolbox()->missionCommandTree()->getUIInfo(vehicle, MAV_CMD_NAV_WAYPOINT) != nullptr); + QVERIFY(qgcApp()->toolbox()->missionCommandTree()->getUIInfo(vehicle, QGCMAVLink::VehicleClassMultiRotor, MAV_CMD_NAV_WAYPOINT) != nullptr); delete vehicle; } } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index ea81cd0f7fa0012dbdf3b61b96beb3cb1a034024..abf2ef0c19eaea1302ed7a90bd7657d5098922bd 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -318,7 +318,7 @@ VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordin _initVisualItem(newItem); if (newItem->specifiesAltitude()) { - const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, command); + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, QGCMAVLink::VehicleClassGeneric, command); if (!uiInfo->isLandCommand()) { double prevAltitude; int prevAltitudeMode; @@ -1672,9 +1672,9 @@ void MissionController::_recalcMissionFlightStatus() case MAV_CMD_DO_VTOL_TRANSITION: { int transitionState = simpleItem->missionItem().param1(); - if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) { + if (transitionState == MAV_VTOL_STATE_MC) { _missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassMultiRotor; - } else if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_FW) { + } else if (transitionState == MAV_VTOL_STATE_FW) { _missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassFixedWing; } } @@ -1901,7 +1901,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) connect(visualItem, &VisualMissionItem::specifiedVehicleYawChanged, this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection); connect(visualItem, &VisualMissionItem::terrainAltitudeChanged, this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection); connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged, this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection); - + connect(visualItem, &VisualMissionItem::currentVTOLModeChanged, this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection); connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence); if (visualItem->isSimpleItem()) { diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 348777ae7f0f57acdc2428f8d25fcd7a41402f1d..7bf8220dc026dbd593e3733ced463f81b73d46c2 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -97,11 +97,11 @@ void MissionManager::generateResumeMission(int resumeIndex) 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()); + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, QGCMAVLink::VehicleClassGeneric, _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()); + uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, QGCMAVLink::VehicleClassGeneric, _missionItems[resumeIndex]->command()); if (uiInfo && (uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate())) { // Found it break; diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 3034dfda9fbdb860a669ee5cb4baf61a993c8f68..2d491fa09a226df074df273a26f899a4408e2dea 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -191,13 +191,18 @@ void SimpleMissionItem::_connectSignals(void) // Whenever these properties change the ui model changes as well connect(this, &SimpleMissionItem::commandChanged, this, &SimpleMissionItem::_rebuildFacts); connect(this, &SimpleMissionItem::rawEditChanged, this, &SimpleMissionItem::_rebuildFacts); + connect(this, &SimpleMissionItem::previousVTOLModeChanged,this, &SimpleMissionItem::_rebuildFacts); + + // The following changes must signal currentVTOLModeChanged to cause a MissionController recalc + connect(this, &SimpleMissionItem::commandChanged, this, &SimpleMissionItem::_signalIfVTOLTransitionCommand); + connect(&_missionItem._param1Fact, &Fact::valueChanged, this, &SimpleMissionItem::_signalIfVTOLTransitionCommand); // These fact signals must alway signal out through SimpleMissionItem signals connect(&_missionItem._commandFact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCommandChanged); // Propogate signals from MissionItem up to SimpleMissionItem - connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged); - connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); + connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged); + connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); } void SimpleMissionItem::_setupMetaData(void) @@ -333,7 +338,7 @@ bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QStrin bool SimpleMissionItem::isStandaloneCoordinate(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, (MAV_CMD)command()); if (uiInfo) { return uiInfo->isStandaloneCoordinate(); } else { @@ -343,7 +348,7 @@ bool SimpleMissionItem::isStandaloneCoordinate(void) const bool SimpleMissionItem::specifiesCoordinate(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, (MAV_CMD)command()); if (uiInfo) { return uiInfo->specifiesCoordinate(); } else { @@ -353,7 +358,7 @@ bool SimpleMissionItem::specifiesCoordinate(void) const bool SimpleMissionItem::specifiesAltitudeOnly(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, (MAV_CMD)command()); if (uiInfo) { return uiInfo->specifiesAltitudeOnly(); } else { @@ -363,7 +368,7 @@ bool SimpleMissionItem::specifiesAltitudeOnly(void) const QString SimpleMissionItem::commandDescription(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, (MAV_CMD)command()); if (uiInfo) { return uiInfo->description(); } else { @@ -374,7 +379,7 @@ QString SimpleMissionItem::commandDescription(void) const QString SimpleMissionItem::commandName(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, (MAV_CMD)command()); if (uiInfo) { return uiInfo->friendlyName(); } else { @@ -444,7 +449,7 @@ void SimpleMissionItem::_rebuildTextFieldFacts(void) Fact* rgParamFacts[7] = { &_missionItem._param1Fact, &_missionItem._param2Fact, &_missionItem._param3Fact, &_missionItem._param4Fact, &_missionItem._param5Fact, &_missionItem._param6Fact, &_missionItem._param7Fact }; FactMetaData* rgParamMetaData[7] = { &_param1MetaData, &_param2MetaData, &_param3MetaData, &_param4MetaData, &_param5MetaData, &_param6MetaData, &_param7MetaData }; - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, command); for (int i=1; i<=7; i++) { bool showUI; @@ -483,7 +488,7 @@ void SimpleMissionItem::_rebuildNaNFacts(void) Fact* rgParamFacts[7] = { &_missionItem._param1Fact, &_missionItem._param2Fact, &_missionItem._param3Fact, &_missionItem._param4Fact, &_missionItem._param5Fact, &_missionItem._param6Fact, &_missionItem._param7Fact }; FactMetaData* rgParamMetaData[7] = { &_param1MetaData, &_param2MetaData, &_param3MetaData, &_param4MetaData, &_param5MetaData, &_param6MetaData, &_param7MetaData }; - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, command); for (int i=1; i<=7; i++) { bool showUI; @@ -496,10 +501,6 @@ void SimpleMissionItem::_rebuildNaNFacts(void) if (!firmwareVehicle) { firmwareVehicle = _controllerVehicle; } - bool hideWaypointHeading = (command == MAV_CMD_NAV_WAYPOINT || command == MAV_CMD_NAV_TAKEOFF) && (i == 4) && firmwareVehicle->firmwarePlugin()->vehicleYawsToNextWaypointInMission(firmwareVehicle); - if (hideWaypointHeading) { - continue; - } Fact* paramFact = rgParamFacts[i-1]; FactMetaData* paramMetaData = rgParamMetaData[i-1]; @@ -541,7 +542,7 @@ void SimpleMissionItem::_rebuildComboBoxFacts(void) for (int i=1; i<=7; i++) { bool showUI; - const MissionCmdParamInfo* paramInfo = _commandTree->getUIInfo(_controllerVehicle, command)->getParamInfo(i, showUI); + const MissionCmdParamInfo* paramInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, command)->getParamInfo(i, showUI); if (showUI && paramInfo && paramInfo->enumStrings().count() != 0) { Fact* paramFact = rgParamFacts[i-1]; @@ -567,7 +568,7 @@ void SimpleMissionItem::_rebuildFacts(void) bool SimpleMissionItem::friendlyEditAllowed(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, static_cast(command())); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, static_cast(command())); if (uiInfo && uiInfo->friendlyEdit()) { if (!_missionItem.autoContinue()) { return false; @@ -740,7 +741,7 @@ void SimpleMissionItem::_setDefaultsForCommand(void) } MAV_CMD command = static_cast(this->command()); - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, command); if (uiInfo) { for (int i=1; i<=7; i++) { bool showUI; @@ -777,7 +778,7 @@ void SimpleMissionItem::_sendFriendlyEditAllowedChanged(void) QString SimpleMissionItem::category(void) const { - return _commandTree->getUIInfo(_controllerVehicle, static_cast(command()))->category(); + return _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, static_cast(command()))->category(); } void SimpleMissionItem::setCommand(int command) @@ -920,7 +921,7 @@ void SimpleMissionItem::appendMissionItems(QList& items, QObject* void SimpleMissionItem::applyNewAltitude(double newAltitude) { MAV_CMD command = static_cast(this->command()); - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, command); if (uiInfo->specifiesCoordinate() || uiInfo->specifiesAltitudeOnly()) { switch (static_cast(this->command())) { @@ -937,8 +938,9 @@ void SimpleMissionItem::applyNewAltitude(double newAltitude) void SimpleMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) { - // If speed and/or gimbal are not specifically set on this item. Then use the flight status values as initial defaults should a user turn them on. VisualMissionItem::setMissionFlightStatus(missionFlightStatus); + + // If speed and/or gimbal are not specifically set on this item. Then use the flight status values as initial defaults should a user turn them on. if (_speedSection->available() && !_speedSection->specifyFlightSpeed() && !qFuzzyCompare(_speedSection->flightSpeed()->rawValue().toDouble(), missionFlightStatus.vehicleSpeed)) { _speedSection->flightSpeed()->setRawValue(missionFlightStatus.vehicleSpeed); } @@ -988,7 +990,7 @@ void SimpleMissionItem::_possibleAdditionalTimeDelayChanged(void) bool SimpleMissionItem::isLandCommand(void) const { MAV_CMD command = static_cast(this->command()); - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, _previousVTOLMode, command); return uiInfo->isLandCommand(); } @@ -1019,3 +1021,11 @@ double SimpleMissionItem::amslEntryAlt(void) const qWarning() << "Internal Error SimpleMissionItem::amslEntryAlt: Invalid altitudeMode:" << _altitudeMode; return qQNaN(); } + +void SimpleMissionItem::_signalIfVTOLTransitionCommand(void) +{ + if (mavCommand() == MAV_CMD_DO_VTOL_TRANSITION) { + // This will cause a MissionController recalc + emit currentVTOLModeChanged(); + } +} diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index 0d7a1995eb15f6d83ee33892aaeb1a846859a3bd..11a66cf4f5eaebef6af73b1975dc51434a9e7b21 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -139,20 +139,21 @@ signals: void altitudeModeChanged (void); private slots: - void _setDirty (void); - void _sectionDirtyChanged (bool dirty); - void _sendCommandChanged (void); - void _sendCoordinateChanged (void); - void _sendFriendlyEditAllowedChanged (void); - void _altitudeChanged (void); - void _altitudeModeChanged (void); - void _terrainAltChanged (void); - void _updateLastSequenceNumber (void); - void _rebuildFacts (void); - void _rebuildTextFieldFacts (void); - void _possibleAdditionalTimeDelayChanged(void); - void _setDefaultsForCommand (void); - void _possibleVehicleYawChanged (void); + void _setDirty (void); + void _sectionDirtyChanged (bool dirty); + void _sendCommandChanged (void); + void _sendCoordinateChanged (void); + void _sendFriendlyEditAllowedChanged (void); + void _altitudeChanged (void); + void _altitudeModeChanged (void); + void _terrainAltChanged (void); + void _updateLastSequenceNumber (void); + void _rebuildFacts (void); + void _rebuildTextFieldFacts (void); + void _possibleAdditionalTimeDelayChanged (void); + void _setDefaultsForCommand (void); + void _possibleVehicleYawChanged (void); + void _signalIfVTOLTransitionCommand (void); private: void _connectSignals (void); diff --git a/src/MissionManager/SimpleMissionItemTest.cc b/src/MissionManager/SimpleMissionItemTest.cc index 056d4b3eacafad4974bb6cd527bb27a2ec095e82..5281b09f21a5ff53724acc062cf55264c616b516 100644 --- a/src/MissionManager/SimpleMissionItemTest.cc +++ b/src/MissionManager/SimpleMissionItemTest.cc @@ -7,14 +7,13 @@ * ****************************************************************************/ - #include "SimpleMissionItemTest.h" #include "QGCApplication.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" #include "PlanMasterController.h" -const SimpleMissionItemTest::ItemInfo_t SimpleMissionItemTest::_rgItemInfo[] = { +static const ItemInfo_t _rgItemInfo[] = { { MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT }, { MAV_CMD_NAV_LOITER_UNLIM, MAV_FRAME_GLOBAL }, { MAV_CMD_NAV_LOITER_TURNS, MAV_FRAME_GLOBAL_RELATIVE_ALT }, @@ -24,42 +23,48 @@ const SimpleMissionItemTest::ItemInfo_t SimpleMissionItemTest::_rgItemInfo[] = { { MAV_CMD_DO_JUMP, MAV_FRAME_MISSION }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesWaypoint[] = { - { "Hold", 10.1234567 }, +static const FactValue_t _rgFactValuesWaypoint[] = { + { "Hold", QGCMAVLink::VehicleClassMultiRotor, false, 1 }, + { "Yaw", QGCMAVLink::VehicleClassMultiRotor, true, 4 }, +}; + +static const FactValue_t _rgFactValuesLoiterUnlim[] = { + { "Radius", QGCMAVLink::VehicleClassFixedWing, false, 3 }, + { "Yaw", QGCMAVLink::VehicleClassMultiRotor, true, 4 }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLoiterUnlim[] = { - { "Radius", 30.1234567 }, +static const FactValue_t _rgFactValuesLoiterTurns[] = { + { "Turns", QGCMAVLink::VehicleClassFixedWing, false, 1 }, + { "Radius", QGCMAVLink::VehicleClassFixedWing, false, 3 }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLoiterTurns[] = { - { "Radius", 30.1234567 }, - { "Turns", 10.1234567 }, +static const FactValue_t _rgFactValuesLoiterTime[] = { + { "Loiter Time", QGCMAVLink::VehicleClassGeneric, false, 1 }, + { "Radius", QGCMAVLink::VehicleClassFixedWing, false, 3 }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLoiterTime[] = { - { "Radius", 30.1234567 }, - { "Loiter Time", 10.1234567 }, +static const FactValue_t _rgFactValuesLand[] = { + { "Yaw", QGCMAVLink::VehicleClassMultiRotor, true, 4 }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesTakeoff[] = { - { "Pitch", 10.1234567 }, +static const FactValue_t _rgFactValuesTakeoff[] = { + { "Pitch", QGCMAVLink::VehicleClassFixedWing, false, 1 }, + { "Yaw", QGCMAVLink::VehicleClassMultiRotor, true, 4 }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesDoJump[] = { - { "Item #", 10.1234567 }, - { "Repeat", 20.1234567 }, +static const FactValue_t _rgFactValuesDoJump[] = { + { "Item #", QGCMAVLink::VehicleClassGeneric, false, 1 }, + { "Repeat", QGCMAVLink::VehicleClassGeneric, false, 2 }, }; -const SimpleMissionItemTest::ItemExpected_t SimpleMissionItemTest::_rgItemExpected[] = { - // Text field facts count Fact Values Altitude Altitude Mode - { sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint)/sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint[0]), SimpleMissionItemTest::_rgFactValuesWaypoint, 70.1234567, QGroundControlQmlGlobal::AltitudeModeRelative }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]), SimpleMissionItemTest::_rgFactValuesLoiterUnlim, 70.1234567, QGroundControlQmlGlobal::AltitudeModeAbsolute }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns[0]), SimpleMissionItemTest::_rgFactValuesLoiterTurns, 70.1234567, QGroundControlQmlGlobal::AltitudeModeRelative }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]), SimpleMissionItemTest::_rgFactValuesLoiterTime, 70.1234567, QGroundControlQmlGlobal::AltitudeModeAbsolute }, - { 0, nullptr, 70.1234567, QGroundControlQmlGlobal::AltitudeModeRelative }, - { sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]), SimpleMissionItemTest::_rgFactValuesTakeoff, 70.1234567, QGroundControlQmlGlobal::AltitudeModeAbsolute }, - { sizeof(SimpleMissionItemTest::_rgFactValuesDoJump)/sizeof(SimpleMissionItemTest::_rgFactValuesDoJump[0]), SimpleMissionItemTest::_rgFactValuesDoJump, qQNaN(), QGroundControlQmlGlobal::AltitudeModeRelative }, +const ItemExpected_t _rgItemExpected[] = { + { sizeof(_rgFactValuesWaypoint)/sizeof(_rgFactValuesWaypoint[0]), _rgFactValuesWaypoint, 70.1234567, QGroundControlQmlGlobal::AltitudeModeRelative }, + { sizeof(_rgFactValuesLoiterUnlim)/sizeof(_rgFactValuesLoiterUnlim[0]), _rgFactValuesLoiterUnlim, 70.1234567, QGroundControlQmlGlobal::AltitudeModeAbsolute }, + { sizeof(_rgFactValuesLoiterTurns)/sizeof(_rgFactValuesLoiterTurns[0]), _rgFactValuesLoiterTurns, 70.1234567, QGroundControlQmlGlobal::AltitudeModeRelative }, + { sizeof(_rgFactValuesLoiterTime)/sizeof(_rgFactValuesLoiterTime[0]), _rgFactValuesLoiterTime, 70.1234567, QGroundControlQmlGlobal::AltitudeModeAbsolute }, + { sizeof(_rgFactValuesLand)/sizeof(_rgFactValuesLand[0]), _rgFactValuesLand, 70.1234567, QGroundControlQmlGlobal::AltitudeModeRelative }, + { sizeof(_rgFactValuesTakeoff)/sizeof(_rgFactValuesTakeoff[0]), _rgFactValuesTakeoff, 70.1234567, QGroundControlQmlGlobal::AltitudeModeAbsolute }, + { sizeof(_rgFactValuesDoJump)/sizeof(_rgFactValuesDoJump[0]), _rgFactValuesDoJump, qQNaN(), QGroundControlQmlGlobal::AltitudeModeRelative }, }; SimpleMissionItemTest::SimpleMissionItemTest(void) @@ -109,16 +114,42 @@ void SimpleMissionItemTest::cleanup(void) VisualMissionItemTest::cleanup(); } -void SimpleMissionItemTest::_testEditorFacts(void) +bool SimpleMissionItemTest::_classMatch(QGCMAVLink::VehicleClass_t vehicleClass, QGCMAVLink::VehicleClass_t testClass) { - PlanMasterController fwMasterController(MAV_AUTOPILOT_PX4, MAV_TYPE_FIXED_WING); + return vehicleClass == QGCMAVLink::VehicleClassGeneric || vehicleClass == testClass; +} + +void SimpleMissionItemTest::_testEditorFactsWorker(QGCMAVLink::VehicleClass_t vehicleClass, QGCMAVLink::VehicleClass_t vtolMode, const ItemExpected_t* rgExpected) +{ + qDebug() << "vehicleClass:vtolMode" << QGCMAVLink::vehicleClassToString(vehicleClass) << QGCMAVLink::vehicleClassToString(vtolMode); + + PlanMasterController planController(MAV_AUTOPILOT_PX4, QGCMAVLink::vehicleClassToMavType(vehicleClass)); + + QGCMAVLink::VehicleClass_t commandVehicleClass = vtolMode == QGCMAVLink::VehicleClassGeneric ? vehicleClass : vtolMode; for (size_t i=0; icommand; - + + // Determine how many fact values we should get back + int cExpectedTextFieldFacts = 0; + int cExpectedNaNFieldFacts = 0; + for (size_t j=0; jcFactValues; j++) { + const FactValue_t* factValue = &expected->rgFactValues[j]; + + if (!_classMatch(factValue->vehicleClass, commandVehicleClass)) { + continue; + } + if (factValue->nanValue) { + cExpectedNaNFieldFacts++; + + } else { + cExpectedTextFieldFacts++; + } + } + MissionItem missionItem(1, // sequence number info->command, info->frame, @@ -131,30 +162,66 @@ void SimpleMissionItemTest::_testEditorFacts(void) 70.1234567, true, // autoContinue false); // isCurrentItem - SimpleMissionItem simpleMissionItem(&fwMasterController, false /* flyView */, missionItem, nullptr); + SimpleMissionItem simpleMissionItem(&planController, false /* flyView */, missionItem, nullptr); + + MissionController::MissionFlightStatus_t missionFlightStatus; + missionFlightStatus.vtolMode = vtolMode; + missionFlightStatus.vehicleSpeed = 10; + missionFlightStatus.gimbalYaw = qQNaN(); + missionFlightStatus.gimbalPitch = qQNaN(); + simpleMissionItem.setMissionFlightStatus(missionFlightStatus); // Validate that the fact values are correctly returned - size_t factCount = 0; + int foundTextFieldCount = 0; for (int i=0; icount(); i++) { Fact* fact = qobject_cast(simpleMissionItem.textFieldFacts()->get(i)); - + bool found = false; for (size_t j=0; jcFactValues; j++) { const FactValue_t* factValue = &expected->rgFactValues[j]; - + + if (!_classMatch(factValue->vehicleClass, commandVehicleClass)) { + continue; + } + if (factValue->name == fact->name()) { - QCOMPARE(fact->rawValue().toDouble(), factValue->value); - factCount ++; + QCOMPARE(fact->rawValue().toDouble(), (factValue->paramIndex * 10.0) + 0.1234567); + foundTextFieldCount ++; found = true; break; } } - + qDebug() << "textFieldFact" << fact->name(); QVERIFY(found); } - QCOMPARE(factCount, expected->cFactValues); + QCOMPARE(foundTextFieldCount, cExpectedTextFieldFacts); + + int foundNaNFieldCount = 0; + for (int i=0; icount(); i++) { + Fact* fact = qobject_cast(simpleMissionItem.nanFacts()->get(i)); + + bool found = false; + for (size_t j=0; jcFactValues; j++) { + const FactValue_t* factValue = &expected->rgFactValues[j]; + + if (!_classMatch(factValue->vehicleClass, commandVehicleClass)) { + continue; + } + + if (factValue->name == fact->name()) { + QCOMPARE(fact->rawValue().toDouble(), (factValue->paramIndex * 10.0) + 0.1234567); + foundNaNFieldCount ++; + found = true; + break; + } + } + + qDebug() << "nanFieldFact" << fact->name(); + QVERIFY(found); + } + QCOMPARE(foundNaNFieldCount, cExpectedNaNFieldFacts); if (!qIsNaN(expected->altValue)) { QCOMPARE(simpleMissionItem.altitudeMode(), expected->altMode); @@ -163,6 +230,14 @@ void SimpleMissionItemTest::_testEditorFacts(void) } } +void SimpleMissionItemTest::_testEditorFacts(void) +{ + _testEditorFactsWorker(QGCMAVLink::VehicleClassMultiRotor, QGCMAVLink::VehicleClassGeneric, _rgItemExpected); + _testEditorFactsWorker(QGCMAVLink::VehicleClassFixedWing, QGCMAVLink::VehicleClassGeneric, _rgItemExpected); + _testEditorFactsWorker(QGCMAVLink::VehicleClassVTOL, QGCMAVLink::VehicleClassMultiRotor, _rgItemExpected); + _testEditorFactsWorker(QGCMAVLink::VehicleClassVTOL, QGCMAVLink::VehicleClassFixedWing, _rgItemExpected); +} + void SimpleMissionItemTest::_testDefaultValues(void) { SimpleMissionItem item(_masterController, false /* flyView */, false /* forLoad */, nullptr); diff --git a/src/MissionManager/SimpleMissionItemTest.h b/src/MissionManager/SimpleMissionItemTest.h index b938c2b52a01f622bb02a93bc83334612c499430..2bab393e1e3c03d78bf0c451d4fc7a38c3adec46 100644 --- a/src/MissionManager/SimpleMissionItemTest.h +++ b/src/MissionManager/SimpleMissionItemTest.h @@ -13,6 +13,26 @@ #include "SimpleMissionItem.h" /// Unit test for SimpleMissionItem + +typedef struct { + MAV_CMD command; + MAV_FRAME frame; +} ItemInfo_t; + +typedef struct { + const char* name; + QGCMAVLink::VehicleClass_t vehicleClass; + bool nanValue; + int paramIndex; +} FactValue_t; + +typedef struct { + size_t cFactValues; + const FactValue_t* rgFactValues; + double altValue; + QGroundControlQmlGlobal::AltitudeMode altMode; +} ItemExpected_t; + class SimpleMissionItemTest : public VisualMissionItemTest { Q_OBJECT @@ -20,18 +40,18 @@ class SimpleMissionItemTest : public VisualMissionItemTest public: SimpleMissionItemTest(void); - void init(void) override; + void init (void) override; void cleanup(void) override; private slots: - void _testSignals(void); - void _testEditorFacts(void); - void _testDefaultValues(void); - void _testCameraSectionDirty(void); - void _testSpeedSectionDirty(void); - void _testCameraSection(void); - void _testSpeedSection(void); - void _testAltitudePropogation(void); + void _testSignals (void); + void _testEditorFacts (void); + void _testDefaultValues (void); + void _testCameraSectionDirty (void); + void _testSpeedSectionDirty (void); + void _testCameraSection (void); + void _testSpeedSection (void); + void _testAltitudePropogation (void); private: enum { @@ -58,35 +78,10 @@ private: static const size_t cSimpleItemSignals = maxSignalIndex; const char* rgSimpleItemSignals[cSimpleItemSignals]; - typedef struct { - MAV_CMD command; - MAV_FRAME frame; - } ItemInfo_t; - - typedef struct { - const char* name; - double value; - } FactValue_t; - - typedef struct { - size_t cFactValues; - const FactValue_t* rgFactValues; - double altValue; - QGroundControlQmlGlobal::AltitudeMode altMode; - } ItemExpected_t; + void _testEditorFactsWorker (QGCMAVLink::VehicleClass_t vehicleClass, QGCMAVLink::VehicleClass_t vtolMode, const ItemExpected_t* rgExpected); + bool _classMatch (QGCMAVLink::VehicleClass_t vehicleClass, QGCMAVLink::VehicleClass_t testClass); SimpleMissionItem* _simpleItem; MultiSignalSpy* _spySimpleItem; MultiSignalSpy* _spyVisualItem; - - static const ItemInfo_t _rgItemInfo[]; - static const ItemExpected_t _rgItemExpected[]; - static const FactValue_t _rgFactValuesWaypoint[]; - static const FactValue_t _rgFactValuesLoiterUnlim[]; - static const FactValue_t _rgFactValuesLoiterTurns[]; - static const FactValue_t _rgFactValuesLoiterTime[]; - static const FactValue_t _rgFactValuesLand[]; - static const FactValue_t _rgFactValuesTakeoff[]; - static const FactValue_t _rgFactValuesConditionDelay[]; - static const FactValue_t _rgFactValuesDoJump[]; }; diff --git a/src/MissionManager/TakeoffMissionItem.cc b/src/MissionManager/TakeoffMissionItem.cc index 971e356104ffacab0413f703bec1c727d9249fbe..aa8b9cd1a6430a4dab7de7da6a8720c5840ab244 100644 --- a/src/MissionManager/TakeoffMissionItem.cc +++ b/src/MissionManager/TakeoffMissionItem.cc @@ -116,7 +116,7 @@ void TakeoffMissionItem::setCoordinate(const QGeoCoordinate& coordinate) bool TakeoffMissionItem::isTakeoffCommand(MAV_CMD command) { - const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(qgcApp()->toolbox()->multiVehicleManager()->offlineEditingVehicle(), command); + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(qgcApp()->toolbox()->multiVehicleManager()->offlineEditingVehicle(), QGCMAVLink::VehicleClassGeneric, command); return uiInfo ? uiInfo->isTakeoffCommand() : false; } diff --git a/src/MissionManager/VisualMissionItem.cc b/src/MissionManager/VisualMissionItem.cc index 1913e449316dec051fb8d77acf5364dcd11f8809..ce9705428e05e34704e235de05b291f428395076 100644 --- a/src/MissionManager/VisualMissionItem.cc +++ b/src/MissionManager/VisualMissionItem.cc @@ -7,7 +7,6 @@ * ****************************************************************************/ - #include #include @@ -18,6 +17,7 @@ #include "TerrainQuery.h" #include "TakeoffMissionItem.h" #include "PlanMasterController.h" +#include "QGC.h" const char* VisualMissionItem::jsonTypeKey = "type"; const char* VisualMissionItem::jsonTypeSimpleItemValue = "SimpleItem"; @@ -152,13 +152,14 @@ void VisualMissionItem::setAzimuth(double azimuth) void VisualMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) { - if (qIsNaN(missionFlightStatus.gimbalYaw) && qIsNaN(_missionGimbalYaw)) { - return; - } - if (!qFuzzyCompare(missionFlightStatus.gimbalYaw, _missionGimbalYaw)) { + if (!QGC::fuzzyCompare(missionFlightStatus.gimbalYaw, _missionGimbalYaw)) { _missionGimbalYaw = missionFlightStatus.gimbalYaw; emit missionGimbalYawChanged(_missionGimbalYaw); } + if (missionFlightStatus.vtolMode != _previousVTOLMode) { + _previousVTOLMode = missionFlightStatus.vtolMode; + emit previousVTOLModeChanged(); + } } void VisualMissionItem::setMissionVehicleYaw(double vehicleYaw) diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index 93f32d17113aac9510e4d394ac8cbb8f04db827d..35b599f9e2744c9a6e39a62e300110c9b6ea6db2 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -81,6 +81,7 @@ public: Q_PROPERTY(double missionVehicleYaw READ missionVehicleYaw NOTIFY missionVehicleYawChanged) ///< Expected vehicle yaw at this point in mission Q_PROPERTY(bool flyView READ flyView CONSTANT) Q_PROPERTY(bool wizardMode READ wizardMode WRITE setWizardMode NOTIFY wizardModeChanged) + Q_PROPERTY(int previousVTOLMode MEMBER _previousVTOLMode NOTIFY previousVTOLModeChanged) ///< Current VTOL mode (VehicleClass_t) prior to executing this item Q_PROPERTY(PlanMasterController* masterController READ masterController CONSTANT) Q_PROPERTY(ReadyForSaveState readyForSaveState READ readyForSaveState NOTIFY readyForSaveStateChanged) @@ -238,6 +239,8 @@ signals: void parentItemChanged (VisualMissionItem* parentItem); void amslEntryAltChanged (double alt); void amslExitAltChanged (double alt); + void previousVTOLModeChanged (void); + void currentVTOLModeChanged (void); ///< Signals that this item has changed the VTOL mode (MAV_CMD_DO_VTOL_TRANSITION) void exitCoordinateSameAsEntryChanged (bool exitCoordinateSameAsEntry); @@ -246,29 +249,30 @@ protected slots: void _amslExitAltChanged(void); // signals new amslExitAlt value protected: - bool _flyView = false; - bool _isCurrentItem = false; - bool _hasCurrentChildItem = false; - bool _dirty = false; - bool _homePositionSpecialCase = false; ///< true: This item is being used as a ui home position indicator - bool _wizardMode = false; ///< true: Item editor is showing wizard completion panel - double _terrainAltitude = qQNaN(); ///< Altitude of terrain at coordinate position, NaN for not known - double _altDifference = 0; ///< Difference in altitude from previous waypoint - double _altPercent = 0; ///< Percent of total altitude change in mission - double _terrainPercent = qQNaN(); ///< Percent of terrain altitude for coordinate - bool _terrainCollision = false; ///< true: item collides with terrain - double _azimuth = 0; ///< Azimuth to previous waypoint - double _distance = 0; ///< Distance to previous waypoint - double _distanceFromStart = 0; ///< Flight path cumalative horizontal distance from home point to this item - QString _editorQml; ///< Qml resource for editing item - double _missionGimbalYaw = qQNaN(); - double _missionVehicleYaw = qQNaN(); - - PlanMasterController* _masterController = nullptr; - MissionController* _missionController = nullptr; - Vehicle* _controllerVehicle = nullptr; - FlightPathSegment * _simpleFlightPathSegment = nullptr; ///< The simple item flight segment (if any) which starts with this visual item. - VisualMissionItem* _parentItem = nullptr; + bool _flyView = false; + bool _isCurrentItem = false; + bool _hasCurrentChildItem = false; + bool _dirty = false; + bool _homePositionSpecialCase = false; ///< true: This item is being used as a ui home position indicator + bool _wizardMode = false; ///< true: Item editor is showing wizard completion panel + double _terrainAltitude = qQNaN(); ///< Altitude of terrain at coordinate position, NaN for not known + double _altDifference = 0; ///< Difference in altitude from previous waypoint + double _altPercent = 0; ///< Percent of total altitude change in mission + double _terrainPercent = qQNaN(); ///< Percent of terrain altitude for coordinate + bool _terrainCollision = false; ///< true: item collides with terrain + double _azimuth = 0; ///< Azimuth to previous waypoint + double _distance = 0; ///< Distance to previous waypoint + double _distanceFromStart = 0; ///< Flight path cumalative horizontal distance from home point to this item + QString _editorQml; ///< Qml resource for editing item + double _missionGimbalYaw = qQNaN(); + double _missionVehicleYaw = qQNaN(); + QGCMAVLink::VehicleClass_t _previousVTOLMode = QGCMAVLink::VehicleClassGeneric; ///< Generic == unknown + + PlanMasterController* _masterController = nullptr; + MissionController* _missionController = nullptr; + Vehicle* _controllerVehicle = nullptr; + FlightPathSegment * _simpleFlightPathSegment = nullptr; ///< The simple item flight segment (if any) which starts with this visual item. + VisualMissionItem* _parentItem = nullptr; QGCGeoBoundingCube _boundingCube; ///< The bounding "cube" of this element. /// This is used to reference any subsequent mission items which do not specify a coordinate. diff --git a/src/QGC.cc b/src/QGC.cc index 4d68bafdc9464356b0e46e80c4736fb04b4646e7..77e5a7baa0c12f68caaf9832d23c6eab328b434e 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -9,9 +9,12 @@ #include "QGC.h" + #include #include +#include + namespace QGC { @@ -137,4 +140,17 @@ quint32 crc32(const quint8 *src, unsigned len, unsigned state) return state; } +bool fuzzyCompare(double value1, double value2) +{ + if (qIsNaN(value1) && qIsNaN(value2)) { + return true; + } else if (qIsNaN(value1) || qIsNaN(value2)) { + return false; + } else if (value1 == value2) { + return true; + } else { + return qFuzzyCompare(value1, value2); + } +} + } diff --git a/src/QGC.h b/src/QGC.h index 658835bd533b6bcc7fe49d2ebf5cd4b217801511..1debeed33622ae42c16644c2dac5155f9f79e7cc 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -7,9 +7,7 @@ * ****************************************************************************/ - -#ifndef QGC_H -#define QGC_H +#pragma once #include #include @@ -42,7 +40,8 @@ void initTimer(); /** @brief Get the ground time since boot in milliseconds */ quint64 bootTimeMilliseconds(); -const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21; +/// Returns true if the two values are equal or close. Correctly handles 0 and NaN values. +bool fuzzyCompare(double value1, double value2); class SLEEP : public QThread { @@ -56,7 +55,3 @@ public: quint32 crc32(const quint8 *src, unsigned len, unsigned state); } - -#define QGC_EVENTLOOP_DEBUG 0 - -#endif // QGC_H diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9128684a85c36501feca8dc921ba98fb31128cb4..617baebe1be4945c4ef5cb654fbc9a33fac680a5 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3835,11 +3835,6 @@ const QVariantList& Vehicle::staticCameraList() const return emptyList; } -bool Vehicle::vehicleYawsToNextWaypointInMission() const -{ - return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this); -} - void Vehicle::_setupAutoDisarmSignalling() { QString param = _firmwarePlugin->autoDisarmParameter(this); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 73162690caa47f64275efe256a5c523e09b08657..407a2650bbd38a6fe6028f012db5fe64f18466fc 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1125,9 +1125,6 @@ public: QGCCameraManager* dynamicCameras () { return _cameras; } QString hobbsMeter (); - /// @true: When flying a mission the vehicle is always facing towards the next waypoint - bool vehicleYawsToNextWaypointInMission() const; - /// The vehicle is responsible for making the initial request for the Plan. /// @return: true: initial request is complete, false: initial request is still in progress; bool initialPlanRequestComplete() const { return _initialPlanRequestComplete; }