|
|
@ -157,10 +157,26 @@ void MissionController::sendMissionItems(void) |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int MissionController::_nextSequenceNumber(void) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
if (_visualItems->count() == 0) { |
|
|
|
|
|
|
|
qWarning() << "Internal error: Empty visual item list"; |
|
|
|
|
|
|
|
return 0; |
|
|
|
|
|
|
|
} else { |
|
|
|
|
|
|
|
VisualMissionItem* lastItem = qobject_cast<VisualMissionItem*>(_visualItems->get(_visualItems->count() - 1)); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (lastItem->isSimpleItem()) { |
|
|
|
|
|
|
|
return lastItem->sequenceNumber() + 1; |
|
|
|
|
|
|
|
} else { |
|
|
|
|
|
|
|
return qobject_cast<ComplexMissionItem*>(lastItem)->lastSequenceNumber() + 1; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) |
|
|
|
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) |
|
|
|
{ |
|
|
|
{ |
|
|
|
SimpleMissionItem * newItem = new SimpleMissionItem(_activeVehicle, this); |
|
|
|
SimpleMissionItem * newItem = new SimpleMissionItem(_activeVehicle, this); |
|
|
|
newItem->setSequenceNumber(_visualItems->count()); |
|
|
|
newItem->setSequenceNumber(_nextSequenceNumber()); |
|
|
|
newItem->setCoordinate(coordinate); |
|
|
|
newItem->setCoordinate(coordinate); |
|
|
|
newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); |
|
|
|
newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); |
|
|
|
_initVisualItem(newItem); |
|
|
|
_initVisualItem(newItem); |
|
|
@ -188,7 +204,7 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) |
|
|
|
int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i) |
|
|
|
int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i) |
|
|
|
{ |
|
|
|
{ |
|
|
|
ComplexMissionItem* newItem = new ComplexMissionItem(_activeVehicle, this); |
|
|
|
ComplexMissionItem* newItem = new ComplexMissionItem(_activeVehicle, this); |
|
|
|
newItem->setSequenceNumber(_visualItems->count()); |
|
|
|
newItem->setSequenceNumber(_nextSequenceNumber()); |
|
|
|
newItem->setCoordinate(coordinate); |
|
|
|
newItem->setCoordinate(coordinate); |
|
|
|
_initVisualItem(newItem); |
|
|
|
_initVisualItem(newItem); |
|
|
|
|
|
|
|
|
|
|
@ -240,7 +256,7 @@ void MissionController::removeAllMissionItems(void) |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* missionItems, QString& errorString) |
|
|
|
bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString) |
|
|
|
{ |
|
|
|
{ |
|
|
|
QJsonParseError jsonParseError; |
|
|
|
QJsonParseError jsonParseError; |
|
|
|
QJsonDocument jsonDoc(QJsonDocument::fromJson(bytes, &jsonParseError)); |
|
|
|
QJsonDocument jsonDoc(QJsonDocument::fromJson(bytes, &jsonParseError)); |
|
|
@ -274,52 +290,81 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// Simple items
|
|
|
|
// Read complex items
|
|
|
|
if (json.contains(jsonSimpleItemsKey)) { |
|
|
|
QJsonArray complexArray(json[_jsonComplexItemsKey].toArray()); |
|
|
|
QJsonArray itemArray(json[jsonSimpleItemsKey].toArray()); |
|
|
|
qCDebug(MissionControllerLog) << "Json load: complex item count" << complexArray.count(); |
|
|
|
foreach (const QJsonValue& itemValue, itemArray) { |
|
|
|
for (int i=0; i<complexArray.count(); i++) { |
|
|
|
|
|
|
|
const QJsonValue& itemValue = complexArray[i]; |
|
|
|
|
|
|
|
|
|
|
|
if (!itemValue.isObject()) { |
|
|
|
if (!itemValue.isObject()) { |
|
|
|
errorString = QStringLiteral("Mission item is not an object"); |
|
|
|
errorString = QStringLiteral("Mission item is not an object"); |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this); |
|
|
|
ComplexMissionItem* item = new ComplexMissionItem(_activeVehicle, this); |
|
|
|
if (item->load(itemValue.toObject(), errorString)) { |
|
|
|
if (item->load(itemValue.toObject(), errorString)) { |
|
|
|
missionItems->append(item); |
|
|
|
qCDebug(MissionControllerLog) << "Json load: complex item start:stop" << item->sequenceNumber() << item->lastSequenceNumber(); |
|
|
|
|
|
|
|
complexItems->append(item); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Read simple items, interspersing complex items into the full list
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int nextSimpleItemIndex= 0; |
|
|
|
|
|
|
|
int nextComplexItemIndex= 0; |
|
|
|
|
|
|
|
int nextSequenceNumber = 1; // Start with 1 since home is in 0
|
|
|
|
|
|
|
|
QJsonArray itemArray(json[jsonSimpleItemsKey].toArray()); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << complexItems->count(); |
|
|
|
|
|
|
|
do { |
|
|
|
|
|
|
|
qCDebug(MissionControllerLog) << "Json load: simple item loop nextSimpleItemIndex:nextComplexItemIndex:nextSequenceNumber" << nextSimpleItemIndex << nextComplexItemIndex << nextSequenceNumber; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// If there is a complex item that should be next in sequence add it in
|
|
|
|
|
|
|
|
if (nextComplexItemIndex < complexItems->count()) { |
|
|
|
|
|
|
|
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(complexItems->get(nextComplexItemIndex)); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (complexItem->sequenceNumber() == nextSequenceNumber) { |
|
|
|
|
|
|
|
qCDebug(MissionControllerLog) << "Json load: injecting complex item expectedSequence:actualSequence:" << nextSequenceNumber << complexItem->sequenceNumber(); |
|
|
|
|
|
|
|
visualItems->append(complexItem); |
|
|
|
|
|
|
|
nextSequenceNumber = complexItem->lastSequenceNumber() + 1; |
|
|
|
|
|
|
|
nextComplexItemIndex++; |
|
|
|
|
|
|
|
continue; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// Complex items
|
|
|
|
// Add the next available simple item
|
|
|
|
if (json.contains(_jsonComplexItemsKey)) { |
|
|
|
if (nextSimpleItemIndex < itemArray.count()) { |
|
|
|
QJsonArray itemArray(json[_jsonComplexItemsKey].toArray()); |
|
|
|
const QJsonValue& itemValue = itemArray[nextSimpleItemIndex++]; |
|
|
|
foreach (const QJsonValue& itemValue, itemArray) { |
|
|
|
|
|
|
|
if (!itemValue.isObject()) { |
|
|
|
if (!itemValue.isObject()) { |
|
|
|
errorString = QStringLiteral("Mission item is not an object"); |
|
|
|
errorString = QStringLiteral("Mission item is not an object"); |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
ComplexMissionItem* item = new ComplexMissionItem(_activeVehicle, this); |
|
|
|
SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this); |
|
|
|
if (item->load(itemValue.toObject(), errorString)) { |
|
|
|
if (item->load(itemValue.toObject(), errorString)) { |
|
|
|
missionItems->append(item); |
|
|
|
qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber(); |
|
|
|
|
|
|
|
visualItems->append(item); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
nextSequenceNumber++; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} while (nextSimpleItemIndex < itemArray.count() | nextComplexItemIndex < complexItems->count()); |
|
|
|
|
|
|
|
|
|
|
|
if (json.contains(_jsonPlannedHomePositionKey)) { |
|
|
|
if (json.contains(_jsonPlannedHomePositionKey)) { |
|
|
|
SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this); |
|
|
|
SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this); |
|
|
|
|
|
|
|
|
|
|
|
if (item->load(json[_jsonPlannedHomePositionKey].toObject(), errorString)) { |
|
|
|
if (item->load(json[_jsonPlannedHomePositionKey].toObject(), errorString)) { |
|
|
|
missionItems->insert(0, item); |
|
|
|
visualItems->insert(0, item); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
_addPlannedHomePosition(missionItems, true /* addToCenter */); |
|
|
|
_addPlannedHomePosition(visualItems, true /* addToCenter */); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
return true; |
|
|
|
return true; |
|
|
@ -383,7 +428,8 @@ void MissionController::_loadMissionFromFile(const QString& filename) |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
QmlObjectListModel* newMissionItems = new QmlObjectListModel(this); |
|
|
|
QmlObjectListModel* newVisualItems = new QmlObjectListModel(this); |
|
|
|
|
|
|
|
QmlObjectListModel* newComplexItems = new QmlObjectListModel(this); |
|
|
|
|
|
|
|
|
|
|
|
QFile file(filename); |
|
|
|
QFile file(filename); |
|
|
|
|
|
|
|
|
|
|
@ -396,14 +442,22 @@ void MissionController::_loadMissionFromFile(const QString& filename) |
|
|
|
QString firstLine = stream.readLine(); |
|
|
|
QString firstLine = stream.readLine(); |
|
|
|
if (firstLine.contains(QRegExp("QGC.*WPL"))) { |
|
|
|
if (firstLine.contains(QRegExp("QGC.*WPL"))) { |
|
|
|
stream.seek(0); |
|
|
|
stream.seek(0); |
|
|
|
_loadTextMissionFile(stream, newMissionItems, errorString); |
|
|
|
_loadTextMissionFile(stream, newVisualItems, errorString); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
_loadJsonMissionFile(bytes, newMissionItems, errorString); |
|
|
|
_loadJsonMissionFile(bytes, newVisualItems, newComplexItems, errorString); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (!errorString.isEmpty()) { |
|
|
|
if (!errorString.isEmpty()) { |
|
|
|
delete newMissionItems; |
|
|
|
for (int i=0; i<newVisualItems->count(); i++) { |
|
|
|
|
|
|
|
newVisualItems->get(i)->deleteLater(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
for (int i=0; i<newComplexItems->count(); i++) { |
|
|
|
|
|
|
|
newComplexItems->get(i)->deleteLater(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
delete newVisualItems; |
|
|
|
|
|
|
|
delete newComplexItems; |
|
|
|
|
|
|
|
|
|
|
|
qgcApp()->showMessage(errorString); |
|
|
|
qgcApp()->showMessage(errorString); |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
@ -412,7 +466,13 @@ void MissionController::_loadMissionFromFile(const QString& filename) |
|
|
|
_deinitAllVisualItems(); |
|
|
|
_deinitAllVisualItems(); |
|
|
|
_visualItems->deleteListAndContents(); |
|
|
|
_visualItems->deleteListAndContents(); |
|
|
|
} |
|
|
|
} |
|
|
|
_visualItems = newMissionItems; |
|
|
|
if (_complexItems) { |
|
|
|
|
|
|
|
_complexItems->deleteLater(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
_visualItems = newVisualItems; |
|
|
|
|
|
|
|
_complexItems = newComplexItems; |
|
|
|
|
|
|
|
|
|
|
|
if (_visualItems->count() == 0) { |
|
|
|
if (_visualItems->count() == 0) { |
|
|
|
_addPlannedHomePosition(_visualItems, true /* addToCenter */); |
|
|
|
_addPlannedHomePosition(_visualItems, true /* addToCenter */); |
|
|
|
} |
|
|
|
} |
|
|
@ -647,7 +707,7 @@ void MissionController::_recalcWaypointLines(void) |
|
|
|
item->setAltDifference(altDifference); |
|
|
|
item->setAltDifference(altDifference); |
|
|
|
item->setAzimuth(azimuth); |
|
|
|
item->setAzimuth(azimuth); |
|
|
|
item->setDistance(distance); |
|
|
|
item->setDistance(distance); |
|
|
|
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); |
|
|
|
_waypointLines.append(new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate())); |
|
|
|
} |
|
|
|
} |
|
|
|
lastCoordinateItem = item; |
|
|
|
lastCoordinateItem = item; |
|
|
|
} |
|
|
|
} |
|
|
@ -689,7 +749,7 @@ void MissionController::_recalcSequence(void) |
|
|
|
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item); |
|
|
|
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item); |
|
|
|
|
|
|
|
|
|
|
|
if (complexItem) { |
|
|
|
if (complexItem) { |
|
|
|
sequenceNumber = complexItem->nextSequenceNumber(); |
|
|
|
sequenceNumber = complexItem->lastSequenceNumber() + 1; |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
qWarning() << "isSimpleItem == false, yet not ComplexMissionItem"; |
|
|
|
qWarning() << "isSimpleItem == false, yet not ComplexMissionItem"; |
|
|
|
} |
|
|
|
} |
|
|
@ -748,8 +808,6 @@ void MissionController::_initAllVisualItems(void) |
|
|
|
homeItem->setShowHomePosition(true); |
|
|
|
homeItem->setShowHomePosition(true); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
qDebug() << "home item" << homeItem->coordinate(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
QmlObjectListModel* newComplexItems = new QmlObjectListModel(this); |
|
|
|
QmlObjectListModel* newComplexItems = new QmlObjectListModel(this); |
|
|
|
for (int i=0; i<_visualItems->count(); i++) { |
|
|
|
for (int i=0; i<_visualItems->count(); i++) { |
|
|
|
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i)); |
|
|
|
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i)); |
|
|
@ -796,6 +854,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) |
|
|
|
_visualItems->setDirty(false); |
|
|
|
_visualItems->setDirty(false); |
|
|
|
|
|
|
|
|
|
|
|
connect(visualItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_itemCoordinateChanged); |
|
|
|
connect(visualItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_itemCoordinateChanged); |
|
|
|
|
|
|
|
connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged, this, &MissionController::_recalcWaypointLines); |
|
|
|
connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); |
|
|
|
connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); |
|
|
|
connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); |
|
|
|
connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); |
|
|
|
|
|
|
|
|
|
|
@ -921,7 +980,6 @@ void MissionController::_dirtyChanged(bool dirty) |
|
|
|
|
|
|
|
|
|
|
|
void MissionController::_autoSyncSend(void) |
|
|
|
void MissionController::_autoSyncSend(void) |
|
|
|
{ |
|
|
|
{ |
|
|
|
qDebug() << "Auto-syncing with vehicle"; |
|
|
|
|
|
|
|
_queuedSend = false; |
|
|
|
_queuedSend = false; |
|
|
|
if (_visualItems) { |
|
|
|
if (_visualItems) { |
|
|
|
sendMissionItems(); |
|
|
|
sendMissionItems(); |
|
|
|