Browse Source

Merge pull request #2055 from DonLakeFlyer/LinesAndMore

Lines and more
QGC4.4
Don Gagne 10 years ago
parent
commit
904518b682
  1. 1
      qgroundcontrol.qrc
  2. 8
      src/FlightDisplay/FlightDisplayView.qml
  3. 6
      src/FlightMap/MapItems/MissionItemView.qml
  4. 45
      src/FlightMap/MapItems/MissionLineView.qml
  5. 1
      src/FlightMap/qmldir
  6. 27
      src/MissionEditor/MissionEditor.qml
  7. 8
      src/MissionEditor/MissionEditorController.cc
  8. 10
      src/MissionItem.cc
  9. 11
      src/MissionItem.h
  10. 96
      src/MissionManager/MissionController.cc
  11. 9
      src/MissionManager/MissionController.h
  12. 22
      src/MissionManager/MissionManager.cc
  13. 36
      src/MissionManager/MissionManagerTest.cc

1
qgroundcontrol.qrc

@ -177,6 +177,7 @@
<file alias="QGroundControl/FlightMap/MissionItemIndicator.qml">src/FlightMap/MapItems/MissionItemIndicator.qml</file> <file alias="QGroundControl/FlightMap/MissionItemIndicator.qml">src/FlightMap/MapItems/MissionItemIndicator.qml</file>
<file alias="QGroundControl/FlightMap/VehicleMapItem.qml">src/FlightMap/MapItems/VehicleMapItem.qml</file> <file alias="QGroundControl/FlightMap/VehicleMapItem.qml">src/FlightMap/MapItems/VehicleMapItem.qml</file>
<file alias="QGroundControl/FlightMap/MissionItemView.qml">src/FlightMap/MapItems/MissionItemView.qml</file> <file alias="QGroundControl/FlightMap/MissionItemView.qml">src/FlightMap/MapItems/MissionItemView.qml</file>
<file alias="QGroundControl/FlightMap/MissionLineView.qml">src/FlightMap/MapItems/MissionLineView.qml</file>
</qresource> </qresource>
<qresource prefix="/res"> <qresource prefix="/res">

8
src/FlightDisplay/FlightDisplayView.qml

@ -52,7 +52,7 @@ Item {
readonly property alias zOrderWidgets: flightMap.zOrderWidgets readonly property alias zOrderWidgets: flightMap.zOrderWidgets
readonly property alias zOrderMapItems: flightMap.zOrderMapItems readonly property alias zOrderMapItems: flightMap.zOrderMapItems
property var __qgcPal: QGCPalette { colorGroupEnabled: enabled } QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
property var _activeVehicle: multiVehicleManager.activeVehicle property var _activeVehicle: multiVehicleManager.activeVehicle
@ -184,6 +184,12 @@ Item {
zOrderMapItems: flightMap.zOrderMapItems zOrderMapItems: flightMap.zOrderMapItems
} }
// Add lines between waypoints
MissionLineView {
model: _missionController.waypointLines
zOrderMapItems: flightMap.zOrderMapItems
}
Loader { Loader {
id: flightMapDelayLoader id: flightMapDelayLoader
anchors.fill: parent anchors.fill: parent

6
src/FlightMap/MapItems/MissionItemView.qml

@ -29,9 +29,7 @@ import QtPositioning 5.3
import QGroundControl 1.0 import QGroundControl 1.0
import QGroundControl.FlightMap 1.0 import QGroundControl.FlightMap 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0 import QGroundControl.Controls 1.0
import QGroundControl.Palette 1.0
/// The MissionItemView control is used to add Mission Item Indicators to a FlightMap. /// The MissionItemView control is used to add Mission Item Indicators to a FlightMap.
MapItemView { MapItemView {
@ -42,11 +40,11 @@ MapItemView {
delegate: MissionItemIndicator { delegate: MissionItemIndicator {
id: itemIndicator id: itemIndicator
label: object.sequenceNumber == 0 ? "H" : object.sequenceNumber label: object.homePosition ? "H" : object.sequenceNumber
isCurrentItem: object.isCurrentItem isCurrentItem: object.isCurrentItem
coordinate: object.coordinate coordinate: object.coordinate
z: zOrderMapItems z: zOrderMapItems
visible: object.specifiesCoordinate visible: object.specifiesCoordinate && (!object.homePosition || object.homePositionValid)
onClicked: setCurrentItem(object.sequenceNumber) onClicked: setCurrentItem(object.sequenceNumber)

45
src/FlightMap/MapItems/MissionLineView.qml

@ -0,0 +1,45 @@
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
import QtQuick 2.4
import QtLocation 5.3
import QtPositioning 5.3
import QGroundControl 1.0
import QGroundControl.Palette 1.0
/// The MissionLineView control is used to add lines between mission items
MapItemView {
property real zOrderMapItems ///< Z order for indicator
delegate: MapPolyline {
line.width: 3
line.color: "#be781c" // Hack, can't get palette to work in here
z: zOrderMapItems - 1 // Under item indicators
path: [
{ latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
{ latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
]
}
}

1
src/FlightMap/qmldir

@ -21,3 +21,4 @@ QGCSpeedWidget 1.0 QGCSpeedWidget.qml
VehicleMapItem 1.0 VehicleMapItem.qml VehicleMapItem 1.0 VehicleMapItem.qml
MissionItemIndicator 1.0 MissionItemIndicator.qml MissionItemIndicator 1.0 MissionItemIndicator.qml
MissionItemView 1.0 MissionItemView.qml MissionItemView 1.0 MissionItemView.qml
MissionLineView 1.0 MissionLineView.qml

27
src/MissionEditor/MissionEditor.qml

@ -69,9 +69,13 @@ QGCView {
MissionEditorController { MissionEditorController {
id: controller id: controller
/*
FIXME: autoSync is temporarily disconnected since it's still buggy
autoSync: QGroundControl.flightMapSettings.loadMapSetting(editorMap.mapName, _autoSyncKey, true) autoSync: QGroundControl.flightMapSettings.loadMapSetting(editorMap.mapName, _autoSyncKey, true)
onAutoSyncChanged: QGroundControl.flightMapSettings.saveMapSetting(editorMap.mapName, _autoSyncKey, autoSync) onAutoSyncChanged: QGroundControl.flightMapSettings.saveMapSetting(editorMap.mapName, _autoSyncKey, autoSync)
*/
onMissionItemsChanged: { onMissionItemsChanged: {
updateHomePosition() updateHomePosition()
@ -98,6 +102,7 @@ QGCView {
function updateHomePosition() { function updateHomePosition() {
homePosition = liveHomePositionAvailable ? liveHomePosition : offlineHomePosition homePosition = liveHomePositionAvailable ? liveHomePosition : offlineHomePosition
_missionItems.get(0).coordinate = homePosition _missionItems.get(0).coordinate = homePosition
_missionItems.get(0).homePositionValid = true
} }
Component.onCompleted: updateHomePosition() Component.onCompleted: updateHomePosition()
@ -222,20 +227,9 @@ QGCView {
} }
// Add lines between waypoints // Add lines between waypoints
MapItemView { MissionLineView {
model: controller.waypointLines model: controller.waypointLines
zOrderMapItems: editorMap.zOrderMapItems
delegate:
MapPolyline {
line.width: 3
line.color: qgcPal.mapButtonHighlight
z: editorMap.zOrderMapItems - 1 // Under item indicators
path: [
{ latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
{ latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
]
}
} }
// Mission Item Editor // Mission Item Editor
@ -880,7 +874,7 @@ QGCView {
} }
Row { Row {
visible: autoSyncCheckBox.enabled && autoSyncCheckBox.checked visible: true //autoSyncCheckBox.enabled && autoSyncCheckBox.checked
spacing: ScreenTools.defaultFontPixelWidth spacing: ScreenTools.defaultFontPixelWidth
QGCButton { QGCButton {
@ -925,6 +919,8 @@ QGCView {
} }
} }
} }
/*
FIXME: autoSync is temporarily disconnected since it's still buggy
QGCLabel { QGCLabel {
id: autoSyncDisallowedLabel id: autoSyncDisallowedLabel
@ -940,6 +936,7 @@ QGCView {
onClicked: controller.autoSync = checked onClicked: controller.autoSync = checked
} }
*/
} }
} }
} // QGCVIew } // QGCVIew

8
src/MissionEditor/MissionEditorController.cc

@ -288,10 +288,6 @@ void MissionEditorController::_recalcWaypointLines(void)
// This will update the sequence numbers to be sequential starting from 0 // This will update the sequence numbers to be sequential starting from 0
void MissionEditorController::_recalcSequence(void) void MissionEditorController::_recalcSequence(void)
{ {
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));
currentParentItem->childItems()->clear();
for (int i=0; i<_missionItems->count(); i++) { for (int i=0; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i)); MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
@ -340,9 +336,7 @@ void MissionEditorController::_initAllMissionItems(void)
_initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i))); _initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
} }
_recalcSequence(); _recalcAll();
_recalcChildItems();
_recalcWaypointLines();
emit missionItemsChanged(); emit missionItemsChanged();
emit canEditChanged(_canEdit); emit canEditChanged(_canEdit);

10
src/MissionItem.cc

@ -90,8 +90,9 @@ MissionItem::MissionItem(QObject* parent,
, _isCurrentItem(isCurrentItem) , _isCurrentItem(isCurrentItem)
, _reachedTime(0) , _reachedTime(0)
, _headingDegreesFact(NULL) , _headingDegreesFact(NULL)
,_dirty(false) , _dirty(false)
, _homePositionSpecialCase(false) , _homePositionSpecialCase(false)
, _homePositionValid(false)
{ {
_latitudeFact = new Fact(0, "Latitude:", FactMetaData::valueTypeDouble, this); _latitudeFact = new Fact(0, "Latitude:", FactMetaData::valueTypeDouble, this);
_longitudeFact = new Fact(0, "Longitude:", FactMetaData::valueTypeDouble, this); _longitudeFact = new Fact(0, "Longitude:", FactMetaData::valueTypeDouble, this);
@ -198,6 +199,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
_altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact; _altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact;
_dirty = other._dirty; _dirty = other._dirty;
_homePositionSpecialCase = other._homePositionSpecialCase; _homePositionSpecialCase = other._homePositionSpecialCase;
_homePositionValid = other._homePositionValid;
*_latitudeFact = *other._latitudeFact; *_latitudeFact = *other._latitudeFact;
*_longitudeFact = *other._longitudeFact; *_longitudeFact = *other._longitudeFact;
@ -915,3 +917,9 @@ void MissionItem::_headingDegreesFactChanged(QVariant value)
{ {
emit headingDegreesChanged(value.toDouble()); emit headingDegreesChanged(value.toDouble());
} }
void MissionItem::setHomePositionValid(bool homePositionValid)
{
_homePositionValid = homePositionValid;
emit homePositionValidChanged(_homePositionValid);
}

11
src/MissionItem.h

@ -84,7 +84,13 @@ public:
Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts NOTIFY commandChanged) Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts NOTIFY commandChanged)
Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged)
Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT)
/// true: this item is being used as a home position indicator
Q_PROPERTY(bool homePosition MEMBER _homePositionSpecialCase CONSTANT)
/// true: home position should be shown
Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged)
// Property accesors // Property accesors
int sequenceNumber(void) const { return _sequenceNumber; } int sequenceNumber(void) const { return _sequenceNumber; }
@ -124,6 +130,9 @@ public:
void setDirty(bool dirty); void setDirty(bool dirty);
QmlObjectListModel* childItems(void) { return &_childItems; } QmlObjectListModel* childItems(void) { return &_childItems; }
bool homePositionValid(void) { return _homePositionValid; }
void setHomePositionValid(bool homePositionValid);
// C++ only methods // C++ only methods
@ -210,6 +219,7 @@ signals:
void coordinateChanged(const QGeoCoordinate& coordinate); void coordinateChanged(const QGeoCoordinate& coordinate);
void headingDegreesChanged(double heading); void headingDegreesChanged(double heading);
void dirtyChanged(bool dirty); void dirtyChanged(bool dirty);
void homePositionValidChanged(bool homePostionValid);
/** @brief Announces a change to the waypoint data */ /** @brief Announces a change to the waypoint data */
void changed(MissionItem* wp); void changed(MissionItem* wp);
@ -290,6 +300,7 @@ private:
bool _dirty; bool _dirty;
bool _homePositionSpecialCase; ///< true: this item is being used as a ui home position indicator bool _homePositionSpecialCase; ///< true: this item is being used as a ui home position indicator
bool _homePositionValid; ///< true: home psition should be displayed
/// This is used to reference any subsequent mission items which do not specify a coordinate. /// This is used to reference any subsequent mission items which do not specify a coordinate.
QmlObjectListModel _childItems; QmlObjectListModel _childItems;

96
src/MissionManager/MissionController.cc

@ -71,28 +71,32 @@ void MissionController::_newMissionItemsAvailable(void)
void MissionController::_recalcWaypointLines(void) void MissionController::_recalcWaypointLines(void)
{ {
bool firstCoordinateItem = true; int firstIndex = _homePositionValid ? 0 : 1;
MissionItem* lastCoordinateItem = qobject_cast<MissionItem*>(_missionItems->get(0));
_waypointLines.clear(); _waypointLines.clear();
for (int i=1; i<_missionItems->count(); i++) { if (firstIndex < _missionItems->count()) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i)); bool firstCoordinateItem = true;
MissionItem* lastCoordinateItem = qobject_cast<MissionItem*>(_missionItems->get(firstIndex));
if (item->specifiesCoordinate()) {
if (firstCoordinateItem) { for (int i=firstIndex; i<_missionItems->count(); i++) {
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
// The first coordinate we hit is a takeoff command so link back to home position
_waypointLines.append(new CoordinateVector(qobject_cast<MissionItem*>(_missionItems->get(0))->coordinate(), item->coordinate())); if (item->specifiesCoordinate()) {
if (firstCoordinateItem) {
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF && _homePositionValid) {
// The first coordinate we hit is a takeoff command so link back to home position if we have one
_waypointLines.append(new CoordinateVector(qobject_cast<MissionItem*>(_missionItems->get(0))->coordinate(), item->coordinate()));
} else {
// First coordiante is not a takeoff command, it does not link backwards to anything
}
firstCoordinateItem = false;
} else { } else {
// First coordiante is not a takeoff command, it does not link backwards to anything // Subsequent coordinate items link to last coordinate item
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
} }
firstCoordinateItem = false; lastCoordinateItem = item;
} else {
// Subsequent coordinate items link to last coordinate item
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
} }
lastCoordinateItem = item;
} }
} }
@ -102,25 +106,41 @@ void MissionController::_recalcWaypointLines(void)
// This will update the child item hierarchy // This will update the child item hierarchy
void MissionController::_recalcChildItems(void) void MissionController::_recalcChildItems(void)
{ {
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0)); int firstIndex = _homePositionValid ? 0 : 1;
currentParentItem->childItems()->clear(); if (_missionItems->count() > firstIndex) {
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(firstIndex));
for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i)); currentParentItem->childItems()->clear();
// Set up non-coordinate item child hierarchy for (int i=firstIndex+1; i<_missionItems->count(); i++) {
if (item->specifiesCoordinate()) { MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
item->childItems()->clear();
currentParentItem = item; // Set up non-coordinate item child hierarchy
} else { if (item->specifiesCoordinate()) {
currentParentItem->childItems()->append(item); item->childItems()->clear();
currentParentItem = item;
} else {
currentParentItem->childItems()->append(item);
}
} }
} }
} }
// This will update the sequence numbers to be sequential starting from 0
void MissionController::_recalcSequence(void)
{
for (int i=0; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
// Setup ascending sequence numbers
item->setSequenceNumber(i);
}
}
void MissionController::_recalcAll(void) void MissionController::_recalcAll(void)
{ {
_recalcSequence();
_recalcChildItems(); _recalcChildItems();
_recalcWaypointLines(); _recalcWaypointLines();
} }
@ -131,11 +151,13 @@ void MissionController::_initAllMissionItems(void)
// Add the home position item to the front // Add the home position item to the front
MissionItem* homeItem = new MissionItem(this); MissionItem* homeItem = new MissionItem(this);
homeItem->setHomePositionSpecialCase(true); homeItem->setHomePositionSpecialCase(true);
homeItem->setHomePositionValid(false);
homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
homeItem->setLatitude(47.3769);
homeItem->setLongitude(8.549444);
_missionItems->insert(0, homeItem); _missionItems->insert(0, homeItem);
_recalcChildItems(); _recalcAll();
_recalcWaypointLines();
emit missionItemsChanged(); emit missionItemsChanged();
} }
@ -156,3 +178,11 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
_newMissionItemsAvailable(); _newMissionItemsAvailable();
} }
} }
void MissionController::setHomePositionValid(bool homePositionValid)
{
_homePositionValid = homePositionValid;
qobject_cast<MissionItem*>(_missionItems->get(0))->setHomePositionValid(homePositionValid);
emit homePositionValidChanged(_homePositionValid);
}

9
src/MissionManager/MissionController.h

@ -40,21 +40,29 @@ public:
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged) Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged)
Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged)
/// true: home position should be shown on map, false: home position not shown on map
Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged)
// Property accessors // Property accessors
QmlObjectListModel* missionItems(void) { return _missionItems; } QmlObjectListModel* missionItems(void) { return _missionItems; }
QmlObjectListModel* waypointLines(void) { return &_waypointLines; } QmlObjectListModel* waypointLines(void) { return &_waypointLines; }
bool homePositionValid(void) { return _homePositionValid; }
void setHomePositionValid(bool homPositionValid);
signals: signals:
void missionItemsChanged(void); void missionItemsChanged(void);
void waypointLinesChanged(void); void waypointLinesChanged(void);
void homePositionValidChanged(bool homePositionValid);
private slots: private slots:
void _newMissionItemsAvailable(); void _newMissionItemsAvailable();
void _activeVehicleChanged(Vehicle* activeVehicle); void _activeVehicleChanged(Vehicle* activeVehicle);
private: private:
void _recalcSequence(void);
void _recalcWaypointLines(void); void _recalcWaypointLines(void);
void _recalcChildItems(void); void _recalcChildItems(void);
void _recalcAll(void); void _recalcAll(void);
@ -64,6 +72,7 @@ private:
QmlObjectListModel* _missionItems; QmlObjectListModel* _missionItems;
QmlObjectListModel _waypointLines; QmlObjectListModel _waypointLines;
Vehicle* _activeVehicle; Vehicle* _activeVehicle;
bool _homePositionValid;
}; };
#endif #endif

22
src/MissionManager/MissionManager.cc

@ -57,21 +57,21 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems, b
{ {
_retryCount = 0; _retryCount = 0;
_missionItems.clear(); _missionItems.clear();
int firstIndex = skipFirstItem ? 1 : 0;
for (int i=skipFirstItem ? 1: 0; i<missionItems.count(); i++) { for (int i=firstIndex; i<missionItems.count(); i++) {
_missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i]))); _missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i])));
}
emit newMissionItemsAvailable(); MissionItem* item = qobject_cast<MissionItem*>(_missionItems.get(_missionItems.count() - 1));
if (skipFirstItem) { // Editor uses 1-based sequence numbers, adjust them before going out
for (int i=0; i<_missionItems.count(); i++) { item->setSequenceNumber(item->sequenceNumber() - 1);
MissionItem* item = qobject_cast<MissionItem*>(_missionItems[i]); if (item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) {
item->setParam1((int)item->param1() - 1);
if (item->command() == MavlinkQmlSingleton::MAV_CMD_CONDITION_DELAY) {
item->setParam1((int)item->param1() - 1);
}
} }
} }
emit newMissionItemsAvailable();
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();

36
src/MissionManager/MissionManagerTest.cc

@ -28,14 +28,14 @@
UT_REGISTER_TEST(MissionManagerTest) UT_REGISTER_TEST(MissionManagerTest)
const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = { const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = {
{ "1\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "0\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 0, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "2\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 2, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "3\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 3, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "4\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 4, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_TAKEOFF, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "5\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 5, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_TAKEOFF, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, { "6\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 6, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } },
{ "1\t0\t2\t177\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, { "7\t0\t2\t177\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 7, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } },
}; };
MissionManagerTest::MissionManagerTest(void) MissionManagerTest::MissionManagerTest(void)
@ -146,6 +146,16 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
QmlObjectListModel* list = new QmlObjectListModel(); QmlObjectListModel* list = new QmlObjectListModel();
// Editor has a home position item on the front, so we do the same
MissionItem* homeItem = new MissionItem(this);
homeItem->setHomePositionSpecialCase(true);
homeItem->setHomePositionValid(false);
homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
homeItem->setLatitude(47.3769);
homeItem->setLongitude(8.549444);
homeItem->setSequenceNumber(0);
list->insert(0, homeItem);
for (size_t i=0; i<cTestCases; i++) { for (size_t i=0; i<cTestCases; i++) {
const TestCase_t* testCase = &_rgTestCases[i]; const TestCase_t* testCase = &_rgTestCases[i];
@ -153,12 +163,19 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly); QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly);
QVERIFY(item->load(loadStream)); QVERIFY(item->load(loadStream));
// Mission Manager expects to get 1-base sequence numbers for write
item->setSequenceNumber(item->sequenceNumber() + 1);
if (item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) {
item->setParam1((int)item->param1() + 1);
}
list->append(item); list->append(item);
} }
// Send the items to the vehicle // Send the items to the vehicle
_missionManager->writeMissionItems(*list, false /* skipFirstItem */); _missionManager->writeMissionItems(*list, true /* skipFirstItem */);
// writeMissionItems should emit these signals before returning: // writeMissionItems should emit these signals before returning:
// inProgressChanged // inProgressChanged
@ -304,16 +321,17 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
MissionItem* actual = qobject_cast<MissionItem*>(_missionManager->missionItems()->get(i)); MissionItem* actual = qobject_cast<MissionItem*>(_missionManager->missionItems()->get(i));
qDebug() << "Test case" << i; qDebug() << "Test case" << i;
QCOMPARE(actual->sequenceNumber(), testCase->expectedItem.sequenceNumber);
QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude()); QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude());
QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude()); QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude());
QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude()); QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude());
QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command); QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command);
QCOMPARE(actual->param1(), testCase->expectedItem.param1);
QCOMPARE(actual->param2(), testCase->expectedItem.param2); QCOMPARE(actual->param2(), testCase->expectedItem.param2);
QCOMPARE(actual->param3(), testCase->expectedItem.param3); QCOMPARE(actual->param3(), testCase->expectedItem.param3);
QCOMPARE(actual->param4(), testCase->expectedItem.param4); QCOMPARE(actual->param4(), testCase->expectedItem.param4);
QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue); QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue);
QCOMPARE(actual->frame(), testCase->expectedItem.frame); QCOMPARE(actual->frame(), testCase->expectedItem.frame);
QCOMPARE(actual->param1(), testCase->expectedItem.param1);
} }
} }

Loading…
Cancel
Save