Skip to content

Q
qgroundcontrol
Commit f9164d3d authored by Don Gagne's avatar Don Gagne
Browse files
Options

Merge pull request #2122 from DonLakeFlyer/WaypointDistance 

Calculate/Display distance between waypoints
parents 907134d9 68072e99
Hide whitespace changes
Inline Side-by-side
Showing with 155 additions and 41 deletions
src/MissionEditor/MissionEditor.qml
View file @ f9164d3d
......@@ -38,7 +38,9 @@ import QGroundControl.Controllers 1.0
/// Mission Editor
QGCView {
viewPanel: panel
id: _root
viewPanel: panel
// zOrder comes from the Loader in MainWindow.qml
z: QGroundControl.zOrderTopMost
......@@ -89,6 +91,15 @@ QGCView {
}
}
function showDistance(missionItem) {
if (missionItem.distance < 0.0) {
waypointDistanceDisplay.visible = false
} else {
waypointDistanceDisplay.distance = missionItem.distance
waypointDistanceDisplay.visible = true
}
}
MissionController {
id: controller
......@@ -184,9 +195,9 @@ QGCView {
y: missionItemIndicator ? (missionItemIndicator.y + missionItemIndicator.anchorPoint.y - (itemDragger.height / 2)) : 100
width: _radius * 2
height: _radius * 2
radius: _radius
border.width: 2
border.color: "white"
//radius: _radius
//border.width: 2
//border.color: "white"
color: "transparent"
visible: false
z: QGroundControl.zOrderMapItems + 1 // Above item icons
......@@ -285,6 +296,7 @@ QGCView {
itemDragger.missionItem.coordinate = coordinate
editorMap.latitude = itemDragger.missionItem.coordinate.latitude
editorMap.longitude = itemDragger.missionItem.coordinate.longitude
_root.showDistance(itemDragger.missionItem)
}
}
}
......@@ -313,19 +325,22 @@ QGCView {
target: object
onIsCurrentItemChanged: {
if (object.isCurrentItem && object.specifiesCoordinate) {
// Setup our drag item
if (object.sequenceNumber != 0) {
itemDragger.visible = true
itemDragger.missionItem = Qt.binding(function() { return object })
itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator })
} else {
itemDragger.clearItem()
}
if (object.isCurrentItem) {
_root.showDistance(object)
if (object.specifiesCoordinate) {
// Setup our drag item
if (object.sequenceNumber != 0) {
itemDragger.visible = true
itemDragger.missionItem = Qt.binding(function() { return object })
itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator })
} else {
itemDragger.clearItem()
}
// Move to the new position
editorMap.latitude = object.coordinate.latitude
editorMap.longitude = object.coordinate.longitude
// Move to the new position
editorMap.latitude = object.coordinate.latitude
editorMap.longitude = object.coordinate.longitude
}
}
}
}
......@@ -854,6 +869,31 @@ QGCView {
z: QGroundControl.zOrderWidgets
checked: _showHelp
}
Rectangle {
id: waypointDistanceDisplay
anchors.margins: margins
anchors.left: parent.left
anchors.bottom: parent.bottom
width: distanceLabel.width + margins
height: distanceLabel.height + margins
radius: ScreenTools.defaultFontPixelWidth
color: qgcPal.window
opacity: 0.80
visible: false
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"
}
}
} // FlightMap
} // Item - split view container
} // QGCViewPanel
......
src/MissionItem.cc
View file @ f9164d3d
......@@ -80,6 +80,7 @@ MissionItem::MissionItem(QObject* parent,
, _autocontinue(autocontinue)
, _isCurrentItem(isCurrentItem)
, _reachedTime(0)
, _distance(0.0)
, _headingDegreesFact(NULL)
, _dirty(false)
, _homePositionSpecialCase(false)
......@@ -189,6 +190,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
_command = other._command;
_autocontinue = other._autocontinue;
_reachedTime = other._reachedTime;
_distance = other._distance;
_altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact;
_dirty = other._dirty;
_homePositionSpecialCase = other._homePositionSpecialCase;
......@@ -231,7 +233,8 @@ void MissionItem::_connectSignals(void)
connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
connect(_headingDegreesFact, &Fact::valueChanged, this, &MissionItem::_headingDegreesFactChanged);
connect(_headingDegreesFact, &Fact::valueChanged, this, &MissionItem::_headingDegreesFactChanged);
connect(_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &MissionItem::_altitudeRelativeToHomeFactChanged);
}
MissionItem::~MissionItem()
......@@ -282,7 +285,6 @@ void MissionItem::setSequenceNumber(int sequenceNumber)
{
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(_sequenceNumber);
emit changed(this);
}
void MissionItem::setX(double x)
......@@ -314,7 +316,6 @@ void MissionItem::setLatitude(double lat)
if (_latitudeFact->value().toDouble() != lat)
{
_latitudeFact->setValue(lat);
emit changed(this);
emit coordinateChanged(coordinate());
}
}
......@@ -324,7 +325,6 @@ void MissionItem::setLongitude(double lon)
if (_longitudeFact->value().toDouble() != lon)
{
_longitudeFact->setValue(lon);
emit changed(this);
emit coordinateChanged(coordinate());
}
}
......@@ -334,7 +334,6 @@ void MissionItem::setAltitude(double altitude)
if (_altitudeFact->value().toDouble() != altitude)
{
_altitudeFact->setValue(altitude);
emit changed(this);
emit valueStringsChanged(valueStrings());
emit coordinateChanged(coordinate());
}
......@@ -376,7 +375,6 @@ void MissionItem::setAction(int /*MAV_CMD*/ action)
setFrame(MAV_FRAME_MISSION);
}
emit changed(this);
emit commandNameChanged(commandName());
emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_command);
emit valueLabelsChanged(valueLabels());
......@@ -398,7 +396,7 @@ void MissionItem::setFrame(int /*MAV_FRAME*/ frame)
if (_frame != frame) {
_altitudeRelativeToHomeFact->setValue(frame == MAV_FRAME_GLOBAL_RELATIVE_ALT);
_frame = frame;
emit changed(this);
emit frameChanged(_frame);
}
}
......@@ -406,7 +404,7 @@ void MissionItem::setAutocontinue(bool autoContinue)
{
if (_autocontinue != autoContinue) {
_autocontinue = autoContinue;
emit changed(this);
emit autoContinueChanged(_autocontinue);
}
}
......@@ -428,7 +426,6 @@ void MissionItem::setParam1(double param)
if (param1() != param)
{
_param1Fact->setValue(param);
emit changed(this);
emit valueStringsChanged(valueStrings());
}
}
......@@ -439,7 +436,6 @@ void MissionItem::setParam2(double param)
{
_param2Fact->setValue(param);
emit valueStringsChanged(valueStrings());
emit changed(this);
}
}
......@@ -473,7 +469,6 @@ void MissionItem::setLoiterOrbitRadius(double radius)
if (loiterOrbitRadius() != radius) {
_loiterOrbitRadiusFact->setValue(radius);
emit valueStringsChanged(valueStrings());
emit changed(this);
}
}
......@@ -813,7 +808,6 @@ void MissionItem::setHeadingDegrees(double headingDegrees)
{
if (_headingDegreesFact->value().toDouble() != headingDegrees) {
_headingDegreesFact->setValue(headingDegrees);
emit changed(this);
emit valueStringsChanged(valueStrings());
emit headingDegreesChanged(headingDegrees);
}
......@@ -911,8 +905,25 @@ void MissionItem::_headingDegreesFactChanged(QVariant value)
emit headingDegreesChanged(value.toDouble());
}
void MissionItem::_altitudeRelativeToHomeFactChanged(QVariant value)
{
// Don't call setFrame, that will cause a signalling loop
int frame = value.toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL;
if (_frame != frame) {
_frame = frame;
emit frameChanged(_frame);
}
}
void MissionItem::setHomePositionValid(bool homePositionValid)
{
_homePositionValid = homePositionValid;
emit homePositionValidChanged(_homePositionValid);
}
void MissionItem::setDistance(double distance)
{
_distance = distance;
emit distanceChanged(_distance);
}
src/MissionItem.h
View file @ f9164d3d
......@@ -91,6 +91,9 @@ public:
/// true: home position should be shown
Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged)
/// Distance to previous waypoint, set by UI controller
Q_PROPERTY(double distance READ distance WRITE setDistance NOTIFY distanceChanged)
// Property accesors
int sequenceNumber(void) const { return _sequenceNumber; }
......@@ -134,6 +137,9 @@ public:
bool homePosition(void) { return _homePositionSpecialCase; }
bool homePositionValid(void) { return _homePositionValid; }
void setHomePositionValid(bool homePositionValid);
double distance(void) { return _distance; }
void setDistance(double distance);
// C++ only methods
......@@ -207,6 +213,8 @@ public:
void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; }
bool relativeAltitude(void) { return _frame == MAV_FRAME_GLOBAL_RELATIVE_ALT; }
static const double defaultPitch;
static const double defaultHeading;
static const double defaultAltitude;
......@@ -221,15 +229,13 @@ signals:
void headingDegreesChanged(double heading);
void dirtyChanged(bool dirty);
void homePositionValidChanged(bool homePostionValid);
/** @brief Announces a change to the waypoint data */
void changed(MissionItem* wp);
void distanceChanged(float distance);
void frameChanged(int frame);
void commandNameChanged(QString type);
void commandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
void valueLabelsChanged(QStringList valueLabels);
void valueStringsChanged(QStringList valueStrings);
bool autoContinueChanged(bool autoContinue);
public:
/** @brief Set the waypoint action */
......@@ -253,14 +259,11 @@ public:
/** @brief Wether this waypoint has been reached yet */
bool isReached () { return (_reachedTime > 0); }
void setChanged() {
emit changed(this);
}
private slots:
void _factValueChanged(QVariant value);
void _coordinateFactChanged(QVariant value);
void _headingDegreesFactChanged(QVariant value);
void _altitudeRelativeToHomeFactChanged(QVariant value);
private:
QString _oneDecimalString(double value);
......@@ -279,6 +282,7 @@ private:
bool _autocontinue;
bool _isCurrentItem;
quint64 _reachedTime;
double _distance;
Fact* _latitudeFact;
Fact* _longitudeFact;
......
src/MissionManager/MissionController.cc
View file @ f9164d3d
......@@ -288,24 +288,72 @@ 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<MissionItem*>(_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);
_waypointLines.clear();
for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_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<MissionItem*>(_missionItems->get(0));
// The first coordinate we hit is a takeoff command so link back to home position if valid
if (homeItem->homePositionValid()) {
_waypointLines.append(new CoordinateVector(qobject_cast<MissionItem*>(_missionItems->get(0))->coordinate(), item->coordinate()));
if (homePositionValid) {
_waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate()));
item->setDistance(_calcDistance(homePositionValid, homeAlt, homeItem, item));
}
} else {
// First coordiante is not a takeoff command, it does not link backwards to anything
......@@ -314,6 +362,7 @@ void MissionController::_recalcWaypointLines(void)
} 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(_calcDistance(homePositionValid, homeAlt, lastCoordinateItem, item));
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
}
lastCoordinateItem = item;
......@@ -407,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)
......@@ -421,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);;
......
src/MissionManager/MissionController.h
View file @ f9164d3d
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment