From 3f9b60f65f99b2d12ae35304a9760296ec4a94d7 Mon Sep 17 00:00:00 2001 From: AndKe Date: Thu, 2 Feb 2023 13:55:40 +0100 Subject: [PATCH] Display Rangefinder Distance --- src/Vehicle/Vehicle.cc | 15 +++++++++++++++ src/Vehicle/Vehicle.h | 5 +++++ 2 files changed, 20 insertions(+) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8ecd7c6f177..6c25a05999b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -80,6 +80,7 @@ const char* Vehicle::_yawRateFactName = "yawRate"; const char* Vehicle::_airSpeedFactName = "airSpeed"; const char* Vehicle::_airSpeedSetpointFactName = "airSpeedSetpoint"; const char* Vehicle::_xTrackErrorFactName = "xTrackError"; +const char* Vehicle::_rangeFinderDistFactName = "rangeFinderDist"; const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_climbRateFactName = "climbRate"; const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative"; @@ -147,6 +148,7 @@ Vehicle::Vehicle(LinkInterface* link, , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) , _xTrackErrorFact (0, _xTrackErrorFactName, FactMetaData::valueTypeDouble) + , _rangeFinderDistFact (0, _rangeFinderDistFactName, FactMetaData::valueTypeFloat) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) @@ -301,6 +303,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) , _xTrackErrorFact (0, _xTrackErrorFactName, FactMetaData::valueTypeDouble) + , _rangeFinderDistFact (0, _rangeFinderDistFactName, FactMetaData::valueTypeFloat) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) @@ -424,6 +427,7 @@ void Vehicle::_commonInit() _addFact(&_altitudeTuningFact, _altitudeTuningFactName); _addFact(&_altitudeTuningSetpointFact, _altitudeTuningSetpointFactName); _addFact(&_xTrackErrorFact, _xTrackErrorFactName); + _addFact(&_rangeFinderDistFact, _rangeFinderDistFactName); _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); _addFact(&_distanceToHomeFact, _distanceToHomeFactName); @@ -719,6 +723,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_VFR_HUD: _handleVfrHud(message); break; + case MAVLINK_MSG_ID_RANGEFINDER: + _handleRangefinder(message); + break; case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: _handleNavControllerOutput(message); break; @@ -1001,6 +1008,14 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _altitudeTuningFact.setRawValue(vfrHud.alt - _altitudeTuningOffset); } +void Vehicle::_handleRangefinder(mavlink_message_t& message) +{ + mavlink_rangefinder_t rangefinder; + mavlink_msg_rangefinder_decode(&message, &rangefinder); + _rangeFinderDistFact.setRawValue(qIsNaN(rangefinder.distance) ? 0 : rangefinder.distance); +} + + void Vehicle::_handleNavControllerOutput(mavlink_message_t& message) { mavlink_nav_controller_output_t navControllerOutput; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index fa8fb6d05f2..126a6f629f0 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -297,6 +297,7 @@ class Vehicle : public FactGroup Q_PROPERTY(Fact* altitudeTuning READ altitudeTuning CONSTANT) Q_PROPERTY(Fact* altitudeTuningSetpoint READ altitudeTuningSetpoint CONSTANT) Q_PROPERTY(Fact* xTrackError READ xTrackError CONSTANT) + Q_PROPERTY(Fact* rangeFinderDist READ rangeFinderDist CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) Q_PROPERTY(Fact* missionItemIndex READ missionItemIndex CONSTANT) @@ -670,6 +671,7 @@ class Vehicle : public FactGroup Fact* altitudeTuning () { return &_altitudeTuningFact; } Fact* altitudeTuningSetpoint () { return &_altitudeTuningSetpointFact; } Fact* xTrackError () { return &_xTrackErrorFact; } + Fact* rangeFinderDist () { return &_rangeFinderDistFact; } Fact* flightDistance () { return &_flightDistanceFact; } Fact* distanceToHome () { return &_distanceToHomeFact; } Fact* missionItemIndex () { return &_missionItemIndexFact; } @@ -1028,6 +1030,7 @@ private slots: void _handleGlobalPositionInt (mavlink_message_t& message); void _handleAltitude (mavlink_message_t& message); void _handleVfrHud (mavlink_message_t& message); + void _handleRangefinder (mavlink_message_t& message); void _handleNavControllerOutput (mavlink_message_t& message); void _handleHighLatency (mavlink_message_t& message); void _handleHighLatency2 (mavlink_message_t& message); @@ -1336,6 +1339,7 @@ private slots: Fact _altitudeTuningFact; Fact _altitudeTuningSetpointFact; Fact _xTrackErrorFact; + Fact _rangeFinderDistFact; Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; @@ -1388,6 +1392,7 @@ private slots: static const char* _altitudeTuningFactName; static const char* _altitudeTuningSetpointFactName; static const char* _xTrackErrorFactName; + static const char* _rangeFinderDistFactName; static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName;