From 4289338af55d9fe2afca38e8a71a69879112681d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 17 Aug 2017 19:13:24 -0700 Subject: [PATCH] Add distance to home to Vehicle FactGroup --- src/Vehicle/Vehicle.cc | 16 ++++++++++++++++ src/Vehicle/Vehicle.h | 5 +++++ src/Vehicle/VehicleFact.json | 7 +++++++ 3 files changed, 28 insertions(+) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9fb8626d0..6131dfa9e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -55,6 +55,7 @@ const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative"; const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; const char* Vehicle::_flightTimeFactName = "flightTime"; +const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; const char* Vehicle::_gpsFactGroupName = "gps"; const char* Vehicle::_batteryFactGroupName = "battery"; @@ -160,6 +161,7 @@ Vehicle::Vehicle(LinkInterface* link, , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) + , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) , _gpsFactGroup(this) , _batteryFactGroup(this) , _windFactGroup(this) @@ -325,6 +327,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) + , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) , _gpsFactGroup(this) , _batteryFactGroup(this) , _windFactGroup(this) @@ -338,6 +341,9 @@ void Vehicle::_commonInit(void) { _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType); + connect(this, &Vehicle::coordinateChanged, this, &Vehicle::_updateDistanceToHome); + connect(this, &Vehicle::homePositionChanged, this, &Vehicle::_updateDistanceToHome); + _missionManager = new MissionManager(this); connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError); connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete); @@ -376,6 +382,7 @@ void Vehicle::_commonInit(void) _addFact(&_altitudeAMSLFact, _altitudeAMSLFactName); _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); + _addFact(&_distanceToHomeFact, _distanceToHomeFactName); _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); @@ -2798,6 +2805,15 @@ void Vehicle::_handleADSBVehicle(const mavlink_message_t& message) } } +void Vehicle::_updateDistanceToHome(void) +{ + if (coordinate().isValid() && homePosition().isValid()) { + _distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition())); + } else { + _distanceToHomeFact.setRawValue(qQNaN()); + } +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 81a2d8d9a..96c60f1ed 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -348,6 +348,7 @@ public: Q_PROPERTY(Fact* altitudeRelative READ altitudeRelative CONSTANT) Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) + Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) Q_PROPERTY(FactGroup* battery READ batteryFactGroup CONSTANT) @@ -613,6 +614,7 @@ public: Fact* altitudeRelative (void) { return &_altitudeRelativeFact; } Fact* altitudeAMSL (void) { return &_altitudeAMSLFact; } Fact* flightDistance (void) { return &_flightDistanceFact; } + Fact* distanceToHome (void) { return &_distanceToHomeFact; } FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } @@ -825,6 +827,7 @@ private slots: void _activeJoystickChanged(void); void _clearTrajectoryPoints(void); void _clearCameraTriggerPoints(void); + void _updateDistanceToHome(void); private: bool _containsLink(LinkInterface* link); @@ -1042,6 +1045,7 @@ private: Fact _altitudeAMSLFact; Fact _flightDistanceFact; Fact _flightTimeFact; + Fact _distanceToHomeFact; VehicleGPSFactGroup _gpsFactGroup; VehicleBatteryFactGroup _batteryFactGroup; @@ -1059,6 +1063,7 @@ private: static const char* _altitudeAMSLFactName; static const char* _flightDistanceFactName; static const char* _flightTimeFactName; + static const char* _distanceToHomeFactName; static const char* _gpsFactGroupName; static const char* _batteryFactGroupName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index 400fec14a..f50f0d860 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -62,6 +62,13 @@ "decimalPlaces": 1, "units": "m" }, +{ + "name": "distanceToHome", + "shortDescription": "Distance to Home", + "type": "double", + "decimalPlaces": 1, + "units": "m" +}, { "name": "flightTime", "shortDescription": "Flight Time", -- 2.22.0