Skip to content

Q
qgroundcontrol
Unverified Commit a07ec8aa authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #8947 from DonLakeFlyer/PlanMissionState 

Plan: Fix recalc bugs associated with vtol state, speed, vehicle/gimbal yaw
parents 7400e5d7 d910670d
Hide whitespace changes
Inline Side-by-side
Showing with 206 additions and 154 deletions
src/MissionManager/MissionController.cc
View file @ a07ec8aa
...@@ -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 = qQNaN();
_missionFlightStatus.gimbalPitch = std::numeric_limits<double>::quiet_NaN(); _missionFlightStatus.gimbalPitch = qQNaN();
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(vtolInHover, hoverTime, cruiseTime, distance, landTime, -1); _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, 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 @ a07ec8aa
...@@ -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 @ a07ec8aa
...@@ -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 @ a07ec8aa
...@@ -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
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment