Browse Source

Merge pull request #1972 from DonLakeFlyer/MissionEditor

More work on waypoint lines
QGC4.4
Don Gagne 10 years ago
parent
commit
27f2c6927f
  1. 169
      src/MissionEditor/MissionEditor.cc
  2. 13
      src/MissionEditor/MissionEditor.h
  3. 112
      src/MissionEditor/MissionEditor.qml
  4. 14
      src/MissionItem.cc
  5. 11
      src/MissionItem.h
  6. 25
      src/MissionManager/MissionManager.cc
  7. 4
      src/MissionManager/MissionManager.h
  8. 2
      src/MissionManager/MissionManagerTest.cc
  9. 18
      src/QmlControls/MissionItemEditor.qml
  10. 15
      src/QmlControls/QmlObjectListModel.cc
  11. 3
      src/QmlControls/QmlObjectListModel.h

169
src/MissionEditor/MissionEditor.cc

@ -52,7 +52,7 @@ MissionEditor::MissionEditor(QWidget *parent)
_newMissionItemsAvailable(); _newMissionItemsAvailable();
} else { } else {
_missionItems = new QmlObjectListModel(this); _missionItems = new QmlObjectListModel(this);
connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged); _initAllMissionItems();
} }
setContextPropertyObject("controller", this); setContextPropertyObject("controller", this);
@ -67,6 +67,7 @@ MissionEditor::~MissionEditor()
void MissionEditor::_newMissionItemsAvailable(void) void MissionEditor::_newMissionItemsAvailable(void)
{ {
if (_missionItems) { if (_missionItems) {
_deinitAllMissionItems();
_missionItems->deleteLater(); _missionItems->deleteLater();
} }
@ -74,14 +75,8 @@ void MissionEditor::_newMissionItemsAvailable(void)
_canEdit = missionManager->canEdit(); _canEdit = missionManager->canEdit();
_missionItems = missionManager->copyMissionItems(); _missionItems = missionManager->copyMissionItems();
_reSequence();
_missionItems->setDirty(false);
connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged);
_rebuildWaypointLines();
emit missionItemsChanged(); _initAllMissionItems();
emit canEditChanged(_canEdit);
} }
void MissionEditor::getMissionItems(void) void MissionEditor::getMissionItems(void)
@ -97,10 +92,11 @@ void MissionEditor::getMissionItems(void)
void MissionEditor::setMissionItems(void) void MissionEditor::setMissionItems(void)
{ {
// FIXME: Need to pull out home position
Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
if (activeVehicle) { if (activeVehicle) {
activeVehicle->missionManager()->writeMissionItems(*_missionItems); activeVehicle->missionManager()->writeMissionItems(*_missionItems, true /* skipFirstItem */);
_missionItems->setDirty(false); _missionItems->setDirty(false);
} }
} }
@ -112,21 +108,17 @@ int MissionEditor::addMissionItem(QGeoCoordinate coordinate)
} }
MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT); MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT);
_initMissionItem(newItem);
newItem->setAltitude(30); newItem->setAltitude(30);
if (_missionItems->count() == 0) { if (_missionItems->count() == 1) {
newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
} }
qDebug() << "MissionItem" << newItem->coordinate(); qDebug() << "MissionItem" << newItem->coordinate();
_missionItems->append(newItem); _missionItems->append(newItem);
return _missionItems->count() - 1; _recalcAll();
}
void MissionEditor::_reSequence(void) return _missionItems->count() - 1;
{
for (int i=0; i<_missionItems->count(); i++) {
qobject_cast<MissionItem*>(_missionItems->get(i))->setSequenceNumber(i);
}
} }
void MissionEditor::removeMissionItem(int index) void MissionEditor::removeMissionItem(int index)
@ -136,8 +128,11 @@ void MissionEditor::removeMissionItem(int index)
return; return;
} }
_missionItems->removeAt(index); MissionItem* item = qobject_cast<MissionItem*>(_missionItems->removeAt(index));
_reSequence();
_deinitMissionItem(item);
_recalcAll();
} }
void MissionEditor::moveUp(int index) void MissionEditor::moveUp(int index)
@ -160,7 +155,7 @@ void MissionEditor::moveUp(int index)
_missionItems->insert(index - 1, new MissionItem(item2, _missionItems)); _missionItems->insert(index - 1, new MissionItem(item2, _missionItems));
_missionItems->insert(index, new MissionItem(item1, _missionItems)); _missionItems->insert(index, new MissionItem(item1, _missionItems));
_reSequence(); _recalcAll();
} }
void MissionEditor::moveDown(int index) void MissionEditor::moveDown(int index)
@ -183,7 +178,7 @@ void MissionEditor::moveDown(int index)
_missionItems->insert(index, new MissionItem(item2, _missionItems)); _missionItems->insert(index, new MissionItem(item2, _missionItems));
_missionItems->insert(index + 1, new MissionItem(item1, _missionItems)); _missionItems->insert(index + 1, new MissionItem(item1, _missionItems));
_reSequence(); _recalcAll();
} }
void MissionEditor::loadMissionFromFile(void) void MissionEditor::loadMissionFromFile(void)
@ -196,6 +191,7 @@ void MissionEditor::loadMissionFromFile(void)
} }
if (_missionItems) { if (_missionItems) {
_deinitAllMissionItems();
_missionItems->deleteLater(); _missionItems->deleteLater();
} }
_missionItems = new QmlObjectListModel(this); _missionItems = new QmlObjectListModel(this);
@ -236,11 +232,7 @@ void MissionEditor::loadMissionFromFile(void)
_missionItems->clear(); _missionItems->clear();
} }
_missionItems->setDirty(false); _initAllMissionItems();
emit canEditChanged(_canEdit);
connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged);
_rebuildWaypointLines();
} }
void MissionEditor::saveMissionToFile(void) void MissionEditor::saveMissionToFile(void)
@ -269,20 +261,131 @@ void MissionEditor::saveMissionToFile(void)
_missionItems->setDirty(false); _missionItems->setDirty(false);
} }
void MissionEditor::_rebuildWaypointLines(void) void MissionEditor::_recalcWaypointLines(void)
{ {
bool firstCoordinateItem = true;
MissionItem* lastCoordinateItem = qobject_cast<MissionItem*>(_missionItems->get(0));
_waypointLines.clear(); _waypointLines.clear();
for (int i=1; i<_missionItems->count(); i++) { for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item1 = qobject_cast<MissionItem*>(_missionItems->get(i-1)); MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
MissionItem* item2 = qobject_cast<MissionItem*>(_missionItems->get(i));
_waypointLines.append(new CoordinateVector(item1->coordinate(), item2->coordinate())); if (item->specifiesCoordinate()) {
if (firstCoordinateItem) {
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
// 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()));
} else {
// First coordiante is not a takeoff command, it does not link backwards to anything
}
firstCoordinateItem = false;
} else {
// Subsequent coordinate items link to last coordinate item
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
}
lastCoordinateItem = item;
}
} }
emit waypointLinesChanged(); emit waypointLinesChanged();
} }
void MissionEditor::_missionListDirtyChanged(bool dirty) // This will update the sequence numbers to be sequential starting from 0
void MissionEditor::_recalcSequence(void)
{
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));
currentParentItem->childItems()->clear();
for (int i=0; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
// Setup ascending sequence numbers
item->setSequenceNumber(i);
}
}
// This will update the child item hierarchy
void MissionEditor::_recalcChildItems(void)
{
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));
currentParentItem->childItems()->clear();
for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
// Set up non-coordinate item child hierarchy
if (item->specifiesCoordinate()) {
item->childItems()->clear();
currentParentItem = item;
} else {
currentParentItem->childItems()->append(item);
}
}
}
void MissionEditor::_recalcAll(void)
{
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
}
/// Initializes a new set of mission items which may have come from the vehicle or have been loaded from a file
void MissionEditor::_initAllMissionItems(void)
{
// Add the home position item to the front
MissionItem* homeItem = new MissionItem(this);
homeItem->setHomePositionSpecialCase(true);
homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
_missionItems->insert(0, homeItem);
for (int i=0; i<_missionItems->count(); i++) {
_initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
}
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
emit missionItemsChanged();
emit canEditChanged(_canEdit);
_missionItems->setDirty(false);
}
void MissionEditor::_deinitAllMissionItems(void)
{
for (int i=0; i<_missionItems->count(); i++) {
_deinitMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
}
}
void MissionEditor::_initMissionItem(MissionItem* item)
{
_missionItems->setDirty(false);
connect(item, &MissionItem::commandChanged, this, &MissionEditor::_itemCommandChanged);
connect(item, &MissionItem::coordinateChanged, this, &MissionEditor::_itemCoordinateChanged);
}
void MissionEditor::_deinitMissionItem(MissionItem* item)
{
disconnect(item, &MissionItem::commandChanged, this, &MissionEditor::_itemCommandChanged);
disconnect(item, &MissionItem::coordinateChanged, this, &MissionEditor::_itemCoordinateChanged);
}
void MissionEditor::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
{
Q_UNUSED(coordinate);
_recalcWaypointLines();
}
void MissionEditor::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command)
{ {
Q_UNUSED(dirty); Q_UNUSED(command);;
_rebuildWaypointLines(); _recalcChildItems();
_recalcWaypointLines();
} }

13
src/MissionEditor/MissionEditor.h

@ -61,11 +61,18 @@ signals:
private slots: private slots:
void _newMissionItemsAvailable(); void _newMissionItemsAvailable();
void _missionListDirtyChanged(bool dirty); void _itemCoordinateChanged(const QGeoCoordinate& coordinate);
void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
private: private:
void _reSequence(void); void _recalcSequence(void);
void _rebuildWaypointLines(void); void _recalcWaypointLines(void);
void _recalcChildItems(void);
void _recalcAll(void);
void _initAllMissionItems(void);
void _deinitAllMissionItems(void);
void _initMissionItem(MissionItem* item);
void _deinitMissionItem(MissionItem* item);
private: private:
QmlObjectListModel* _missionItems; QmlObjectListModel* _missionItems;

112
src/MissionEditor/MissionEditor.qml

@ -32,6 +32,7 @@ import QGroundControl.FlightMap 1.0
import QGroundControl.ScreenTools 1.0 import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0 import QGroundControl.Controls 1.0
import QGroundControl.Palette 1.0 import QGroundControl.Palette 1.0
import QGroundControl.Mavlink 1.0
/// Mission Editor /// Mission Editor
@ -50,7 +51,7 @@ QGCView {
property var _homePositionManager: QGroundControl.homePositionManager property var _homePositionManager: QGroundControl.homePositionManager
property string _homePositionName: _homePositionManager.homePositions.get(0).name property string _homePositionName: _homePositionManager.homePositions.get(0).name
property var _homePositionCoordinate: _homePositionManager.homePositions.get(0).coordinate property var homePositionCoordinate: _homePositionManager.homePositions.get(0).coordinate
QGCPalette { id: _qgcPal; colorGroupEnabled: enabled } QGCPalette { id: _qgcPal; colorGroupEnabled: enabled }
@ -68,6 +69,22 @@ QGCView {
} }
} }
// Home position is mission item 0, so keep them in sync
onHomePositionCoordinateChanged: {
// Changing the coordinate will set the dirty bit, so we save and reset it
var dirtyBit = _missionItems.dirty
_missionItems.get(0).coordinate = homePositionCoordinate
_missionItems.dirty = dirtyBit
}
Component.onCompleted: onHomePositionCoordinateChanged
Connections {
target: controller
onMissionItemsChanged: _missionItems.get(0).coordinate = homePositionCoordinate
}
QGCViewPanel { QGCViewPanel {
id: panel id: panel
anchors.fill: parent anchors.fill: parent
@ -84,8 +101,8 @@ QGCView {
mapName: "MissionEditor" mapName: "MissionEditor"
Component.onCompleted: { Component.onCompleted: {
latitude = _homePositionCoordinate.latitude latitude = homePositionCoordinate.latitude
longitude = _homePositionCoordinate.longitude longitude = homePositionCoordinate.longitude
} }
MouseArea { MouseArea {
@ -97,7 +114,7 @@ QGCView {
coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces)
coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces)
if (_showHomePositionManager) { if (_showHomePositionManager) {
_homePositionCoordinate = coordinate homePositionCoordinate = coordinate
} else if (_addMissionItems) { } else if (_addMissionItems) {
var index = controller.addMissionItem(coordinate) var index = controller.addMissionItem(coordinate)
setCurrentItem(index) setCurrentItem(index)
@ -172,7 +189,7 @@ QGCView {
onClicked: { onClicked: {
centerMapButton.hideDropDown() centerMapButton.hideDropDown()
editorMap.center = QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude) editorMap.center = QtPositioning.coordinate(homePositionCoordinate.latitude, homePositionCoordinate.longitude)
_showHomePositionManager = true _showHomePositionManager = true
} }
} }
@ -201,8 +218,8 @@ QGCView {
centerMapButton.hideDropDown() centerMapButton.hideDropDown()
// Begin with only the home position in the region // Begin with only the home position in the region
var region = QtPositioning.rectangle(QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude), var region = QtPositioning.rectangle(QtPositioning.coordinate(homePositionCoordinate.latitude, _homePositionCoordinate.longitude),
QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude)) QtPositioning.coordinate(homePositionCoordinate.latitude, _homePositionCoordinate.longitude))
// Now expand the region to include all mission items // Now expand the region to include all mission items
for (var i=0; i<_missionItems.count; i++) { for (var i=0; i<_missionItems.count; i++) {
@ -313,63 +330,48 @@ QGCView {
} }
} }
MissionItemIndicator {
label: "H"
isCurrentItem: _showHomePositionManager
coordinate: _homePositionCoordinate
z: 2
onClicked: _showHomePositionManager = true
}
// Add the mission items to the map // Add the mission items to the map
MapItemView { MapItemView {
model: controller.missionItems model: controller.missionItems
delegate: delegate:
MissionItemIndicator { MissionItemIndicator {
label: object.sequenceNumber id: itemIndicator
label: object.sequenceNumber == 0 ? "H" : object.sequenceNumber
isCurrentItem: !_showHomePositionManager && object.isCurrentItem isCurrentItem: !_showHomePositionManager && object.isCurrentItem
coordinate: object.coordinate coordinate: object.coordinate
z: 2 z: 2
visible: object.specifiesCoordinate
onClicked: { onClicked: {
_showHomePositionManager = false _showHomePositionManager = false
setCurrentItem(object.sequenceNumber) setCurrentItem(object.sequenceNumber)
} }
}
}
MapPolyline {
id: homePositionLine
line.width: 3
line.color: "orange"
z: 1
property var homePositionCoordinate: _homePositionCoordinate Row {
anchors.top: parent.top
function update() { anchors.left: parent.right
while (homePositionLine.path.length != 0) {
homePositionLine.removeCoordinate(homePositionLine.path[0])
}
if (_missionItems && _missionItems.count != 0) {
homePositionLine.addCoordinate(homePositionCoordinate)
homePositionLine.addCoordinate(_missionItems.get(0).coordinate)
}
}
onHomePositionCoordinateChanged: update() Repeater {
model: object.childItems
Connections { delegate:
target: controller MissionItemIndexLabel {
label: object.sequenceNumber
isCurrentItem: !_showHomePositionManager && object.isCurrentItem
z: 2
onWaypointLinesChanged: homePositionLine.update() onClicked: {
} _showHomePositionManager = false
setCurrentItem(object.sequenceNumber)
}
Component.onCompleted: homePositionLine.update() }
}
}
}
} }
// Add lines between waypoints // Add lines between waypoints
MapItemView { MapItemView {
model: controller.waypointLines model: controller.waypointLines
@ -417,7 +419,7 @@ QGCView {
// Mission Item Editor // Mission Item Editor
Item { Item {
anchors.fill: parent anchors.fill: parent
visible: !_showHomePositionManager && controller.missionItems.count != 0 visible: !_showHomePositionManager && controller.missionItems.count != 1
ListView { ListView {
id: missionItemSummaryList id: missionItemSummaryList
@ -438,7 +440,7 @@ QGCView {
onRemove: { onRemove: {
var newCurrentItem = object.sequenceNumber - 1 var newCurrentItem = object.sequenceNumber - 1
controller.removeMissionItem(object.sequenceNumber) controller.removeMissionItem(object.sequenceNumber)
if (_missionItems.count) { if (_missionItems.count > 1) {
newCurrentItem = Math.min(_missionItems.count - 1, newCurrentItem) newCurrentItem = Math.min(_missionItems.count - 1, newCurrentItem)
setCurrentItem(newCurrentItem) setCurrentItem(newCurrentItem)
} }
@ -490,9 +492,9 @@ QGCView {
if (currentIndex != -1) { if (currentIndex != -1) {
var homePos = _homePositionManager.homePositions.get(currentIndex) var homePos = _homePositionManager.homePositions.get(currentIndex)
_homePositionName = homePos.name _homePositionName = homePos.name
_homePositionCoordinate = homePos.coordinate homePositionCoordinate = homePos.coordinate
editorMap.latitude = _homePositionCoordinate.latitude editorMap.latitude = homePositionCoordinate.latitude
editorMap.longitude = _homePositionCoordinate.longitude editorMap.longitude = homePositionCoordinate.longitude
} }
} }
} }
@ -551,7 +553,7 @@ QGCView {
id: latitudeField id: latitudeField
anchors.right: parent.right anchors.right: parent.right
width: _editFieldWidth width: _editFieldWidth
text: _homePositionCoordinate.latitude text: homePositionCoordinate.latitude
} }
} }
@ -573,7 +575,7 @@ QGCView {
id: longitudeField id: longitudeField
anchors.right: parent.right anchors.right: parent.right
width: _editFieldWidth width: _editFieldWidth
text: _homePositionCoordinate.longitude text: homePositionCoordinate.longitude
} }
} }
@ -595,7 +597,7 @@ QGCView {
id: altitudeField id: altitudeField
anchors.right: parent.right anchors.right: parent.right
width: _editFieldWidth width: _editFieldWidth
text: _homePositionCoordinate.altitude text: homePositionCoordinate.altitude
} }
} }
@ -611,8 +613,8 @@ QGCView {
text: "Add/Update" text: "Add/Update"
onClicked: { onClicked: {
_homePositionCoordinate = QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text) homePositionCoordinate = QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text)
_homePositionManager.updateHomePosition(nameField.text, _homePositionCoordinate) _homePositionManager.updateHomePosition(nameField.text, homePositionCoordinate)
homePosCombo.currentIndex = homePosCombo.find(nameField.text) homePosCombo.currentIndex = homePosCombo.find(nameField.text)
} }
} }
@ -626,7 +628,7 @@ QGCView {
homePosCombo.currentIndex = 0 homePosCombo.currentIndex = 0
var homePos = _homePositionManager.homePositions.get(0) var homePos = _homePositionManager.homePositions.get(0)
_homePositionName = homePos.name _homePositionName = homePos.name
_homePositionCoordinate = homePos.coordinate homePositionCoordinate = homePos.coordinate
} }
} }
} }
@ -636,7 +638,7 @@ QGCView {
// Help Panel // Help Panel
Item { Item {
anchors.fill: parent anchors.fill: parent
visible: !_showHomePositionManager && controller.missionItems.count == 0 visible: !_showHomePositionManager && controller.missionItems.count == 1
QGCLabel { QGCLabel {
id: helpTitle id: helpTitle

14
src/MissionItem.cc

@ -84,6 +84,7 @@ MissionItem::MissionItem(QObject* parent,
, _reachedTime(0) , _reachedTime(0)
, _yawRadiansFact(NULL) , _yawRadiansFact(NULL)
,_dirty(false) ,_dirty(false)
, _homePositionSpecialCase(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);
@ -219,6 +220,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
_reachedTime = other._reachedTime; _reachedTime = other._reachedTime;
_altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact; _altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact;
_dirty = other._dirty; _dirty = other._dirty;
_homePositionSpecialCase = other._homePositionSpecialCase;
*_latitudeFact = *other._latitudeFact; *_latitudeFact = *other._latitudeFact;
*_longitudeFact = *other._longitudeFact; *_longitudeFact = *other._longitudeFact;
@ -664,9 +666,11 @@ QmlObjectListModel* MissionItem::textFieldFacts(void)
model->append(_latitudeFact); model->append(_latitudeFact);
model->append(_longitudeFact); model->append(_longitudeFact);
model->append(_altitudeFact); model->append(_altitudeFact);
model->append(_yawRadiansFact); if (!_homePositionSpecialCase) {
model->append(_param2Fact); model->append(_yawRadiansFact);
model->append(_param1Fact); model->append(_param2Fact);
model->append(_param1Fact);
}
break; break;
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
model->append(_latitudeFact); model->append(_latitudeFact);
@ -736,7 +740,9 @@ QmlObjectListModel* MissionItem::checkboxFacts(void)
switch ((MAV_CMD)_command) { switch ((MAV_CMD)_command) {
case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_WAYPOINT:
model->append(_altitudeRelativeToHomeFact); if (!_homePositionSpecialCase) {
model->append(_altitudeRelativeToHomeFact);
}
break; break;
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
model->append(_altitudeRelativeToHomeFact); model->append(_altitudeRelativeToHomeFact);

11
src/MissionItem.h

@ -36,6 +36,7 @@
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
#include "Fact.h" #include "Fact.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "QmlObjectListModel.h"
Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) Q_DECLARE_LOGGING_CATEGORY(MissionItemLog)
@ -76,6 +77,7 @@ public:
Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts NOTIFY commandChanged) Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts NOTIFY commandChanged)
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)
// Property accesors // Property accesors
@ -111,6 +113,8 @@ public:
bool dirty(void) { return _dirty; } bool dirty(void) { return _dirty; }
void setDirty(bool dirty); void setDirty(bool dirty);
QmlObjectListModel* childItems(void) { return &_childItems; }
// C++ only methods // C++ only methods
/// Returns true if this item can be edited in the ui /// Returns true if this item can be edited in the ui
@ -184,6 +188,8 @@ public:
void save(QTextStream &saveStream); void save(QTextStream &saveStream);
bool load(QTextStream &loadStream); bool load(QTextStream &loadStream);
void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; }
signals: signals:
void sequenceNumberChanged(int sequenceNumber); void sequenceNumberChanged(int sequenceNumber);
void isCurrentItemChanged(bool isCurrentItem); void isCurrentItemChanged(bool isCurrentItem);
@ -265,6 +271,11 @@ private:
bool _dirty; bool _dirty;
bool _homePositionSpecialCase; ///< true: this item is being used as a ui home position indicator
/// This is used to reference any subsequent mission items which do not specify a coordinate.
QmlObjectListModel _childItems;
static const int _cMavCmd2Name = 9; static const int _cMavCmd2Name = 9;
static const MavCmd2Name_t _rgMavCmd2Name[_cMavCmd2Name]; static const MavCmd2Name_t _rgMavCmd2Name[_cMavCmd2Name];
}; };

25
src/MissionManager/MissionManager.cc

@ -53,14 +53,25 @@ MissionManager::~MissionManager()
} }
void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem)
{ {
_retryCount = 0; _retryCount = 0;
_missionItems.clear(); _missionItems.clear();
for (int i=0; i<missionItems.count(); i++) {
for (int i=skipFirstItem ? 1: 0; i<missionItems.count(); i++) {
_missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i]))); _missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i])));
} }
if (skipFirstItem) {
for (int i=0; i<_missionItems.count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems[i]);
if (item->command() == MavlinkQmlSingleton::MAV_CMD_CONDITION_DELAY) {
item->setParam1((int)item->param1() - 1);
}
}
}
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();
if (inProgress()) { if (inProgress()) {
@ -370,24 +381,24 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
mavlink_msg_mission_ack_decode(&message, &missionAck); mavlink_msg_mission_ack_decode(&message, &missionAck);
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type; qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedRetryAck) { switch (savedRetryAck) {
case AckNone: case AckNone:
// State machine is idle. Vehicle is confused. // State machine is idle. Vehicle is confused.
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << missionAck.type; qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break; break;
case AckMissionCount: case AckMissionCount:
// MISSION_COUNT message expected // MISSION_COUNT message expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << missionAck.type; qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionCount)) { if (!_retrySequence(AckMissionCount)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
} }
break; break;
case AckMissionItem: case AckMissionItem:
// MISSION_ITEM expected // MISSION_ITEM expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << missionAck.type; qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionItem)) { if (!_retrySequence(AckMissionItem)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
} }
@ -405,7 +416,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
} }
} }
} else { } else {
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionRequest)) { if (!_retrySequence(AckMissionRequest)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
} }

4
src/MissionManager/MissionManager.h

@ -62,7 +62,9 @@ public:
void requestMissionItems(void); void requestMissionItems(void);
/// Writes the specified set of mission items to the vehicle /// Writes the specified set of mission items to the vehicle
void writeMissionItems(const QmlObjectListModel& missionItems); /// @oaram missionItems Items to send to vehicle
/// @param skipFirstItem true: Don't send first item
void writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem);
/// Returns a copy of the current set of mission items. Caller is responsible for /// Returns a copy of the current set of mission items. Caller is responsible for
/// freeing returned object. /// freeing returned object.

2
src/MissionManager/MissionManagerTest.cc

@ -158,7 +158,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
} }
// Send the items to the vehicle // Send the items to the vehicle
_missionManager->writeMissionItems(*list); _missionManager->writeMissionItems(*list, false /* skipFirstItem */);
// writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it // writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress()); QVERIFY(_missionManager->inProgress());

18
src/QmlControls/MissionItemEditor.qml

@ -53,7 +53,7 @@ Rectangle {
id: label id: label
anchors.verticalCenter: commandPicker.verticalCenter anchors.verticalCenter: commandPicker.verticalCenter
isCurrentItem: missionItem.isCurrentItem isCurrentItem: missionItem.isCurrentItem
label: missionItem.sequenceNumber label: missionItem.sequenceNumber == 0 ? "H" : missionItem.sequenceNumber
} }
MouseArea { MouseArea {
@ -71,11 +71,27 @@ Rectangle {
anchors.right: parent.right anchors.right: parent.right
currentIndex: missionItem.commandByIndex currentIndex: missionItem.commandByIndex
model: missionItem.commandNames model: missionItem.commandNames
visible: missionItem.sequenceNumber != 0
onActivated: missionItem.commandByIndex = index onActivated: missionItem.commandByIndex = index
} }
Rectangle { Rectangle {
anchors.fill: commandPicker
color: qgcPal.button
visible: missionItem.sequenceNumber == 0
QGCLabel {
id: homeLabel
anchors.leftMargin: ScreenTools.defaultFontPixelWidth
anchors.fill: parent
verticalAlignment: Text.AlignVCenter
text: "Home"
color: qgcPal.buttonText
}
}
Rectangle {
anchors.topMargin: _margin anchors.topMargin: _margin
anchors.top: commandPicker.bottom anchors.top: commandPicker.bottom
anchors.bottom: parent.bottom anchors.bottom: parent.bottom

15
src/QmlControls/QmlObjectListModel.cc

@ -35,6 +35,7 @@ const int QmlObjectListModel::TextRole = Qt::UserRole + 1;
QmlObjectListModel::QmlObjectListModel(QObject* parent) QmlObjectListModel::QmlObjectListModel(QObject* parent)
: QAbstractListModel(parent) : QAbstractListModel(parent)
, _dirty(false) , _dirty(false)
, _skipDirtyFirstItem(false)
{ {
} }
@ -147,16 +148,22 @@ void QmlObjectListModel::clear(void)
} }
} }
void QmlObjectListModel::removeAt(int i) QObject* QmlObjectListModel::removeAt(int i)
{ {
QObject* removedObject = _objectList[i];
// Look for a dirtyChanged signal on the object // Look for a dirtyChanged signal on the object
if (_objectList[i]->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) { if (_objectList[i]->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); if (!_skipDirtyFirstItem || i != 0) {
QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
}
} }
removeRows(i, 1); removeRows(i, 1);
setDirty(true); setDirty(true);
return removedObject;
} }
void QmlObjectListModel::insert(int i, QObject* object) void QmlObjectListModel::insert(int i, QObject* object)
@ -169,7 +176,9 @@ void QmlObjectListModel::insert(int i, QObject* object)
// Look for a dirtyChanged signal on the object // Look for a dirtyChanged signal on the object
if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) { if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); if (!_skipDirtyFirstItem || i != 0) {
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
}
} }
_objectList.insert(i, object); _objectList.insert(i, object);

3
src/QmlControls/QmlObjectListModel.h

@ -51,7 +51,7 @@ public:
void append(QObject* object); void append(QObject* object);
void clear(void); void clear(void);
void removeAt(int i); QObject* removeAt(int i);
void insert(int i, QObject* object); void insert(int i, QObject* object);
QObject* operator[](int i); QObject* operator[](int i);
const QObject* operator[](int i) const; const QObject* operator[](int i) const;
@ -79,6 +79,7 @@ private:
QList<QObject*> _objectList; QList<QObject*> _objectList;
bool _dirty; bool _dirty;
bool _skipDirtyFirstItem;
static const int ObjectRole; static const int ObjectRole;
static const int TextRole; static const int TextRole;

Loading…
Cancel
Save