Skip to content

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

Calc/Display azimuth to last waypoint

parent 73b18544
Hide whitespace changes
Inline Side-by-side
Showing with 64 additions and 21 deletions
src/MissionEditor/MissionEditor.qml
View file @ ff7567c3
......@@ -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
......
src/MissionManager/MissionController.cc
View file @ ff7567c3
......@@ -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<MissionItem*>(_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;
......
src/MissionManager/MissionController.h
View file @ ff7567c3
......@@ -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:
......
src/MissionManager/MissionItem.cc
View file @ ff7567c3
......@@ -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());
......
src/MissionManager/MissionItem.h
View file @ ff7567c3
......@@ -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
......
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