From 68072e99415cc768f606938cf5eedab96e7a78d8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 27 Oct 2015 10:50:19 -0700 Subject: [PATCH] Correct for relativeAlt, !relativeAlt, homePositionValid mix --- src/MissionManager/MissionController.cc | 69 ++++++++++++++++++++----- src/MissionManager/MissionController.h | 2 + 2 files changed, 59 insertions(+), 12 deletions(-) diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index ab91693d17..35f60c92bd 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -288,10 +288,54 @@ void MissionController::saveMissionToFile(void) _missionItems->setDirty(false); } +double MissionController::_calcDistance(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2) +{ + QGeoCoordinate coord1 = item1->coordinate(); + QGeoCoordinate coord2 = item2->coordinate(); + bool distanceOk = false; + + // Convert to fixed altitudes + + qCDebug(MissionControllerLog) << homePositionValid << homeAlt + << item1->relativeAltitude() << item1->coordinate().altitude() + << item2->relativeAltitude() << item2->coordinate().altitude(); + + if (homePositionValid) { + distanceOk = true; + if (item1->relativeAltitude()) { + coord1.setAltitude(homeAlt + coord1.altitude()); + } + if (item2->relativeAltitude()) { + coord2.setAltitude(homeAlt + coord2.altitude()); + } + } else { + if (item1->relativeAltitude() && item2->relativeAltitude()) { + distanceOk = true; + } + } + + qCDebug(MissionControllerLog) << "distanceOk" << distanceOk; + + if (distanceOk) { + return item1->coordinate().distanceTo(item2->coordinate()); + } else { + return -1.0; + } +} + void MissionController::_recalcWaypointLines(void) { MissionItem* lastCoordinateItem = qobject_cast(_missionItems->get(0)); + MissionItem* homeItem = lastCoordinateItem; bool firstCoordinateItem = true; + bool homePositionValid = homeItem->homePositionValid(); + double homeAlt = homeItem->coordinate().altitude(); + + qCDebug(MissionControllerLog) << "_recalcWaypointLines"; + + // If home position is valid we can calculate distances between all waypoints. + // If home position is not valid we can only calculate distances between waypoints which are + // both relative altitude. // No distance for first item lastCoordinateItem->setDistance(-1.0); @@ -301,34 +345,27 @@ void MissionController::_recalcWaypointLines(void) for (int i=1; i<_missionItems->count(); i++) { MissionItem* item = qobject_cast(_missionItems->get(i)); + item->setDistance(-1.0); // Assume the worst + if (item->specifiesCoordinate()) { if (firstCoordinateItem) { if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { - MissionItem* homeItem = qobject_cast(_missionItems->get(0)); - // The first coordinate we hit is a takeoff command so link back to home position if valid - if (homeItem->homePositionValid()) { - MissionItem* homeItem = qobject_cast(_missionItems->get(0)); - - item->setDistance(homeItem->coordinate().distanceTo(item->coordinate())); + if (homePositionValid) { _waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate())); - } else { - item->setDistance(-1.0); + item->setDistance(_calcDistance(homePositionValid, homeAlt, homeItem, item)); } } else { // First coordiante is not a takeoff command, it does not link backwards to anything - item->setDistance(-1.0); } firstCoordinateItem = false; } else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) { // Subsequent coordinate items link to last coordinate item. If the last coordinate item // is an invalid home position we skip the line - item->setDistance(lastCoordinateItem->coordinate().distanceTo(item->coordinate())); + item->setDistance(_calcDistance(homePositionValid, homeAlt, lastCoordinateItem, item)); _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); } lastCoordinateItem = item; - } else { - item->setDistance(-1.0); } } @@ -419,12 +456,14 @@ void MissionController::_initMissionItem(MissionItem* item) connect(item, &MissionItem::commandChanged, this, &MissionController::_itemCommandChanged); connect(item, &MissionItem::coordinateChanged, this, &MissionController::_itemCoordinateChanged); + connect(item, &MissionItem::frameChanged, this, &MissionController::_itemFrameChanged); } void MissionController::_deinitMissionItem(MissionItem* item) { disconnect(item, &MissionItem::commandChanged, this, &MissionController::_itemCommandChanged); disconnect(item, &MissionItem::coordinateChanged, this, &MissionController::_itemCoordinateChanged); + disconnect(item, &MissionItem::frameChanged, this, &MissionController::_itemFrameChanged); } void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate) @@ -433,6 +472,12 @@ void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate) _recalcWaypointLines(); } +void MissionController::_itemFrameChanged(int frame) +{ + Q_UNUSED(frame); + _recalcWaypointLines(); +} + void MissionController::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command) { Q_UNUSED(command);; diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 2913268ded..939d5fa80a 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -77,6 +77,7 @@ signals: private slots: void _newMissionItemsAvailableFromVehicle(); void _itemCoordinateChanged(const QGeoCoordinate& coordinate); + void _itemFrameChanged(int frame); void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command); void _activeVehicleChanged(Vehicle* activeVehicle); void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable); @@ -96,6 +97,7 @@ private: void _autoSyncSend(void); void _setupMissionItems(bool loadFromVehicle, bool forceLoad); void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle); + double _calcDistance(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2); private: bool _editMode; -- GitLab