diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 94fce9df93b4806634b38fd81452d7a6c037b895..ea81cd0f7fa0012dbdf3b61b96beb3cb1a034024 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -96,12 +96,9 @@ void MissionController::_resetMissionFlightStatus(void) _missionFlightStatus.cruiseSpeed = _controllerVehicle->defaultCruiseSpeed(); _missionFlightStatus.hoverSpeed = _controllerVehicle->defaultHoverSpeed(); _missionFlightStatus.vehicleSpeed = _controllerVehicle->multiRotor() || _managerVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed; - _missionFlightStatus.vehicleYaw = 0.0; - _missionFlightStatus.gimbalYaw = std::numeric_limits::quiet_NaN(); - _missionFlightStatus.gimbalPitch = std::numeric_limits::quiet_NaN(); - - // Battery information - + _missionFlightStatus.vehicleYaw = qQNaN(); + _missionFlightStatus.gimbalYaw = qQNaN(); + _missionFlightStatus.gimbalPitch = qQNaN(); _missionFlightStatus.mAhBattery = 0; _missionFlightStatus.hoverAmps = 0; _missionFlightStatus.cruiseAmps = 0; @@ -110,6 +107,7 @@ void MissionController::_resetMissionFlightStatus(void) _missionFlightStatus.cruiseAmpsTotal = 0; _missionFlightStatus.batteryChangePoint = -1; _missionFlightStatus.batteriesRequired = -1; + _missionFlightStatus.vtolMode = _missionContainsVTOLTakeoff ? QGCMAVLink::VehicleClassMultiRotor : QGCMAVLink::VehicleClassFixedWing; _controllerVehicle->firmwarePlugin()->batteryConsumptionData(_controllerVehicle, _missionFlightStatus.mAhBattery, _missionFlightStatus.hoverAmps, _missionFlightStatus.cruiseAmps); if (_missionFlightStatus.mAhBattery != 0) { @@ -904,11 +902,11 @@ bool MissionController::_loadItemsFromJson(const QJsonObject& json, QmlObjectLis int fileVersion; JsonHelper::validateExternalQGCJsonFile(json, - _jsonFileTypeValue, // expected file type - 1, // minimum supported version - 2, // maximum supported version - fileVersion, - errorString); + _jsonFileTypeValue, // expected file type + 1, // minimum supported version + 2, // maximum supported version + fileVersion, + errorString); if (fileVersion == 1) { return _loadJsonMissionFileV1(json, visualItems, errorString); @@ -1505,10 +1503,8 @@ void MissionController::_recalcMissionFlightStatus() _resetMissionFlightStatus(); - bool vtolInHover = _missionContainsVTOLTakeoff; bool linkStartToHome = false; bool foundRTL = false; - bool vehicleYawSpecificallySet = false; double totalHorizontalDistance = 0; for (int i=0; i<_visualItems->count(); i++) { @@ -1525,22 +1521,7 @@ void MissionController::_recalcMissionFlightStatus() item->setDistance(0); item->setDistanceFromStart(0); - // Look for speed changed - 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; - } + // Gimbal states reflect the state AFTER executing the item // ROI commands cancel out previous gimbal yaw/pitch if (simpleItem) { @@ -1548,8 +1529,8 @@ void MissionController::_recalcMissionFlightStatus() case MAV_CMD_NAV_ROI: case MAV_CMD_DO_SET_ROI_LOCATION: case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: - _missionFlightStatus.gimbalYaw = std::numeric_limits::quiet_NaN(); - _missionFlightStatus.gimbalPitch = std::numeric_limits::quiet_NaN(); + _missionFlightStatus.gimbalYaw = qQNaN(); + _missionFlightStatus.gimbalPitch = qQNaN(); break; default: break; @@ -1566,28 +1547,115 @@ void MissionController::_recalcMissionFlightStatus() _missionFlightStatus.gimbalPitch = gimbalPitch; } - if (i == 0) { - // We only process speed and gimbal from Mission Settings item - continue; + // We don't need to do any more processing if: + // Mission Settings Item + // 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) { - // No more vehicle distances after RTL - continue; - } - - // 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); + // Speed, VTOL states changes are processed last since they take affect on the next item + + double newSpeed = item->specifiedFlightSpeed(); + if (!qIsNaN(newSpeed)) { + if (_controllerVehicle->multiRotor()) { + _missionFlightStatus.hoverSpeed = newSpeed; + } else if (_controllerVehicle->vtol()) { + if (_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor) { + _missionFlightStatus.hoverSpeed = newSpeed; + } else { + _missionFlightStatus.cruiseSpeed = newSpeed; } + } else { + _missionFlightStatus.cruiseSpeed = newSpeed; } + _missionFlightStatus.vehicleSpeed = newSpeed; } // Update VTOL state @@ -1595,21 +1663,19 @@ void MissionController::_recalcMissionFlightStatus() switch (simpleItem->command()) { 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 - vtolInHover = false; - break; case MAV_CMD_NAV_LAND: - vtolInHover = false; + _missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassFixedWing; break; case MAV_CMD_NAV_VTOL_LAND: - vtolInHover = true; + _missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassMultiRotor; break; case MAV_CMD_DO_VTOL_TRANSITION: { int transitionState = simpleItem->missionItem().param1(); if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) { - vtolInHover = true; + _missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassMultiRotor; } else if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_FW) { - vtolInHover = false; + _missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassFixedWing; } } break; @@ -1617,74 +1683,6 @@ void MissionController::_recalcMissionFlightStatus() 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); @@ -1697,7 +1695,7 @@ void MissionController::_recalcMissionFlightStatus() double hoverTime = distance / _missionFlightStatus.hoverSpeed; double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble(); - _addTimeDistance(vtolInHover, hoverTime, cruiseTime, distance, landTime, -1); + _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, distance, landTime, -1); } if (_missionFlightStatus.mAhBattery != 0 && _missionFlightStatus.batteryChangePoint == -1) { diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 7df122531e7348070282179ca117bfcc958b9f4b..9f6c936b7c4704c23e7d546ab7c2d784a3fef494 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -45,27 +45,29 @@ public: ~MissionController(); typedef struct { - double maxTelemetryDistance; - double totalDistance; - double totalTime; - double hoverDistance; - double hoverTime; - double cruiseDistance; - double cruiseTime; - double cruiseSpeed; - double hoverSpeed; - double vehicleSpeed; ///< Either cruise or hover speed based on vehicle type and vtol state - double vehicleYaw; - double gimbalYaw; ///< NaN signals yaw was never changed - double gimbalPitch; ///< NaN signals pitch was never changed - int mAhBattery; ///< 0 for not available - double hoverAmps; ///< Amp consumption during hover - double cruiseAmps; ///< Amp consumption during cruise - double ampMinutesAvailable; ///< Amp minutes available from single battery - double hoverAmpsTotal; ///< Total hover amps used - double cruiseAmpsTotal; ///< Total cruise amps used - int batteryChangePoint; ///< -1 for not supported, 0 for not needed - int batteriesRequired; ///< -1 for not supported + double maxTelemetryDistance; + double totalDistance; + double totalTime; + double hoverDistance; + double hoverTime; + double cruiseDistance; + double cruiseTime; + int mAhBattery; ///< 0 for not available + double hoverAmps; ///< Amp consumption during hover + double cruiseAmps; ///< Amp consumption during cruise + double ampMinutesAvailable; ///< Amp minutes available from single battery + double hoverAmpsTotal; ///< Total hover amps used + double cruiseAmpsTotal; ///< Total cruise amps used + int batteryChangePoint; ///< -1 for not supported, 0 for not needed + int batteriesRequired; ///< -1 for not supported + double vehicleYaw; + double gimbalYaw; ///< NaN signals yaw was never changed + double gimbalPitch; ///< NaN signals pitch was never changed + // The following values are the state prior to executing this item + QGCMAVLink::VehicleClass_t vtolMode; ///< Either VehicleClassFixedWing, VehicleClassMultiRotor, VehicleClassGeneric (mode unknown) + double cruiseSpeed; + double hoverSpeed; + double vehicleSpeed; ///< Either cruise or hover speed based on vehicle type and vtol state } MissionFlightStatus_t; Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged) diff --git a/src/MissionManager/MissionControllerTest.cc b/src/MissionManager/MissionControllerTest.cc index 710a10402149bb037435611e04c3a1ef74533a14..10e34caa94c9cd17bd66881b01d88a2632765eb7 100644 --- a/src/MissionManager/MissionControllerTest.cc +++ b/src/MissionManager/MissionControllerTest.cc @@ -167,17 +167,68 @@ void MissionControllerTest::_testGimbalRecalc(void) QVERIFY(qIsNaN(visualItem->missionGimbalYaw())); } -#if 0 - // FIXME: No longer works due to signal compression - // Specify gimbal yaw on settings item should generate yaw on all items - MissionSettingsItem* settingsItem = _missionController->visualItems()->value(0); - settingsItem->cameraSection()->setSpecifyGimbal(true); - settingsItem->cameraSection()->gimbalYaw()->setRawValue(0.0); + // Specify gimbal yaw on settings item should generate yaw on all subsequent items + const int yawIndex = 2; + SimpleMissionItem* item = _missionController->visualItems()->value(yawIndex); + item->cameraSection()->setSpecifyGimbal(true); + item->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++) { + //qDebug() << i; VisualMissionItem* visualItem = _missionController->visualItems()->value(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; ivisualItems()->value(i); + QCOMPARE(visualItem->missionVehicleYaw(), expectedVehicleYaw); + if (i <= cMissionItems - 1) { + expectedVehicleYaw += wpAngleInc; + } + } + + SimpleMissionItem* simpleItem = _missionController->visualItems()->value(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; ivisualItems()->value(i); + QCOMPARE(visualItem->missionVehicleYaw(), i == 3 ? 66.0 : expectedVehicleYaw); + if (i <= cMissionItems - 1) { + expectedVehicleYaw += wpAngleInc; + } } -#endif } void MissionControllerTest::_testLoadJsonSectionAvailable(void) diff --git a/src/MissionManager/MissionControllerTest.h b/src/MissionManager/MissionControllerTest.h index 902f77523537ea65425647fb4707107f3985714f..94cba3d5cba98d54a12cf6d81ab650ac86a55ffb 100644 --- a/src/MissionManager/MissionControllerTest.h +++ b/src/MissionManager/MissionControllerTest.h @@ -32,11 +32,12 @@ public: private slots: void cleanup(void); - void _testGimbalRecalc (void); void _testLoadJsonSectionAvailable (void); void _testEmptyVehicleAPM (void); void _testEmptyVehiclePX4 (void); void _testGlobalAltMode (void); + void _testGimbalRecalc (void); + void _testVehicleYawRecalc (void); private: #if 0