From 5e55b7f365d5d690163ca36dbb012d590d9e19ba Mon Sep 17 00:00:00 2001 From: AndKe Date: Wed, 9 Jun 2021 18:20:35 +0200 Subject: [PATCH] Added xTrackError fact --- src/Vehicle/Vehicle.cc | 5 +++++ src/Vehicle/Vehicle.h | 4 ++++ 2 files changed, 9 insertions(+) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index be9794f..07989ee 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -77,6 +77,7 @@ const char* Vehicle::_pitchRateFactName = "pitchRate"; const char* Vehicle::_yawRateFactName = "yawRate"; const char* Vehicle::_airSpeedFactName = "airSpeed"; const char* Vehicle::_airSpeedSetpointFactName = "airSpeedSetpoint"; +const char* Vehicle::_xTrackErrorFactName = "xTrackError"; const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_climbRateFactName = "climbRate"; const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative"; @@ -142,6 +143,7 @@ Vehicle::Vehicle(LinkInterface* link, , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) + , _xTrackErrorFact (0, _xTrackErrorFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) @@ -294,6 +296,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) + , _xTrackErrorFact (0, _xTrackErrorFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) @@ -413,6 +416,7 @@ void Vehicle::_commonInit() _addFact(&_altitudeAMSLFact, _altitudeAMSLFactName); _addFact(&_altitudeTuningFact, _altitudeTuningFactName); _addFact(&_altitudeTuningSetpointFact, _altitudeTuningSetpointFactName); + _addFact(&_xTrackErrorFact, _xTrackErrorFactName); _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); _addFact(&_distanceToHomeFact, _distanceToHomeFactName); @@ -981,6 +985,7 @@ void Vehicle::_handleNavControllerOutput(mavlink_message_t& message) mavlink_msg_nav_controller_output_decode(&message, &navControllerOutput); _altitudeTuningSetpointFact.setRawValue(_altitudeTuningFact.rawValue().toDouble() - navControllerOutput.alt_error); + _xTrackErrorFact.setRawValue(_altitudeTuningFact.rawValue().toDouble() - navControllerOutput.xtrack_error); _airSpeedSetpointFact.setRawValue(_airSpeedFact.rawValue().toDouble() - navControllerOutput.aspd_error); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index bd98054..b9d437e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -289,6 +289,7 @@ public: Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT) Q_PROPERTY(Fact* altitudeTuning READ altitudeTuning CONSTANT) Q_PROPERTY(Fact* altitudeTuningSetpoint READ altitudeTuningSetpoint CONSTANT) + Q_PROPERTY(Fact* xTrackError READ xTrackError CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) Q_PROPERTY(Fact* missionItemIndex READ missionItemIndex CONSTANT) @@ -627,6 +628,7 @@ public: Fact* altitudeAMSL () { return &_altitudeAMSLFact; } Fact* altitudeTuning () { return &_altitudeTuningFact; } Fact* altitudeTuningSetpoint () { return &_altitudeTuningSetpointFact; } + Fact* xTrackError () { return &_xTrackErrorFact; } Fact* flightDistance () { return &_flightDistanceFact; } Fact* distanceToHome () { return &_distanceToHomeFact; } Fact* missionItemIndex () { return &_missionItemIndexFact; } @@ -1251,6 +1253,7 @@ private: Fact _altitudeAMSLFact; Fact _altitudeTuningFact; Fact _altitudeTuningSetpointFact; + Fact _xTrackErrorFact; Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; @@ -1300,6 +1303,7 @@ private: static const char* _altitudeAMSLFactName; static const char* _altitudeTuningFactName; static const char* _altitudeTuningSetpointFactName; + static const char* _xTrackErrorFactName; static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName;