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
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 31
      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

31
src/MissionEditor/MissionEditor.qml
Unescape View File

@ -94,10 +94,11 @@ QGCView {
function showDistance(missionItem) { function showDistance(missionItem) {
if (missionItem.distance < 0.0) { if (missionItem.distance < 0.0) {
waypointDistanceDisplay.visible = false waypointValuesDisplay.visible = false
} else { } else {
waypointDistanceDisplay.distance = missionItem.distance waypointValuesDisplay.azimuth = missionItem.azimuth
waypointDistanceDisplay.visible = true waypointValuesDisplay.distance = missionItem.distance
waypointValuesDisplay.visible = true
} }
} }
@ -897,27 +898,39 @@ QGCView {
} }
Rectangle { Rectangle {
id: waypointDistanceDisplay id: waypointValuesDisplay
anchors.margins: margins anchors.margins: margins
anchors.left: parent.left anchors.left: parent.left
anchors.bottom: parent.bottom anchors.bottom: parent.bottom
width: distanceLabel.width + margins width: distanceLabel.width + (margins * 2)
height: distanceLabel.height + margins height: valuesColumn.height + (margins * 2)
radius: ScreenTools.defaultFontPixelWidth radius: ScreenTools.defaultFontPixelWidth
color: qgcPal.window color: qgcPal.window
opacity: 0.80 opacity: 0.80
visible: false visible: false
property real azimuth: 0
property real distance: 0 property real distance: 0
readonly property real margins: ScreenTools.defaultFontPixelWidth readonly property real margins: ScreenTools.defaultFontPixelWidth
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 { QGCLabel {
id: distanceLabel id: distanceLabel
anchors.verticalCenter: parent.verticalCenter
anchors.horizontalCenter: parent.horizonalCenter
color: qgcPal.text color: qgcPal.text
text: "Distance: " + Math.round(parent.distance) + " meters" text: "Distance: " + Math.round(waypointValuesDisplay.distance) + " meters"
}
} }
} }
} // FlightMap } // FlightMap

29
src/MissionManager/MissionController.cc
Unescape View File

@ -273,7 +273,7 @@ void MissionController::saveMissionToFile(void)
_missionItems->setDirty(false); _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 coord1 = item1->coordinate();
QGeoCoordinate coord2 = item2->coordinate(); QGeoCoordinate coord2 = item2->coordinate();
@ -302,9 +302,11 @@ double MissionController::_calcDistance(bool homePositionValid, double homeAlt,
qCDebug(MissionControllerLog) << "distanceOk" << distanceOk; qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;
if (distanceOk) { if (distanceOk) {
return item1->coordinate().distanceTo(item2->coordinate()); *distance = item1->coordinate().distanceTo(item2->coordinate());
*azimuth = item1->coordinate().azimuthTo(item2->coordinate());
} else { } 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 // If home position is not valid we can only calculate distances between waypoints which are
// both relative altitude. // both relative altitude.
// No distance for first item // No values for first item
lastCoordinateItem->setAzimuth(0.0);
lastCoordinateItem->setDistance(-1.0); lastCoordinateItem->setDistance(-1.0);
_waypointLines.clear(); _waypointLines.clear();
@ -330,24 +333,36 @@ void MissionController::_recalcWaypointLines(void)
for (int i=1; i<_missionItems->count(); i++) { for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(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 (item->specifiesCoordinate()) {
if (firstCoordinateItem) { if (firstCoordinateItem) {
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { 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 // The first coordinate we hit is a takeoff command so link back to home position if valid
if (homePositionValid) { if (homePositionValid) {
double azimuth, distance;
_waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate())); _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 { } else {
// First coordiante is not a takeoff command, it does not link backwards to anything // First coordiante is not a takeoff command, it does not link backwards to anything
} }
firstCoordinateItem = false; firstCoordinateItem = false;
} else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) { } else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) {
double azimuth, distance;
// Subsequent coordinate items link to last coordinate item. If the last coordinate item // Subsequent coordinate items link to last coordinate item. If the last coordinate item
// is an invalid home position we skip the line // 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())); _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
} }
lastCoordinateItem = item; lastCoordinateItem = item;

2
src/MissionManager/MissionController.h
Unescape View File

@ -94,7 +94,7 @@ private:
void _autoSyncSend(void); void _autoSyncSend(void);
void _setupMissionItems(bool loadFromVehicle, bool forceLoad); void _setupMissionItems(bool loadFromVehicle, bool forceLoad);
void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle); 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); double _findLastAltitude(void);
private: private:

10
src/MissionManager/MissionItem.cc
Unescape View File

@ -110,6 +110,7 @@ MissionItem::MissionItem(QObject* parent)
, _dirty(false) , _dirty(false)
, _sequenceNumber(0) , _sequenceNumber(0)
, _isCurrentItem(false) , _isCurrentItem(false)
, _azimuth(0.0)
, _distance(0.0) , _distance(0.0)
, _homePositionSpecialCase(false) , _homePositionSpecialCase(false)
, _homePositionValid(false) , _homePositionValid(false)
@ -157,6 +158,7 @@ MissionItem::MissionItem(int sequenceNumber,
, _dirty(false) , _dirty(false)
, _sequenceNumber(sequenceNumber) , _sequenceNumber(sequenceNumber)
, _isCurrentItem(isCurrentItem) , _isCurrentItem(isCurrentItem)
, _azimuth(0.0)
, _distance(0.0) , _distance(0.0)
, _homePositionSpecialCase(false) , _homePositionSpecialCase(false)
, _homePositionValid(false) , _homePositionValid(false)
@ -204,6 +206,7 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent)
, _dirty(false) , _dirty(false)
, _sequenceNumber(0) , _sequenceNumber(0)
, _isCurrentItem(false) , _isCurrentItem(false)
, _azimuth(0.0)
, _distance(0.0) , _distance(0.0)
, _homePositionSpecialCase(false) , _homePositionSpecialCase(false)
, _homePositionValid(false) , _homePositionValid(false)
@ -241,6 +244,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
setSequenceNumber(other._sequenceNumber); setSequenceNumber(other._sequenceNumber);
setAutoContinue(other.autoContinue()); setAutoContinue(other.autoContinue());
setIsCurrentItem(other._isCurrentItem); setIsCurrentItem(other._isCurrentItem);
setAzimuth(other._distance);
setDistance(other._distance); setDistance(other._distance);
setHomePositionSpecialCase(other._homePositionSpecialCase); setHomePositionSpecialCase(other._homePositionSpecialCase);
setHomePositionValid(other._homePositionValid); setHomePositionValid(other._homePositionValid);
@ -839,6 +843,12 @@ void MissionItem::setDistance(double distance)
emit distanceChanged(_distance); emit distanceChanged(_distance);
} }
void MissionItem::setAzimuth(double azimuth)
{
_azimuth = azimuth;
emit azimuthChanged(_azimuth);
}
void MissionItem::_sendCoordinateChanged(void) void MissionItem::_sendCoordinateChanged(void)
{ {
emit coordinateChanged(coordinate()); emit coordinateChanged(coordinate());

7
src/MissionManager/MissionItem.h
Unescape View File

@ -69,6 +69,7 @@ public:
const MissionItem& operator=(const MissionItem& other); 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(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged)
Q_PROPERTY(QString commandDescription READ commandDescription NOTIFY commandChanged) Q_PROPERTY(QString commandDescription READ commandDescription NOTIFY commandChanged)
Q_PROPERTY(QString commandName READ commandName NOTIFY commandChanged) Q_PROPERTY(QString commandName READ commandName NOTIFY commandChanged)
@ -95,6 +96,7 @@ public:
// Property accesors // Property accesors
double azimuth (void) const { return _azimuth; }
MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_commandFact.cookedValue().toInt(); }; MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_commandFact.cookedValue().toInt(); };
QString commandDescription (void) const; QString commandDescription (void) const;
QString commandName (void) const; QString commandName (void) const;
@ -132,6 +134,7 @@ public:
void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; } void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; }
void setDistance(double distance); void setDistance(double distance);
void setAzimuth(double azimuth);
// C++ only methods // C++ only methods
@ -174,10 +177,11 @@ public slots:
void setDefaultsForCommand(void); void setDefaultsForCommand(void);
signals: signals:
void azimuthChanged (double azimuth);
void commandChanged (MavlinkQmlSingleton::Qml_MAV_CMD command); void commandChanged (MavlinkQmlSingleton::Qml_MAV_CMD command);
void coordinateChanged (const QGeoCoordinate& coordinate); void coordinateChanged (const QGeoCoordinate& coordinate);
void dirtyChanged (bool dirty); void dirtyChanged (bool dirty);
void distanceChanged (float distance); void distanceChanged (double distance);
void frameChanged (int frame); void frameChanged (int frame);
void friendlyEditAllowedChanged (bool friendlyEditAllowed); void friendlyEditAllowedChanged (bool friendlyEditAllowed);
void headingDegreesChanged (double heading); void headingDegreesChanged (double heading);
@ -232,6 +236,7 @@ private:
bool _dirty; bool _dirty;
int _sequenceNumber; int _sequenceNumber;
bool _isCurrentItem; bool _isCurrentItem;
double _azimuth; ///< Azimuth to previous waypoint
double _distance; ///< Distance to previous waypoint double _distance; ///< Distance to previous waypoint
bool _homePositionSpecialCase; ///< true: This item is being used as a ui home position indicator bool _homePositionSpecialCase; ///< true: This item is being used as a ui home position indicator
bool _homePositionValid; ///< true: Home psition should be displayed bool _homePositionValid; ///< true: Home psition should be displayed

Write Preview
Loading…
Cancel
Save