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

Vehicle: add altitude above terrain fact and implementation

QGC4.4
davidsastresas 2 years ago committed by David Sastre
parent
9e652a812b
commit
22b3408f03
3 changed files with 87 additions and 0 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. 67
      src/Vehicle/Vehicle.cc
  2. 13
      src/Vehicle/Vehicle.h
  3. 7
      src/Vehicle/VehicleFact.json

67
src/Vehicle/Vehicle.cc
Unescape View File

@ -84,6 +84,7 @@ const char* Vehicle::_groundSpeedFactName = "groundSpeed"; @@ -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";
@ -148,6 +149,7 @@ Vehicle::Vehicle(LinkInterface* link, @@ -148,6 +149,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)
@ -260,6 +262,9 @@ Vehicle::Vehicle(LinkInterface* link, @@ -260,6 +262,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
@ -296,6 +301,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, @@ -296,6 +301,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)
@ -361,6 +367,9 @@ void Vehicle::_commonInit() @@ -361,6 +367,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);
@ -428,6 +437,7 @@ void Vehicle::_commonInit() @@ -428,6 +437,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);
@ -4193,6 +4203,63 @@ void Vehicle::_doSetHomeTerrainReceived(bool success, QList<double> heights) @@ -4193,6 +4203,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()) {

13
src/Vehicle/Vehicle.h
Unescape View File

@ -295,6 +295,7 @@ public: @@ -295,6 +295,7 @@ public:
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)
@ -688,6 +689,7 @@ public: @@ -688,6 +689,7 @@ public:
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; }
@ -1057,6 +1059,8 @@ private slots: @@ -1057,6 +1059,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 ();
@ -1392,6 +1396,7 @@ private: @@ -1392,6 +1396,7 @@ private:
Fact _climbRateFact;
Fact _altitudeRelativeFact;
Fact _altitudeAMSLFact;
Fact _altitudeAboveTerrFact;
Fact _altitudeTuningFact;
Fact _altitudeTuningSetpointFact;
Fact _xTrackErrorFact;
@ -1451,6 +1456,7 @@ private: @@ -1451,6 +1456,7 @@ private:
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;
@ -1493,6 +1499,13 @@ private: @@ -1493,6 +1499,13 @@ private:
// 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
src/Vehicle/VehicleFact.json
Unescape View File

@ -81,6 +81,13 @@ @@ -81,6 +81,13 @@
"units": "m"
},
{
"name": "altitudeAboveTerr",
"shortDesc": "Alt (Above Terrain)",
"type": "double",
"decimalPlaces": 1,
"units": "m"
},
{
"name": "flightDistance",
"shortDesc": "Flight Distance",
"type": "double",

Write Preview
Loading…
Cancel
Save