Skip to content

Commit

Permalink
Vehicle: add altitude above terrain fact and implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@Davidsastresas
Davidsastresas committed Nov 20, 2023
1 parent b7489f3 commit aaddeb6
Show file tree
Hide file tree
Showing 3 changed files with 87 additions and 0 deletions.
67 changes: 67 additions & 0 deletions src/Vehicle/Vehicle.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -4187,6 +4197,63 @@ void Vehicle::_doSetHomeTerrainReceived(bool success, QList<double> 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<QGeoCoordinate> rgCoord;
rgCoord.append(_coordinate);
_altitudeAboveTerrTerrainAtCoordinateQuery->requestData(rgCoord);
_altitudeAboveTerrQueryTimer.restart();
}
}

void Vehicle::_altitudeAboveTerrainReceived(bool success, QList<double> 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()) {
Expand Down
13 changes: 13 additions & 0 deletions src/Vehicle/Vehicle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -1051,6 +1053,8 @@ private slots:
void _updateFlightTime ();
void _gotProgressUpdate (float progressValue);
void _doSetHomeTerrainReceived (bool success, QList<double> heights);
void _updateAltAboveTerrain ();
void _altitudeAboveTerrainReceived (bool sucess, QList<double> heights);

private:
void _loadJoystickSettings ();
Expand Down Expand Up @@ -1386,6 +1390,7 @@ private slots:
Fact _climbRateFact;
Fact _altitudeRelativeFact;
Fact _altitudeAMSLFact;
Fact _altitudeAboveTerrFact;
Fact _altitudeTuningFact;
Fact _altitudeTuningSetpointFact;
Fact _xTrackErrorFact;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
7 changes: 7 additions & 0 deletions src/Vehicle/VehicleFact.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down

0 comments on commit aaddeb6

Please sign in to comment.