From 6f05b03887ea8839b5037d9d264751a3d2cd0739 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 25 Oct 2018 15:04:52 -0700 Subject: [PATCH] Heading to Home on Vehicle FactGroup --- ChangeLog.md | 3 ++- src/Vehicle/Vehicle.cc | 16 +++++++++++++--- src/Vehicle/Vehicle.h | 6 +++++- src/Vehicle/VehicleFact.json | 7 +++++++ 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/ChangeLog.md b/ChangeLog.md index c5d56d6ab..289c5992e 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -8,7 +8,8 @@ Note: This file only contains high level features or important fixes. * Add support for specifying fixed RTK based station location in Settings/General. * Added Airmap integration to QGC * Add ESTIMATOR_STATUS values to new estimatorStatus Vehicle FactGroup. These are now available to display in instrument panel. -* Make Distance to GCS to available for display from instrument panel. +* Make Distance to GCS available for display from instrument panel. +* Make Heading to Home available for display from instrument panel. ## 3.4 diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6ef17f959..da5b9a208 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -70,6 +70,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; +const char* Vehicle::_headingToHomeFactName = "headingToHome"; const char* Vehicle::_distanceToGCSFactName = "distanceToGCS"; const char* Vehicle::_hobbsFactName = "hobbs"; @@ -194,6 +195,7 @@ Vehicle::Vehicle(LinkInterface* link, , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) + , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _gpsFactGroup(this) @@ -388,6 +390,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) + , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _gpsFactGroup(this) @@ -408,9 +411,9 @@ void Vehicle::_commonInit(void) connect(_firmwarePlugin, &FirmwarePlugin::toolbarIndicatorsChanged, this, &Vehicle::toolBarIndicatorsChanged); - connect(this, &Vehicle::coordinateChanged, this, &Vehicle::_updateDistanceToHome); + connect(this, &Vehicle::coordinateChanged, this, &Vehicle::_updateDistanceHeadingToHome); connect(this, &Vehicle::coordinateChanged, this, &Vehicle::_updateDistanceToGCS); - connect(this, &Vehicle::homePositionChanged, this, &Vehicle::_updateDistanceToHome); + connect(this, &Vehicle::homePositionChanged, this, &Vehicle::_updateDistanceHeadingToHome); connect(this, &Vehicle::hobbsMeterChanged, this, &Vehicle::_updateHobbsMeter); connect(_toolbox->qgcPositionManager(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateDistanceToGCS); @@ -460,6 +463,7 @@ void Vehicle::_commonInit(void) _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); _addFact(&_distanceToHomeFact, _distanceToHomeFactName); + _addFact(&_headingToHomeFact, _headingToHomeFactName); _addFact(&_distanceToGCSFact, _distanceToGCSFactName); _hobbsFact.setRawValue(QVariant(QString("0000:00:00"))); @@ -3597,12 +3601,18 @@ void Vehicle::_handleADSBVehicle(const mavlink_message_t& message) } } -void Vehicle::_updateDistanceToHome(void) +void Vehicle::_updateDistanceHeadingToHome(void) { if (coordinate().isValid() && homePosition().isValid()) { _distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition())); + if (_distanceToHomeFact.rawValue().toDouble() > 1.0) { + _headingToHomeFact.setRawValue(coordinate().azimuthTo(homePosition())); + } else { + _headingToHomeFact.setRawValue(qQNaN()); + } } else { _distanceToHomeFact.setRawValue(qQNaN()); + _headingToHomeFact.setRawValue(qQNaN()); } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index bb60a67e0..9a1b5da5e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -656,6 +656,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* headingToHome READ headingToHome CONSTANT) Q_PROPERTY(Fact* distanceToGCS READ distanceToGCS CONSTANT) Q_PROPERTY(Fact* hobbs READ hobbs CONSTANT) @@ -946,6 +947,7 @@ public: Fact* altitudeAMSL (void) { return &_altitudeAMSLFact; } Fact* flightDistance (void) { return &_flightDistanceFact; } Fact* distanceToHome (void) { return &_distanceToHomeFact; } + Fact* headingToHome (void) { return &_headingToHomeFact; } Fact* distanceToGCS (void) { return &_distanceToGCSFact; } Fact* hobbs (void) { return &_hobbsFact; } @@ -1191,7 +1193,7 @@ private slots: void _sendMavCommandAgain(void); void _clearTrajectoryPoints(void); void _clearCameraTriggerPoints(void); - void _updateDistanceToHome(void); + void _updateDistanceHeadingToHome(void); void _updateDistanceToGCS(void); void _updateHobbsMeter(void); void _vehicleParamLoaded(bool ready); @@ -1456,6 +1458,7 @@ private: Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; + Fact _headingToHomeFact; Fact _distanceToGCSFact; Fact _hobbsFact; @@ -1484,6 +1487,7 @@ private: static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName; + 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 520c5048b..7adda5a1b 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -90,6 +90,13 @@ "decimalPlaces": 1, "units": "m" }, +{ + "name": "headingToHome", + "shortDescription": "Heading to Home", + "type": "double", + "decimalPlaces": 0, + "units": "deg" +}, { "name": "distanceToGCS", "shortDescription": "Distance to GCS", -- 2.22.0