Skip to content

Commit

Permalink
Display Rangefinder Distance
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndKe @patrickelectric
AndKe authored and patrickelectric committed Feb 2, 2023
1 parent 206518a commit 3f9b60f
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 0 deletions.
15 changes: 15 additions & 0 deletions src/Vehicle/Vehicle.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
5 changes: 5 additions & 0 deletions src/Vehicle/Vehicle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -1336,6 +1339,7 @@ private slots:
Fact _altitudeTuningFact;
Fact _altitudeTuningSetpointFact;
Fact _xTrackErrorFact;
Fact _rangeFinderDistFact;
Fact _flightDistanceFact;
Fact _flightTimeFact;
Fact _distanceToHomeFact;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 3f9b60f

Please sign in to comment.