From 3f32dd414d2c41d30537e1ca30f23c26edcb1564 Mon Sep 17 00:00:00 2001 From: Philipp Borgers Date: Wed, 24 May 2023 11:14:47 +0200 Subject: [PATCH] Add Vehicle fact timeToHome TimeToHome fact calculates the time in seconds till the UAV reaches the home position. We take the distance to home and divide it by the current ground speed to get the seconds till we arrive at the home point. I did not add any fancy logic to find out if we actually fly towards the home point or are in RTL so the value shows increasing seconds even if we fly away from the home point --- src/Vehicle/Vehicle.cc | 6 ++++++ src/Vehicle/Vehicle.h | 4 ++++ src/Vehicle/VehicleFact.json | 7 +++++++ 3 files changed, 17 insertions(+) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2faeab4..de37158 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -91,6 +91,7 @@ const char* Vehicle::_altitudeTuningSetpointFactName = "altitudeTuningSetpoint"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; +const char* Vehicle::_timeToHomeFactName = "timeToHome"; const char* Vehicle::_missionItemIndexFactName = "missionItemIndex"; const char* Vehicle::_headingToNextWPFactName = "headingToNextWP"; const char* Vehicle::_headingToHomeFactName = "headingToHome"; @@ -154,6 +155,7 @@ Vehicle::Vehicle(LinkInterface* link, , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) + , _timeToHomeFact (0, _timeToHomeFactName, FactMetaData::valueTypeDouble) , _missionItemIndexFact (0, _missionItemIndexFactName, FactMetaData::valueTypeUint16) , _headingToNextWPFact (0, _headingToNextWPFactName, FactMetaData::valueTypeDouble) , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) @@ -439,6 +441,7 @@ void Vehicle::_commonInit() _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); _addFact(&_distanceToHomeFact, _distanceToHomeFactName); + _addFact(&_timeToHomeFact, _timeToHomeFactName); _addFact(&_missionItemIndexFact, _missionItemIndexFactName); _addFact(&_headingToNextWPFact, _headingToNextWPFactName); _addFact(&_headingToHomeFact, _headingToHomeFactName); @@ -1023,6 +1026,9 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _altitudeTuningOffset = vfrHud.alt; } _altitudeTuningFact.setRawValue(vfrHud.alt - _altitudeTuningOffset); + if (!qIsNaN(vfrHud.groundspeed) && !qIsNaN(_distanceToHomeFact.cookedValue().toDouble())) { + _timeToHomeFact.setRawValue(_distanceToHomeFact.cookedValue().toDouble() / vfrHud.groundspeed); + } } void Vehicle::_handleNavControllerOutput(mavlink_message_t& message) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 231e300..05c11fd 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -302,6 +302,7 @@ public: Q_PROPERTY(Fact* rangeFinderDist READ rangeFinderDist CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) + Q_PROPERTY(Fact* timeToHome READ timeToHome CONSTANT) Q_PROPERTY(Fact* missionItemIndex READ missionItemIndex CONSTANT) Q_PROPERTY(Fact* headingToNextWP READ headingToNextWP CONSTANT) Q_PROPERTY(Fact* headingToHome READ headingToHome CONSTANT) @@ -679,6 +680,7 @@ public: Fact* rangeFinderDist () { return &_rangeFinderDistFact; } Fact* flightDistance () { return &_flightDistanceFact; } Fact* distanceToHome () { return &_distanceToHomeFact; } + Fact* timeToHome () { return &_timeToHomeFact; } Fact* missionItemIndex () { return &_missionItemIndexFact; } Fact* headingToNextWP () { return &_headingToNextWPFact; } Fact* headingToHome () { return &_headingToHomeFact; } @@ -1358,6 +1360,7 @@ private: Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; + Fact _timeToHomeFact; Fact _missionItemIndexFact; Fact _headingToNextWPFact; Fact _headingToHomeFact; @@ -1413,6 +1416,7 @@ private: static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName; + static const char* _timeToHomeFactName; static const char* _missionItemIndexFactName; static const char* _headingToNextWPFactName; static const char* _headingToHomeFactName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index e194c82..3e7be99 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -95,6 +95,13 @@ "units": "m" }, { + "name": "timeToHome", + "shortDesc": "Time to Home", + "type": "double", + "decimalPlaces": 1, + "units": "s" +}, +{ "name": "headingToHome", "shortDesc": "Heading to Home", "type": "double",