diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 570e3ab6935..d5378461a90 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -84,6 +84,7 @@ const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_climbRateFactName = "climbRate"; const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative"; const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; +const char* Vehicle::_altitudeAboveTerrFactName = "altitudeAboveTerr"; const char* Vehicle::_altitudeTuningFactName = "altitudeTuning"; const char* Vehicle::_altitudeTuningSetpointFactName = "altitudeTuningSetpoint"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; @@ -146,6 +147,7 @@ Vehicle::Vehicle(LinkInterface* link, , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) + , _altitudeAboveTerrFact (0, _altitudeAboveTerrFactName, FactMetaData::valueTypeDouble) , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) , _xTrackErrorFact (0, _xTrackErrorFactName, FactMetaData::valueTypeDouble) @@ -256,6 +258,9 @@ Vehicle::Vehicle(LinkInterface* link, // Start csv logger connect(&_csvLogTimer, &QTimer::timeout, this, &Vehicle::_writeCsvLine); _csvLogTimer.start(1000); + + // Start timer to limit altitude above terrain queries + _altitudeAboveTerrQueryTimer.restart(); } // Disconnected Vehicle for offline editing @@ -292,6 +297,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) + , _altitudeAboveTerrFact (0, _altitudeAboveTerrFactName, FactMetaData::valueTypeDouble) , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) , _xTrackErrorFact (0, _xTrackErrorFactName, FactMetaData::valueTypeDouble) @@ -357,6 +363,9 @@ void Vehicle::_commonInit() connect(this, &Vehicle::coordinateChanged, this, &Vehicle::_updateDistanceToGCS); connect(this, &Vehicle::homePositionChanged, this, &Vehicle::_updateDistanceHeadingToHome); connect(this, &Vehicle::hobbsMeterChanged, this, &Vehicle::_updateHobbsMeter); + connect(this, &Vehicle::coordinateChanged, this, &Vehicle::_updateAltAboveTerrain); + // Initialize alt above terrain to Nan so frontend can display it correctly in case the terrain query had no response + _altitudeAboveTerrFact.setRawValue(qQNaN()); connect(_toolbox->qgcPositionManager(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateDistanceToGCS); connect(_toolbox->qgcPositionManager(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateHomepoint); @@ -424,6 +433,7 @@ void Vehicle::_commonInit() _addFact(&_climbRateFact, _climbRateFactName); _addFact(&_altitudeRelativeFact, _altitudeRelativeFactName); _addFact(&_altitudeAMSLFact, _altitudeAMSLFactName); + _addFact(&_altitudeAboveTerrFact, _altitudeAboveTerrFactName); _addFact(&_altitudeTuningFact, _altitudeTuningFactName); _addFact(&_altitudeTuningSetpointFact, _altitudeTuningSetpointFactName); _addFact(&_xTrackErrorFact, _xTrackErrorFactName); @@ -4187,6 +4197,63 @@ void Vehicle::_doSetHomeTerrainReceived(bool success, QList heights) _doSetHomeCoordinate = QGeoCoordinate(); // So isValid() will no longer return true, for extra safety } +void Vehicle::_updateAltAboveTerrain() +{ + // We won't do another query if the previous query was done closer than 2 meters from current position + // or if altitude change has been less than 0.5 meters since then. + const qreal minimumDistanceTraveled = 2; + const float minimumAltitudeChanged = 0.5f; + + // This is not super elegant but it works to limit the amount of queries we do. It seems more than 500ms is not possible to get + // serviced on time. It is not a big deal if it is not serviced on time as terrain queries can manage that just fine, but QGC would + // use resources to service those queries, and it is pointless, so this is a quick workaround to not waste that little computing time + int altitudeAboveTerrQueryMinInterval = 500; + if (_altitudeAboveTerrQueryTimer.elapsed() < altitudeAboveTerrQueryMinInterval) { + // qCDebug(VehicleLog) << "_updateAltAboveTerrain: minimum 500ms interval between queries not reached, returning"; + return; + } + // Sanity check, although it is very unlikely that vehicle coordinate is not valid + if (_coordinate.isValid()) { + // Check for minimum distance and altitude traveled before doing query, to not do a lot of queries of the same data + if (_altitudeAboveTerrLastCoord.isValid() && !qIsNaN(_altitudeAboveTerrLastRelAlt)) { + if (_altitudeAboveTerrLastCoord.distanceTo(_coordinate) < minimumDistanceTraveled && fabs(_altitudeRelativeFact.rawValue().toFloat() - _altitudeAboveTerrLastRelAlt) < minimumAltitudeChanged ) { + return; + } + } + _altitudeAboveTerrLastCoord = _coordinate; + _altitudeAboveTerrLastRelAlt = _altitudeRelativeFact.rawValue().toFloat(); + + // If for some reason we already did a query and it hasn't arrived yet, disconnect signals and unset current query. TerrainQuery system will + // automatically delete that forgotten query. + if (_altitudeAboveTerrTerrainAtCoordinateQuery) { + // qCDebug(VehicleLog) << "_updateAltAboveTerrain: cleaning previous query, no longer needed"; + disconnect(_altitudeAboveTerrTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_altitudeAboveTerrainReceived); + _altitudeAboveTerrTerrainAtCoordinateQuery = nullptr; + } + // Now setup and trigger the new terrain query + _altitudeAboveTerrTerrainAtCoordinateQuery = new TerrainAtCoordinateQuery(true /* autoDelet */); + connect(_altitudeAboveTerrTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_altitudeAboveTerrainReceived); + QList rgCoord; + rgCoord.append(_coordinate); + _altitudeAboveTerrTerrainAtCoordinateQuery->requestData(rgCoord); + _altitudeAboveTerrQueryTimer.restart(); + } +} + +void Vehicle::_altitudeAboveTerrainReceived(bool success, QList heights) +{ + if (!success) { + qCDebug(VehicleLog) << "_altitudeAboveTerrainReceived: terrain data not available for vehicle coordinate"; + } else { + // Query was succesful, save the data. + double terrainAltitude = heights[0]; + double altitudeAboveTerrain = altitudeAMSL()->rawValue().toDouble() - terrainAltitude; + _altitudeAboveTerrFact.setRawValue(altitudeAboveTerrain); + } + // Clean up + _altitudeAboveTerrTerrainAtCoordinateQuery = nullptr; +} + void Vehicle::gimbalControlValue(double pitch, double yaw) { if (apmFirmware()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4c35c30e7fe..4b53ee2467c 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -293,6 +293,7 @@ class Vehicle : public FactGroup Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT) Q_PROPERTY(Fact* altitudeRelative READ altitudeRelative CONSTANT) Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT) + Q_PROPERTY(Fact* altitudeAboveTerr READ altitudeAboveTerr CONSTANT) Q_PROPERTY(Fact* altitudeTuning READ altitudeTuning CONSTANT) Q_PROPERTY(Fact* altitudeTuningSetpoint READ altitudeTuningSetpoint CONSTANT) Q_PROPERTY(Fact* xTrackError READ xTrackError CONSTANT) @@ -684,6 +685,7 @@ class Vehicle : public FactGroup Fact* climbRate () { return &_climbRateFact; } Fact* altitudeRelative () { return &_altitudeRelativeFact; } Fact* altitudeAMSL () { return &_altitudeAMSLFact; } + Fact* altitudeAboveTerr () { return &_altitudeAboveTerrFact; } Fact* altitudeTuning () { return &_altitudeTuningFact; } Fact* altitudeTuningSetpoint () { return &_altitudeTuningSetpointFact; } Fact* xTrackError () { return &_xTrackErrorFact; } @@ -1051,6 +1053,8 @@ private slots: void _updateFlightTime (); void _gotProgressUpdate (float progressValue); void _doSetHomeTerrainReceived (bool success, QList heights); + void _updateAltAboveTerrain (); + void _altitudeAboveTerrainReceived (bool sucess, QList heights); private: void _loadJoystickSettings (); @@ -1386,6 +1390,7 @@ private slots: Fact _climbRateFact; Fact _altitudeRelativeFact; Fact _altitudeAMSLFact; + Fact _altitudeAboveTerrFact; Fact _altitudeTuningFact; Fact _altitudeTuningSetpointFact; Fact _xTrackErrorFact; @@ -1443,6 +1448,7 @@ private slots: static const char* _climbRateFactName; static const char* _altitudeRelativeFactName; static const char* _altitudeAMSLFactName; + static const char* _altitudeAboveTerrFactName; static const char* _altitudeTuningFactName; static const char* _altitudeTuningSetpointFactName; static const char* _xTrackErrorFactName; @@ -1483,6 +1489,13 @@ private slots: // Terrain query members, used to get terrain altitude for doSetHome() TerrainAtCoordinateQuery* _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; QGeoCoordinate _doSetHomeCoordinate; + + // Terrain query members, used to get altitude above terrain Fact + QElapsedTimer _altitudeAboveTerrQueryTimer; + TerrainAtCoordinateQuery* _altitudeAboveTerrTerrainAtCoordinateQuery = nullptr; + // We use this to limit above terrain altitude queries based on distance and altitude change + QGeoCoordinate _altitudeAboveTerrLastCoord; + float _altitudeAboveTerrLastRelAlt = qQNaN(); }; Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t) diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index 3e7be998cee..5336b00c5a4 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -80,6 +80,13 @@ "decimalPlaces": 1, "units": "m" }, +{ + "name": "altitudeAboveTerr", + "shortDesc": "Alt (Above Terrain)", + "type": "double", + "decimalPlaces": 1, + "units": "m" +}, { "name": "flightDistance", "shortDesc": "Flight Distance",