zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Calc/Display azimuth to last waypoint

QGC4.4
Don Gagne 10 years ago
parent
73b185448c
commit
ff7567c388
5 changed files with 64 additions and 21 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 37
      src/MissionEditor/MissionEditor.qml
  2. 29
      src/MissionManager/MissionController.cc
  3. 2
      src/MissionManager/MissionController.h
  4. 10
      src/MissionManager/MissionItem.cc
  5. 7
      src/MissionManager/MissionItem.h

37
src/MissionEditor/MissionEditor.qml
Unescape View File

@ -94,10 +94,11 @@ QGCView { @@ -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 { @@ -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

29
src/MissionManager/MissionController.cc
Unescape View File

@ -273,7 +273,7 @@ void MissionController::saveMissionToFile(void) @@ -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, @@ -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) @@ -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) @@ -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;

2
src/MissionManager/MissionController.h
Unescape View File

@ -94,7 +94,7 @@ private: @@ -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:

10
src/MissionManager/MissionItem.cc
Unescape View File

@ -110,6 +110,7 @@ MissionItem::MissionItem(QObject* parent) @@ -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, @@ -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) @@ -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) @@ -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) @@ -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());

7
src/MissionManager/MissionItem.h
Unescape View File

@ -69,6 +69,7 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save