From e5b5b95cb78111f5eb79221df2fe4c33edf009dc Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Tue, 16 Jul 2019 16:43:54 +1200 Subject: [PATCH] Compass - Add indicator of next WP Heading --- qgcimages.qrc | 1 + src/FlightMap/Images/compassDottedLine.svg | 50 ++++++++++++++++++++++ src/FlightMap/Widgets/QGCCompassWidget.qml | 23 +++++++++- src/Vehicle/Vehicle.cc | 24 +++++++++++ src/Vehicle/Vehicle.h | 6 +++ src/Vehicle/VehicleFact.json | 7 +++ 6 files changed, 109 insertions(+), 2 deletions(-) create mode 100644 src/FlightMap/Images/compassDottedLine.svg diff --git a/qgcimages.qrc b/qgcimages.qrc index e47fcc1e1..fcd413a5a 100644 --- a/qgcimages.qrc +++ b/qgcimages.qrc @@ -69,6 +69,7 @@ src/MissionManager/CogWheel.svg src/FlightMap/Images/compassInstrumentArrow.svg src/FlightMap/Images/compassInstrumentDial.svg + src/FlightMap/Images/compassDottedLine.svg src/FlightMap/Images/cOGPointer.svg src/ui/toolbar/Images/Connect.svg src/FlightMap/Images/crossHair.svg diff --git a/src/FlightMap/Images/compassDottedLine.svg b/src/FlightMap/Images/compassDottedLine.svg new file mode 100644 index 000000000..58df75b13 --- /dev/null +++ b/src/FlightMap/Images/compassDottedLine.svg @@ -0,0 +1,50 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/src/FlightMap/Widgets/QGCCompassWidget.qml b/src/FlightMap/Widgets/QGCCompassWidget.qml index d66bf3f08..3a9629c2c 100644 --- a/src/FlightMap/Widgets/QGCCompassWidget.qml +++ b/src/FlightMap/Widgets/QGCCompassWidget.qml @@ -36,7 +36,8 @@ Item { property int _fontSize: ScreenTools.defaultFontPointSize * _sizeRatio property real _heading: vehicle ? vehicle.heading.rawValue : 0 property real _headingToHome: vehicle ? vehicle.headingToHome.rawValue : 0 - property real _groundSpeed: vehicle ? vehicle.groundSpeed.rawValue : 0 + property real _groundSpeed: vehicle ? vehicle.groundSpeed.rawValue : 0 + property real _headingToNextWP: vehicle ? vehicle.headingToNextWP.rawValue : 0 property real _courseOverGround:activeVehicle ? activeVehicle.gps.courseOverGround.rawValue : 0 function isCOGAngleOK(){ @@ -52,12 +53,15 @@ Item { return _showHomeHeadingCompass && !isNaN(_headingToHome) } + function isHeadingToNextWPOK(){ + return !isNaN(_headingToNextWP) + } + readonly property bool _showHomeHeadingCompass: QGroundControl.settingsManager.flyViewSettings.showHomeHeadingCompass.value readonly property bool _showCOGAngleCompass: QGroundControl.settingsManager.flyViewSettings.showCOGAngleCompass.value QGCPalette { id: qgcPal; colorGroupEnabled: enabled } - Rectangle { id: borderRect anchors.fill: parent @@ -72,6 +76,21 @@ Item { anchors.fill: parent visible: false + Image { + id: nextWPPointer + source: isHeadingToNextWPOK() ? "/qmlimages/compassDottedLine.svg":"" + mipmap: true + fillMode: Image.PreserveAspectFit + anchors.fill: parent + sourceSize.height: parent.height + + transform: Rotation { + origin.x: cOGPointer.width / 2 + origin.y: cOGPointer.height / 2 + angle: _headingToNextWP - _heading + } + } + Image { id: cOGPointer source: isCOGAngleOK() ? "/qmlimages/cOGPointer.svg" : "" diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index eaa65792e..9fceffd04 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -71,6 +71,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; +const char* Vehicle::_headingToNextWPFactName = "headingToNextWP"; const char* Vehicle::_headingToHomeFactName = "headingToHome"; const char* Vehicle::_distanceToGCSFactName = "distanceToGCS"; const char* Vehicle::_hobbsFactName = "hobbs"; @@ -202,6 +203,7 @@ Vehicle::Vehicle(LinkInterface* link, , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) + , _headingToNextWPFact (0, _headingToNextWPFactName, FactMetaData::valueTypeDouble) , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) @@ -404,6 +406,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) + , _headingToNextWPFact (0, _headingToNextWPFactName, FactMetaData::valueTypeDouble) , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) @@ -438,6 +441,7 @@ void Vehicle::_commonInit(void) connect(this, &Vehicle::homePositionChanged, this, &Vehicle::_updateDistanceHeadingToHome); connect(this, &Vehicle::hobbsMeterChanged, this, &Vehicle::_updateHobbsMeter); + connect(_toolbox->qgcPositionManager(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateDistanceToGCS); _missionManager = new MissionManager(this); @@ -447,6 +451,7 @@ void Vehicle::_commonInit(void) connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearTrajectoryPoints); connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearCameraTriggerPoints); connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearTrajectoryPoints); + connect(_missionManager, &MissionManager::currentIndexChanged, this, &Vehicle::_updateHeadingToNextWP); _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); @@ -3790,6 +3795,25 @@ void Vehicle::_updateDistanceHeadingToHome(void) } } +void Vehicle::_updateHeadingToNextWP(void) +{ + const int _currentIndex = _missionManager->currentIndex(); + MissionItem _currentItem; + QList llist = _missionManager->missionItems(); + + if(llist.size()>_currentIndex && _currentIndex!=-1 + && llist[_currentIndex]->coordinate().longitude()!=0.0 + && coordinate().distanceTo(llist[_currentIndex]->coordinate())>5.0 ){ + + _headingToNextWPFact.setRawValue(coordinate().azimuthTo(llist[_currentIndex]->coordinate())); + } + else{ + _headingToNextWPFact.setRawValue(qQNaN()); + } + + qDebug() << " Vehicle updateHeadingToNextWP "<< _currentIndex; +} + void Vehicle::_updateDistanceToGCS(void) { QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition(); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 231e82f4a..1696c24aa 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -669,6 +669,7 @@ public: Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) + Q_PROPERTY(Fact* headingToNextWP READ headingToNextWP CONSTANT) Q_PROPERTY(Fact* headingToHome READ headingToHome CONSTANT) Q_PROPERTY(Fact* distanceToGCS READ distanceToGCS CONSTANT) Q_PROPERTY(Fact* hobbs READ hobbs CONSTANT) @@ -971,6 +972,7 @@ public: Fact* altitudeAMSL (void) { return &_altitudeAMSLFact; } Fact* flightDistance (void) { return &_flightDistanceFact; } Fact* distanceToHome (void) { return &_distanceToHomeFact; } + Fact* headingToNextWP (void) { return &_headingToNextWPFact; } Fact* headingToHome (void) { return &_headingToHomeFact; } Fact* distanceToGCS (void) { return &_distanceToGCSFact; } Fact* hobbs (void) { return &_hobbsFact; } @@ -1243,6 +1245,7 @@ private slots: void _clearTrajectoryPoints(void); void _clearCameraTriggerPoints(void); void _updateDistanceHeadingToHome(void); + void _updateHeadingToNextWP(void); void _updateDistanceToGCS(void); void _updateHobbsMeter(void); void _vehicleParamLoaded(bool ready); @@ -1535,7 +1538,9 @@ private: Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; + Fact _headingToNextWPFact; Fact _headingToHomeFact; + Fact _headingToNextWP; Fact _distanceToGCSFact; Fact _hobbsFact; Fact _throttlePctFact; @@ -1565,6 +1570,7 @@ private: static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName; + static const char* _headingToNextWPFactName; static const char* _headingToHomeFactName; static const char* _distanceToGCSFactName; static const char* _hobbsFactName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index ce0b5d391..1d18d1794 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -104,6 +104,13 @@ "decimalPlaces": 1, "units": "m" }, +{ + "name": "headingToNextWP", + "shortDescription": "Heading to Next Waypoint", + "type": "double", + "decimalPlaces": 0, + "units": "deg" +}, { "name": "flightTime", "shortDescription": "Flight Time", -- 2.22.0