zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fix recalc bugs associated with vtol state, speed, vehicle/gimbal yaw

QGC4.4
DonLakeFlyer 5 years ago
parent
ac51a6d24e
commit
d910670dcc
4 changed files with 206 additions and 154 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 148
      src/MissionManager/MissionController.cc
  2. 14
      src/MissionManager/MissionController.h
  3. 65
      src/MissionManager/MissionControllerTest.cc
  4. 3
      src/MissionManager/MissionControllerTest.h

148
src/MissionManager/MissionController.cc
Unescape View File

@ -96,12 +96,9 @@ void MissionController::_resetMissionFlightStatus(void) @@ -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<double>::quiet_NaN();
_missionFlightStatus.gimbalPitch = std::numeric_limits<double>::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) @@ -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) {
@ -1505,10 +1503,8 @@ void MissionController::_recalcMissionFlightStatus() @@ -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() @@ -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() @@ -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<double>::quiet_NaN();
_missionFlightStatus.gimbalPitch = std::numeric_limits<double>::quiet_NaN();
_missionFlightStatus.gimbalYaw = qQNaN();
_missionFlightStatus.gimbalPitch = qQNaN();
break;
default:
break;
@ -1566,15 +1547,13 @@ void MissionController::_recalcMissionFlightStatus() @@ -1566,15 +1547,13 @@ void MissionController::_recalcMissionFlightStatus()
_missionFlightStatus.gimbalPitch = gimbalPitch;
}
if (i == 0) {
// We only process speed and gimbal from Mission Settings item
continue;
}
if (foundRTL) {
// No more vehicle distances after RTL
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)) {
@ -1590,35 +1569,7 @@ void MissionController::_recalcMissionFlightStatus() @@ -1590,35 +1569,7 @@ void MissionController::_recalcMissionFlightStatus()
}
}
// Update VTOL state
if (simpleItem && _controllerVehicle->vtol()) {
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;
break;
case MAV_CMD_NAV_VTOL_LAND:
vtolInHover = true;
break;
case MAV_CMD_DO_VTOL_TRANSITION:
{
int transitionState = simpleItem->missionItem().param1();
if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) {
vtolInHover = true;
} else if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_FW) {
vtolInHover = false;
}
}
break;
default:
break;
}
}
_addTimeDistance(vtolInHover, 0, 0, item->additionalTimeDelay(), 0, -1);
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, 0, 0, item->additionalTimeDelay(), 0, -1);
if (item->specifiesCoordinate()) {
@ -1639,14 +1590,17 @@ void MissionController::_recalcMissionFlightStatus() @@ -1639,14 +1590,17 @@ void MissionController::_recalcMissionFlightStatus()
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());
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());
}
lastFlyThroughVI->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
} else {
_missionFlightStatus.vehicleYaw = newVehicleYaw;
}
simpleItem->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
}
if (lastFlyThroughVI != _settingsItem || linkStartToHome) {
@ -1665,7 +1619,7 @@ void MissionController::_recalcMissionFlightStatus() @@ -1665,7 +1619,7 @@ void MissionController::_recalcMissionFlightStatus()
// Calculate time/distance
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
_addTimeDistance(vtolInHover, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
}
if (complexItem) {
@ -1675,17 +1629,61 @@ void MissionController::_recalcMissionFlightStatus() @@ -1675,17 +1629,61 @@ void MissionController::_recalcMissionFlightStatus()
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
_addTimeDistance(vtolInHover, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
totalHorizontalDistance += distance;
}
item->setMissionFlightStatus(_missionFlightStatus);
lastFlyThroughVI = item;
}
}
}
// 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
if (simpleItem && _controllerVehicle->vtol()) {
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
case MAV_CMD_NAV_LAND:
_missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassFixedWing;
break;
case MAV_CMD_NAV_VTOL_LAND:
_missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassMultiRotor;
break;
case MAV_CMD_DO_VTOL_TRANSITION:
{
int transitionState = simpleItem->missionItem().param1();
if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) {
_missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassMultiRotor;
} else if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_FW) {
_missionFlightStatus.vtolMode = QGCMAVLink::VehicleClassFixedWing;
}
}
break;
default:
break;
}
}
}
lastFlyThroughVI->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
// Add the information for the final segment back to home
@ -1697,7 +1695,7 @@ void MissionController::_recalcMissionFlightStatus() @@ -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) {

14
src/MissionManager/MissionController.h
Unescape View File

@ -52,12 +52,6 @@ public: @@ -52,12 +52,6 @@ public:
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
@ -66,6 +60,14 @@ public: @@ -66,6 +60,14 @@ public:
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)

65
src/MissionManager/MissionControllerTest.cc
Unescape View File

@ -167,17 +167,68 @@ void MissionControllerTest::_testGimbalRecalc(void) @@ -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<MissionSettingsItem*>(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<SimpleMissionItem*>(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<VisualMissionItem*>(i);
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)

3
src/MissionManager/MissionControllerTest.h
Unescape View File

@ -32,11 +32,12 @@ public: @@ -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

Write Preview
Loading…
Cancel
Save