From 3d568b342f20a42d02a8edb0d72c9f4c52918bd2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 27 Jan 2021 17:53:42 +0100 Subject: [PATCH] Vehicle: add facts for TECS tuninig (alt+airspeed setpoints) --- src/Vehicle/Vehicle.cc | 34 ++++++++++++++++++++++++++++++++++ src/Vehicle/Vehicle.h | 15 +++++++++++++++ 2 files changed, 49 insertions(+) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index e4c08af..f7bd1d0 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -75,10 +75,13 @@ const char* Vehicle::_rollRateFactName = "rollRate"; const char* Vehicle::_pitchRateFactName = "pitchRate"; const char* Vehicle::_yawRateFactName = "yawRate"; const char* Vehicle::_airSpeedFactName = "airSpeed"; +const char* Vehicle::_airSpeedSetpointFactName = "airSpeedSetpoint"; const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_climbRateFactName = "climbRate"; const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative"; const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; +const char* Vehicle::_altitudeTuningFactName = "altitudeTuning"; +const char* Vehicle::_altitudeTuningSetpointFactName = "altitudeTuningSetpoint"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; @@ -132,9 +135,12 @@ Vehicle::Vehicle(LinkInterface* link, , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) + , _airSpeedSetpointFact (0, _airSpeedSetpointFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) + , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) + , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) @@ -282,9 +288,12 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) + , _airSpeedSetpointFact (0, _airSpeedSetpointFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) + , _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble) + , _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) @@ -391,9 +400,12 @@ void Vehicle::_commonInit() _addFact(&_yawRateFact, _yawRateFactName); _addFact(&_groundSpeedFact, _groundSpeedFactName); _addFact(&_airSpeedFact, _airSpeedFactName); + _addFact(&_airSpeedSetpointFact, _airSpeedSetpointFactName); _addFact(&_climbRateFact, _climbRateFactName); _addFact(&_altitudeRelativeFact, _altitudeRelativeFactName); _addFact(&_altitudeAMSLFact, _altitudeAMSLFactName); + _addFact(&_altitudeTuningFact, _altitudeTuningFactName); + _addFact(&_altitudeTuningSetpointFact, _altitudeTuningSetpointFactName); _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); _addFact(&_distanceToHomeFact, _distanceToHomeFactName); @@ -687,6 +699,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_VFR_HUD: _handleVfrHud(message); break; + case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: + _handleNavControllerOutput(message); + break; case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: _handleCameraImageCaptured(message); break; @@ -934,6 +949,19 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _groundSpeedFact.setRawValue(qIsNaN(vfrHud.groundspeed) ? 0 : vfrHud.groundspeed); _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb); _throttlePctFact.setRawValue(static_cast(vfrHud.throttle)); + if (qIsNaN(_altitudeTuningOffset)) { + _altitudeTuningOffset = vfrHud.alt; + } + _altitudeTuningFact.setRawValue(vfrHud.alt - _altitudeTuningOffset); +} + +void Vehicle::_handleNavControllerOutput(mavlink_message_t& message) +{ + mavlink_nav_controller_output_t navControllerOutput; + mavlink_msg_nav_controller_output_decode(&message, &navControllerOutput); + + _altitudeTuningSetpointFact.setRawValue(_altitudeTuningFact.rawValue().toDouble() - navControllerOutput.alt_error); + _airSpeedSetpointFact.setRawValue(_airSpeedFact.rawValue().toDouble() - navControllerOutput.aspd_error); } // Ignore warnings from mavlink headers for both GCC/Clang and MSVC @@ -3575,6 +3603,12 @@ void Vehicle::setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode) break; case ModeAltitudeAndAirspeed: _mavlinkStreamConfig.setHighRateAltAirspeed(); + // reset the altitude offset to the current value, so the plotted value is around 0 + if (!qIsNaN(_altitudeTuningOffset)) { + _altitudeTuningOffset += _altitudeTuningFact.rawValue().toDouble(); + _altitudeTuningSetpointFact.setRawValue(0.f); + _altitudeTuningFact.setRawValue(0.f); + } break; } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 803bb03..7001cfc 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -272,9 +272,12 @@ public: Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) Q_PROPERTY(Fact* groundSpeed READ groundSpeed CONSTANT) Q_PROPERTY(Fact* airSpeed READ airSpeed CONSTANT) + Q_PROPERTY(Fact* airSpeedSetpoint READ airSpeedSetpoint CONSTANT) Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT) Q_PROPERTY(Fact* altitudeRelative READ altitudeRelative CONSTANT) Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT) + Q_PROPERTY(Fact* altitudeTuning READ altitudeTuning CONSTANT) + Q_PROPERTY(Fact* altitudeTuningSetpoint READ altitudeTuningSetpoint CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) Q_PROPERTY(Fact* missionItemIndex READ missionItemIndex CONSTANT) @@ -605,10 +608,13 @@ public: Fact* pitchRate () { return &_pitchRateFact; } Fact* yawRate () { return &_yawRateFact; } Fact* airSpeed () { return &_airSpeedFact; } + Fact* airSpeedSetpoint () { return &_airSpeedSetpointFact; } Fact* groundSpeed () { return &_groundSpeedFact; } Fact* climbRate () { return &_climbRateFact; } Fact* altitudeRelative () { return &_altitudeRelativeFact; } Fact* altitudeAMSL () { return &_altitudeAMSLFact; } + Fact* altitudeTuning () { return &_altitudeTuningFact; } + Fact* altitudeTuningSetpoint () { return &_altitudeTuningSetpointFact; } Fact* flightDistance () { return &_flightDistanceFact; } Fact* distanceToHome () { return &_distanceToHomeFact; } Fact* missionItemIndex () { return &_missionItemIndexFact; } @@ -932,6 +938,7 @@ private: void _handleGlobalPositionInt (mavlink_message_t& message); void _handleAltitude (mavlink_message_t& message); void _handleVfrHud (mavlink_message_t& message); + void _handleNavControllerOutput (mavlink_message_t& message); void _handleHighLatency (mavlink_message_t& message); void _handleHighLatency2 (mavlink_message_t& message); void _handleAttitudeWorker (double rollRadians, double pitchRadians, double yawRadians); @@ -1199,6 +1206,8 @@ private: QMap _lowestBatteryChargeStateAnnouncedMap; + float _altitudeTuningOffset = qQNaN(); // altitude offset, so the plotted value is around 0 + // FactGroup facts Fact _rollFact; @@ -1209,9 +1218,12 @@ private: Fact _yawRateFact; Fact _groundSpeedFact; Fact _airSpeedFact; + Fact _airSpeedSetpointFact; Fact _climbRateFact; Fact _altitudeRelativeFact; Fact _altitudeAMSLFact; + Fact _altitudeTuningFact; + Fact _altitudeTuningSetpointFact; Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; @@ -1254,9 +1266,12 @@ private: static const char* _yawRateFactName; static const char* _groundSpeedFactName; static const char* _airSpeedFactName; + static const char* _airSpeedSetpointFactName; static const char* _climbRateFactName; static const char* _altitudeRelativeFactName; static const char* _altitudeAMSLFactName; + static const char* _altitudeTuningFactName; + static const char* _altitudeTuningSetpointFactName; static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName;