diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 808991e2d432f13b15a4fff13652b7168b542db7..358237fa511c62c05678117648ef966794091a05 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -94,10 +94,11 @@ QGCView { function showDistance(missionItem) { if (missionItem.distance < 0.0) { - waypointDistanceDisplay.visible = false + waypointValuesDisplay.visible = false } else { - waypointDistanceDisplay.distance = missionItem.distance - waypointDistanceDisplay.visible = true + waypointValuesDisplay.azimuth = missionItem.azimuth + waypointValuesDisplay.distance = missionItem.distance + waypointValuesDisplay.visible = true } } @@ -897,27 +898,39 @@ QGCView { } Rectangle { - id: waypointDistanceDisplay + id: waypointValuesDisplay anchors.margins: margins anchors.left: parent.left anchors.bottom: parent.bottom - width: distanceLabel.width + margins - height: distanceLabel.height + margins + width: distanceLabel.width + (margins * 2) + height: valuesColumn.height + (margins * 2) radius: ScreenTools.defaultFontPixelWidth color: qgcPal.window opacity: 0.80 visible: false + property real azimuth: 0 property real distance: 0 readonly property real margins: ScreenTools.defaultFontPixelWidth - QGCLabel { - id: distanceLabel - anchors.verticalCenter: parent.verticalCenter - anchors.horizontalCenter: parent.horizonalCenter - color: qgcPal.text - text: "Distance: " + Math.round(parent.distance) + " meters" + Column { + id: valuesColumn + anchors.leftMargin: parent.margins + anchors.topMargin: parent.margins + anchors.left: parent.left + anchors.top: parent.top + + QGCLabel { + color: qgcPal.text + text: "Azimuth: " + Math.round(waypointValuesDisplay.azimuth) + } + + QGCLabel { + id: distanceLabel + color: qgcPal.text + text: "Distance: " + Math.round(waypointValuesDisplay.distance) + " meters" + } } } } // FlightMap diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 6c2ef95214f81d8b99061e3e52fd738a0aea0aae..38811c5c39a07642c7327f49c6146962192c799b 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -273,7 +273,7 @@ void MissionController::saveMissionToFile(void) _missionItems->setDirty(false); } -double MissionController::_calcDistance(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2) +void MissionController::_calcPrevWaypointValues(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2, double* azimuth, double* distance) { QGeoCoordinate coord1 = item1->coordinate(); QGeoCoordinate coord2 = item2->coordinate(); @@ -302,9 +302,11 @@ double MissionController::_calcDistance(bool homePositionValid, double homeAlt, qCDebug(MissionControllerLog) << "distanceOk" << distanceOk; if (distanceOk) { - return item1->coordinate().distanceTo(item2->coordinate()); + *distance = item1->coordinate().distanceTo(item2->coordinate()); + *azimuth = item1->coordinate().azimuthTo(item2->coordinate()); } else { - return -1.0; + *azimuth = 0.0; + *distance = -1.0; } } @@ -322,7 +324,8 @@ void MissionController::_recalcWaypointLines(void) // If home position is not valid we can only calculate distances between waypoints which are // both relative altitude. - // No distance for first item + // No values for first item + lastCoordinateItem->setAzimuth(0.0); lastCoordinateItem->setDistance(-1.0); _waypointLines.clear(); @@ -330,24 +333,36 @@ 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 + // Assume the worst + item->setAzimuth(0.0); + item->setDistance(-1.0); 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 if valid if (homePositionValid) { + double azimuth, distance; + _waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate())); - item->setDistance(_calcDistance(homePositionValid, homeAlt, homeItem, item)); + _calcPrevWaypointValues(homePositionValid, homeAlt, homeItem, item, &azimuth, &distance); + qDebug() << azimuth << distance; + item->setAzimuth(azimuth); + item->setDistance(distance); } } else { // First coordiante is not a takeoff command, it does not link backwards to anything } firstCoordinateItem = false; } else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) { + double azimuth, distance; + // Subsequent coordinate items link to last coordinate item. If the last coordinate item // is an invalid home position we skip the line - item->setDistance(_calcDistance(homePositionValid, homeAlt, lastCoordinateItem, item)); + _calcPrevWaypointValues(homePositionValid, homeAlt, lastCoordinateItem, item, &azimuth, &distance); + qDebug() << azimuth << distance; + item->setAzimuth(azimuth); + item->setDistance(distance); _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); } lastCoordinateItem = item; diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 6c4d1e3291706dbc7135aba51b0e72f0878328ce..daeea3f23a08c128dd43bf37c1bb0e5f5e0bb0f8 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -94,7 +94,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); + void _calcPrevWaypointValues(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2, double* azimuth, double* distance); double _findLastAltitude(void); private: diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 8aa33ce24ecf2610475b7276438a821aed56c984..7c5d3ba43aab1d6cb27498ffdb529da6521036b0 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -110,6 +110,7 @@ MissionItem::MissionItem(QObject* parent) , _dirty(false) , _sequenceNumber(0) , _isCurrentItem(false) + , _azimuth(0.0) , _distance(0.0) , _homePositionSpecialCase(false) , _homePositionValid(false) @@ -157,6 +158,7 @@ MissionItem::MissionItem(int sequenceNumber, , _dirty(false) , _sequenceNumber(sequenceNumber) , _isCurrentItem(isCurrentItem) + , _azimuth(0.0) , _distance(0.0) , _homePositionSpecialCase(false) , _homePositionValid(false) @@ -204,6 +206,7 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent) , _dirty(false) , _sequenceNumber(0) , _isCurrentItem(false) + , _azimuth(0.0) , _distance(0.0) , _homePositionSpecialCase(false) , _homePositionValid(false) @@ -241,6 +244,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other) setSequenceNumber(other._sequenceNumber); setAutoContinue(other.autoContinue()); setIsCurrentItem(other._isCurrentItem); + setAzimuth(other._distance); setDistance(other._distance); setHomePositionSpecialCase(other._homePositionSpecialCase); setHomePositionValid(other._homePositionValid); @@ -839,6 +843,12 @@ void MissionItem::setDistance(double distance) emit distanceChanged(_distance); } +void MissionItem::setAzimuth(double azimuth) +{ + _azimuth = azimuth; + emit azimuthChanged(_azimuth); +} + void MissionItem::_sendCoordinateChanged(void) { emit coordinateChanged(coordinate()); diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h index 6459c49ef133cbf9416ff5594045989d84d53d8f..51fcfa8824b3f8f94be527a8ab5ffd221dc05f1f 100644 --- a/src/MissionManager/MissionItem.h +++ b/src/MissionManager/MissionItem.h @@ -69,6 +69,7 @@ public: const MissionItem& operator=(const MissionItem& other); + Q_PROPERTY(double azimuth READ azimuth WRITE setAzimuth NOTIFY azimuthChanged) ///< Azimuth to previous waypoint Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) Q_PROPERTY(QString commandDescription READ commandDescription NOTIFY commandChanged) Q_PROPERTY(QString commandName READ commandName NOTIFY commandChanged) @@ -95,6 +96,7 @@ public: // Property accesors + double azimuth (void) const { return _azimuth; } MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_commandFact.cookedValue().toInt(); }; QString commandDescription (void) const; QString commandName (void) const; @@ -132,6 +134,7 @@ public: void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; } void setDistance(double distance); + void setAzimuth(double azimuth); // C++ only methods @@ -174,10 +177,11 @@ public slots: void setDefaultsForCommand(void); signals: + void azimuthChanged (double azimuth); void commandChanged (MavlinkQmlSingleton::Qml_MAV_CMD command); void coordinateChanged (const QGeoCoordinate& coordinate); void dirtyChanged (bool dirty); - void distanceChanged (float distance); + void distanceChanged (double distance); void frameChanged (int frame); void friendlyEditAllowedChanged (bool friendlyEditAllowed); void headingDegreesChanged (double heading); @@ -232,6 +236,7 @@ private: bool _dirty; int _sequenceNumber; bool _isCurrentItem; + double _azimuth; ///< Azimuth to previous waypoint double _distance; ///< Distance to previous waypoint bool _homePositionSpecialCase; ///< true: This item is being used as a ui home position indicator bool _homePositionValid; ///< true: Home psition should be displayed